diff --git a/Description.pdf b/Description.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..c94d222095610b0947b32ef7e46102710878f6cd
Binary files /dev/null and b/Description.pdf differ
diff --git a/MBotBoardBalancer-1.0-SNAPSHOT.jar b/MBotBoardBalancer-1.0-SNAPSHOT.jar
new file mode 100644
index 0000000000000000000000000000000000000000..852e004a556788e31a1a6183f230cff2e134ee8e
Binary files /dev/null and b/MBotBoardBalancer-1.0-SNAPSHOT.jar differ
diff --git a/MbotFuzzyBalance_Tile.png b/MbotFuzzyBalance_Tile.png
new file mode 100644
index 0000000000000000000000000000000000000000..fa5e7e8d4a33a6cdba869bc6f86d893c2df840a0
Binary files /dev/null and b/MbotFuzzyBalance_Tile.png differ
diff --git a/MbotFuzzyBalancer_1.png b/MbotFuzzyBalancer_1.png
new file mode 100644
index 0000000000000000000000000000000000000000..84569d513496b1e7bbd02219314484ae121e9fef
Binary files /dev/null and b/MbotFuzzyBalancer_1.png differ
diff --git a/MbotFuzzyLogicBalancing.MP4 b/MbotFuzzyLogicBalancing.MP4
new file mode 100644
index 0000000000000000000000000000000000000000..524b3fce1e6eaadc8df7d29b03ef2aaef104af6b
Binary files /dev/null and b/MbotFuzzyLogicBalancing.MP4 differ
diff --git a/README.txt b/README.txt
new file mode 100644
index 0000000000000000000000000000000000000000..488a865328443f664faa3835f62049f8c0c47537
--- /dev/null
+++ b/README.txt
@@ -0,0 +1,16 @@
+FOLDER CONTAINS:
+
+- Source Code with the pom.xml for Maven
+
+Note: The fuzzy logic library must be manually added to the Maven
+repository before building the project via Maven. See the library's folder.
+
+- Fuzzy Logic .fcl file for balancing the car
+
+- Client .jar file to control the Mbot car
+
+HOW TO USE THE CLIENT:
+
+- Run the .jar
+- Provide the full path to the .fcl , i.e."C:Path\To\The\myFuzzySets.fcl"
+- Provide the bluetooth port for the communication, i.e "COM3"
\ No newline at end of file
diff --git a/balance.fcl b/balance.fcl
new file mode 100644
index 0000000000000000000000000000000000000000..e9025989b43194d5aa616ff57d4b0136eaa873e4
--- /dev/null
+++ b/balance.fcl
@@ -0,0 +1,42 @@
+FUNCTION_BLOCK balance // Block definition
+
+VAR_INPUT // Define input variables
+ angle : REAL;
+END_VAR
+
+VAR_OUTPUT // Define output variable
+ direction : REAL;
+END_VAR
+
+FUZZIFY angle // Fuzzify input variable
+ TERM rightStrong := (-50,1) (-5, 1) (-4, 0);
+ TERM rightLight := (-5,0) (-3, 1) (-2, 0);
+ TERM balanced := (-2, 0) (0, 1) (2, 0);
+ TERM leftLight := (2,0) (3, 1) (5, 0);
+ TERM leftStrong := (4, 0) (5, 1) (50, 1);
+END_FUZZIFY
+
+DEFUZZIFY direction // Defzzzify output variable
+ TERM driveBackFast := (-110, 1)(-70, 1) (-60, 0);
+ TERM driveBackSlow := (-60, 0)(-50, 1) (-40, 1) (-30, 0);
+ TERM standStill := (-30, 0)(0, 1) (30, 0);
+ TERM driveForwardSlow := (30, 0)(40, 1) (50, 1) (60, 0);
+ TERM driveForwardFast := (60, 0)(70, 1) (110, 1);
+ METHOD : COG; // Use 'Center Of Gravity' defuzzification method
+ DEFAULT := 0; // Default value is 0 (if no rule activates defuzzifier)
+END_DEFUZZIFY
+
+RULEBLOCK No1
+ AND : MIN; // Use 'min' for 'and' (also implicit use 'max' for 'or' to fulfill DeMorgan's Law)
+ ACT : MIN; // Use 'min' activation method
+ ACCU : MAX; // Use 'max' accumulation method
+
+ RULE 1 : IF angle IS leftStrong THEN direction IS driveForwardFast;
+ RULE 2 : IF angle IS leftLight THEN direction IS driveForwardSlow;
+ RULE 3 : IF angle IS balanced THEN direction IS standStill;
+ RULE 4 : IF angle IS rightLight THEN direction IS driveBackSlow;
+ RULE 5 : IF angle IS rightStrong THEN direction IS driveBackFast;
+
+END_RULEBLOCK
+
+END_FUNCTION_BLOCK
diff --git a/jFuzzyLogicLibrary/jFuzzyLogic-1.0.jar b/jFuzzyLogicLibrary/jFuzzyLogic-1.0.jar
new file mode 100644
index 0000000000000000000000000000000000000000..14c404d4c9f973f4b3c55fa1f5168ec5f3c7a228
Binary files /dev/null and b/jFuzzyLogicLibrary/jFuzzyLogic-1.0.jar differ
diff --git a/jFuzzyLogicLibrary/jFuzzyLogicInstallGuide.txt b/jFuzzyLogicLibrary/jFuzzyLogicInstallGuide.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3ce83bece086a046cc088e0c303b65d10f64c3e1
--- /dev/null
+++ b/jFuzzyLogicLibrary/jFuzzyLogicInstallGuide.txt
@@ -0,0 +1,21 @@
+Since the libary doesn't exist in the public maven repository, it has to be manually added to the project.
+
+- Run the line below to save the jar in the local repo.
+- Note that I have given it a version number, since the library's authors don't provide anything like it.
+
+mvn install:install-file -Dfile=jFuzzyLogic-1.0.jar -DgroupId=net.sourceforge -DartifactId=jFuzzyLogic -Dversion=1.0 -Dpackaging=jar
+
+And then I can load it in the pom with :
+
+<dependencies>
+ <dependency>
+ <groupId>net.sourceforge</groupId>
+ <artifactId>jFuzzyLogic</artifactId>
+ <version>1.0</version>
+ </dependency>
+</dependencies>
+
+--------------------------------------------------------------------------------
+
+Note: I used the following guide:
+https://softwarecave.org/2014/06/14/adding-external-jars-into-maven-project/.
diff --git a/logging.properties b/logging.properties
new file mode 100644
index 0000000000000000000000000000000000000000..c009e977afac214293c2bc3373c9612e9998d22e
--- /dev/null
+++ b/logging.properties
@@ -0,0 +1,6 @@
+handlers = java.util.logging.ConsoleHandler
+.level= INFO
+java.util.logging.ConsoleHandler.level = INFO
+java.util.logging.ConsoleHandler.formatter = java.util.logging.SimpleFormatter
+# Pattern works since Java 7
+java.util.logging.SimpleFormatter.format = [%1$tc] %4$s: %2$s - %5$s %6$s%n
\ No newline at end of file
diff --git a/pom.xml b/pom.xml
new file mode 100644
index 0000000000000000000000000000000000000000..2cc22464ccf182fedbb87721f71f5df2792c49cf
--- /dev/null
+++ b/pom.xml
@@ -0,0 +1,104 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project xmlns="http://maven.apache.org/POM/4.0.0"
+ xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+ xsi:schemaLocation="http://maven.apache.org/POM/4.0.0 http://maven.apache.org/xsd/maven-4.0.0.xsd">
+ <modelVersion>4.0.0</modelVersion>
+
+ <groupId>org.omilab.omirob</groupId>
+ <artifactId>MBotBoardBalancer</artifactId>
+ <version>1.0-SNAPSHOT</version>
+
+ <dependencies>
+ <dependency>
+ <groupId>junit</groupId>
+ <artifactId>junit</artifactId>
+ <version>4.11</version>
+ <scope>test</scope>
+ </dependency>
+ <dependency>
+ <groupId>com.sparetimelabs</groupId>
+ <artifactId>purejavacomm</artifactId>
+ <version>1.0.1</version>
+ </dependency>
+ <dependency>
+ <groupId>net.java.dev.jna</groupId>
+ <artifactId>jna</artifactId>
+ <version>4.2.2</version>
+ </dependency>
+ <dependency>
+ <groupId>org.slf4j</groupId>
+ <artifactId>slf4j-api</artifactId>
+ <version>1.7.21</version>
+ </dependency>
+ <dependency>
+ <groupId>org.slf4j</groupId>
+ <artifactId>slf4j-jdk14</artifactId>
+ <version>1.7.21</version>
+ </dependency>
+
+ <!-- Fuzzy Logic -->
+ <dependency>
+ <groupId>net.sourceforge</groupId>
+ <artifactId>jFuzzyLogic</artifactId>
+ <version>1.0</version>
+ </dependency>
+
+ </dependencies>
+ <build>
+ <plugins>
+ <plugin>
+ <groupId>org.apache.maven.plugins</groupId>
+ <artifactId>maven-compiler-plugin</artifactId>
+ <configuration>
+ <source>1.8</source>
+ <target>1.8</target>
+ </configuration>
+ </plugin>
+
+ <plugin>
+ <groupId>org.codehaus.mojo</groupId>
+ <artifactId>exec-maven-plugin</artifactId>
+ <executions>
+ <execution>
+ <phase>compile</phase>
+ <goals>
+ <goal>java</goal>
+ </goals>
+ </execution>
+ </executions>
+ <configuration>
+ <mainClass>org.omilab.omirob.mbot.balancer.App</mainClass>
+ </configuration>
+ </plugin>
+
+ <plugin>
+ <groupId>org.apache.maven.plugins</groupId>
+ <artifactId>maven-shade-plugin</artifactId>
+ <version>3.0.0</version>
+ <executions>
+ <execution>
+ <phase>package</phase>
+ <goals>
+ <goal>shade</goal>
+ </goals>
+ <configuration>
+ <transformers>
+ <transformer implementation="org.apache.maven.plugins.shade.resource.ManifestResourceTransformer">
+ <mainClass>org.omilab.omirob.mbot.balancer.App</mainClass>
+ </transformer>
+ </transformers>
+ </configuration>
+ </execution>
+ </executions>
+ </plugin>
+
+ </plugins>
+ </build>
+ <repositories>
+ <repository>
+ <id>sparetimelabs</id>
+ <name>sparetimelabs</name>
+ <url>http://www.sparetimelabs.com/maven2</url>
+ </repository>
+ </repositories>
+</project>
\ No newline at end of file
diff --git a/presentation.pdf b/presentation.pdf
new file mode 100644
index 0000000000000000000000000000000000000000..27ad448af06bebc40aea4baf2280d8843131cec7
Binary files /dev/null and b/presentation.pdf differ
diff --git a/src/main/java/org/omilab/omirob/mbot/balancer/App.java b/src/main/java/org/omilab/omirob/mbot/balancer/App.java
new file mode 100644
index 0000000000000000000000000000000000000000..e420ede385d275424ffd29df0d074ce8e10656e9
--- /dev/null
+++ b/src/main/java/org/omilab/omirob/mbot/balancer/App.java
@@ -0,0 +1,29 @@
+package org.omilab.omirob.mbot.balancer;
+
+import javafx.application.Application;
+import javafx.fxml.FXMLLoader;
+import javafx.scene.Parent;
+import javafx.scene.Scene;
+import javafx.stage.Stage;
+
+import java.io.IOException;
+
+/**
+ * Created by Mangat on 10.02.2017.
+ */
+public class App extends Application {
+
+ public static void main(String[] args) {
+ launch(args);
+ }
+
+ @Override
+ public void start(Stage primaryStage) throws IOException {
+ Parent root = FXMLLoader.load(getClass().getResource("/mbotController.fxml"));
+ primaryStage.setTitle("Mbot - Board Balancer");
+ Scene scene = new Scene(root);
+ //scene.getStylesheets().add(getClass().getResource("/fxml/app.css").toExternalForm());
+ primaryStage.setScene(scene);
+ primaryStage.show();
+ }
+}
diff --git a/src/main/java/org/omilab/omirob/mbot/balancer/MbotController.java b/src/main/java/org/omilab/omirob/mbot/balancer/MbotController.java
new file mode 100644
index 0000000000000000000000000000000000000000..b0b9823eb168e177f52e333662457f10da787983
--- /dev/null
+++ b/src/main/java/org/omilab/omirob/mbot/balancer/MbotController.java
@@ -0,0 +1,157 @@
+package org.omilab.omirob.mbot.balancer;
+
+import javafx.application.Platform;
+import javafx.concurrent.Task;
+import javafx.event.ActionEvent;
+import javafx.fxml.FXML;
+import javafx.fxml.Initializable;
+import javafx.scene.control.Button;
+import javafx.scene.control.TextArea;
+import javafx.scene.control.TextField;
+import net.sourceforge.jFuzzyLogic.FIS;
+import org.omilab.omirob.mbot.client.IMbotEvent;
+import org.omilab.omirob.mbot.client.MbotClient;
+import purejavacomm.CommPortIdentifier;
+
+import java.io.FileInputStream;
+import java.net.URL;
+import java.util.Enumeration;
+import java.util.ResourceBundle;
+
+
+/**
+ * Created by Mangat on 10.02.2017.
+ */
+public class MbotController implements Initializable, IMbotEvent {
+
+ @FXML
+ Button btnStart;
+
+ @FXML
+ Button btnStop;
+
+ @FXML
+ TextField txtFCLFile;
+
+ @FXML
+ TextField txtComPort;
+
+
+ @FXML
+ TextArea txtStatus;
+
+ private MbotClient mc;
+
+ private boolean stop;
+
+ @Override
+ public void initialize(URL location, ResourceBundle resources) {
+
+ printCommPorts();
+
+ }
+
+
+ @FXML
+ public void startBot(ActionEvent event) {
+
+ try {
+
+ stop = false;
+ String comPortName = txtComPort.getText();
+ String fuzzyFCLFile = txtFCLFile.getText();
+
+ if(comPortName == null || fuzzyFCLFile == null || comPortName.isEmpty() || fuzzyFCLFile.isEmpty())
+ {
+ txtStatus.appendText("Please provide all necessary inputs! \n");
+ return;
+ }
+
+
+ //MBot Initialization
+ mc = new MbotClient(CommPortIdentifier.getPortIdentifier(comPortName));
+ mc.addListener(this);
+ mc.reset();
+
+ // Fuzzy Logic
+ FileInputStream fileInputStream = new FileInputStream(fuzzyFCLFile);
+ FIS fis = FIS.load(fileInputStream,true);
+
+ btnStart.setDisable(true);
+
+ Thread th = new Thread(new Task<Void>() {
+ @Override
+ protected Void call() throws Exception {
+
+ while(true)
+ {
+ if(stop){
+
+ int speed = 0;
+ mc.motorLeft(speed);
+ mc.motorRight(speed);
+ mc.close();
+ Platform.runLater(new Runnable() {
+ @Override
+ public void run() {
+ btnStart.setDisable(false);
+ }
+ });
+ return null;
+ }
+
+ float y=mc.readGyro(2);
+ // Set inputs
+ System.out.println("y: " + y);
+ fis.setVariable("angle",y);
+ // Evaluate
+ fis.evaluate();
+ int speed = (int)fis.getVariable("direction").getValue();
+ mc.motorLeft(speed);
+ mc.motorRight(speed);
+ System.out.println("Direction:" + speed);
+ }
+
+ }
+ });
+ th.setDaemon(true);
+ th.start();
+
+ } catch(Exception e)
+ {
+ Platform.runLater(new Runnable() {
+ @Override
+ public void run() {
+ txtStatus.appendText(e.getMessage());
+ btnStart.setDisable(false);
+ }
+ });
+ e.printStackTrace();
+ }
+ return;
+
+ }
+
+ @FXML
+ public void stopBot(ActionEvent event)
+ {
+ stop = true;
+ }
+
+ private void printCommPorts(){
+ Enumeration<CommPortIdentifier> identifiers = CommPortIdentifier.getPortIdentifiers();
+ txtStatus.appendText("Detected com ports:\n");
+ while(identifiers.hasMoreElements()){
+ CommPortIdentifier id = (CommPortIdentifier) identifiers.nextElement();
+ txtStatus.appendText(String.format("Name: \"%s\", Type: %s, Owner: %s\n",id.getName(), id.getPortType(), id.isCurrentlyOwned()?id.getCurrentOwner():"-"));
+ }
+
+ }
+
+ @Override
+ public void onButton(boolean pressed) {
+ System.out.println("button" +pressed);
+ }
+}
+
+
diff --git a/src/main/java/org/omilab/omirob/mbot/client/DeviceType.java b/src/main/java/org/omilab/omirob/mbot/client/DeviceType.java
new file mode 100644
index 0000000000000000000000000000000000000000..afa615467d122ade96ccdb8e04124ca53d8b1be3
--- /dev/null
+++ b/src/main/java/org/omilab/omirob/mbot/client/DeviceType.java
@@ -0,0 +1,64 @@
+package org.omilab.omirob.mbot.client;
+
+/**
+ * Device IDs from mBot firmware files(mbot_firmware.ino, orion_firmware.ino)
+ *
+ * @author Martin Kunz <martin.michael.kunz@gmail.com>
+ */
+public enum DeviceType {
+ NONE((byte)0),
+ ULTRASONIC_SENSOR((byte)1),
+ TEMPERATURE_SENSOR((byte)2),
+ LIGHT_SENSOR((byte)3),
+ POTENTIONMETER((byte)4),
+ JOYSTICK((byte)5),
+ GYRO((byte)6),
+ SOUND_SENSOR((byte)7),
+ RGBLED((byte)8),
+ SEVSEG((byte)9),
+ MOTOR((byte)10),
+ SERVO((byte)11),
+ ENCODER((byte)12),
+ IR((byte)13),
+ IRREMOTE((byte)14),
+ PIRMOTION((byte)15),
+ INFRARED((byte)16),
+ LINEFOLLOWER((byte)17),
+ IRREMOTECODE((byte)18),
+ SHUTTER((byte)20),
+ LIMITSWITCH((byte)21),
+ BUTTON((byte)22),
+ HUMITURE((byte)23),
+ FLAMESENSOR((byte)24),
+ GASSENSOR((byte)25),
+ COMPASS((byte)26),
+ DIGITAL((byte)30),
+ ANALOG((byte)31),
+ PWM((byte)32),
+ SERVO_PIN((byte)33),
+ TONE((byte)34),
+ BUTTON_INNER((byte)35),
+ ULTRASONIC_ARDUINO((byte)36),
+ PULSEIN((byte)37),
+ STEPPER((byte)40),
+ LEDMATRIX((byte)41),
+ TIMER((byte)50),
+ TOUCH_SENSOR((byte)51);
+
+ private final byte id;
+
+ private DeviceType(final byte id) {
+ this.id = id;
+ }
+
+ public byte getId() {
+ return id;
+ }
+
+ public static DeviceType getById(int id) {
+ for(DeviceType e : values()) {
+ if(e.id==id) return e;
+ }
+ return null;
+ }
+}
diff --git a/src/main/java/org/omilab/omirob/mbot/client/IMbotEvent.java b/src/main/java/org/omilab/omirob/mbot/client/IMbotEvent.java
new file mode 100644
index 0000000000000000000000000000000000000000..302227cbfc0d177e19a15c10047e19a383ec0812
--- /dev/null
+++ b/src/main/java/org/omilab/omirob/mbot/client/IMbotEvent.java
@@ -0,0 +1,10 @@
+package org.omilab.omirob.mbot.client;
+
+/**
+ * Event Interface for mBot onboard button
+ *
+ * @author Martin Kunz <martin.michael.kunz@gmail.com>
+ */
+public interface IMbotEvent {
+ void onButton(boolean pressed);
+}
diff --git a/src/main/java/org/omilab/omirob/mbot/client/MbotClient.java b/src/main/java/org/omilab/omirob/mbot/client/MbotClient.java
new file mode 100644
index 0000000000000000000000000000000000000000..4fdbdc417e8a3afb87fc95a83f1a6e2e1d5d59be
--- /dev/null
+++ b/src/main/java/org/omilab/omirob/mbot/client/MbotClient.java
@@ -0,0 +1,423 @@
+package org.omilab.omirob.mbot.client;
+
+//import com.sun.istack.internal.NotNull;
+
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+import purejavacomm.*;
+
+
+import java.io.IOException;
+import java.io.InputStream;
+import java.io.OutputStream;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.util.*;
+import java.util.concurrent.LinkedBlockingQueue;
+import java.util.concurrent.TimeUnit;
+
+/**
+ * Client library for original mBot firmware
+ * Make sure matching mBlock firmware is running on your device (mbot_firmwae.ino, orion_firmware.ino, ...)
+ *
+ * @author Martin Kunz <martin.michael.kunz@gmail.com>
+ */
+public class MbotClient implements Runnable, AutoCloseable {
+ private static final Logger log = LoggerFactory.getLogger(MbotClient.class);
+
+ private static final byte TYPE_READ = 0x01;
+ private static final byte TYPE_WRITE = 0x02;
+ private static final byte TYPE_RESET = 0x04;
+ private static final byte TYPE_START = 0x05;
+ private static final byte MB_MCU_DATATYPE_ZERO = 0x00;
+ private static final byte MB_MCU_DATATYPE_BYTE = 0x01;
+ private static final byte MB_MCU_DATATYPE_FLOAT = 0x02;
+ private static final byte MB_MCU_DATATYPE_SHORT = 0x03;
+ private static final byte MB_MCU_DATATYPE_STRING = 0x04; //length(byte)+string
+ private static final byte MB_MCU_DATATYPE_DOUBLE = 0x05;
+
+ public final static int PORT_LIGHTSENSOR_INTERNAL = 6;
+ public final static int RGB_INTERNAL_IDX = 2;
+ public final static int LEDMATRIX_ACTION_STRING = 1;
+ public final static int LEDMATRIX_ACTION_BITMAP = 2;
+ public final static int LEDMATRIX_ACTION_CLOCK = 3;
+ public final static byte REMOTE_LEFT = 0x07;
+ public final static byte REMOTE_UP = 0x40;
+ public final static byte REMOTE_RIGHT = 0x09;
+ public final static byte REMOTE_DOWN = 0x19;
+ public final static byte REMOTE_A = 0x45;
+ public final static byte REMOTE_B = 0x46;
+ public final static byte REMOTE_C = 0x47;
+ public final static byte REMOTE_D = 0x44;
+ public final static byte REMOTE_E = 0x43;
+ public final static byte REMOTE_F = 0x0D;
+ public final static byte REMOTE_OK = 0x15;
+ public final static byte REMOTE_0 = 0x16;
+ public final static byte REMOTE_1 = 0x0C;
+ public final static byte REMOTE_2 = 0x18;
+ public final static byte REMOTE_3 = 0x5E;
+ public final static byte REMOTE_4 = 0x08;
+ public final static byte REMOTE_5 = 0x1C;
+ public final static byte REMOTE_6 = 0x5A;
+ public final static byte REMOTE_7 = 0x42;
+ public final static byte REMOTE_8 = 0x52;
+ public final static byte REMOTE_9 = 0x4A;
+
+
+ private final InputStream portIS;
+ private final OutputStream portOS;
+ private SerialPort serialPort;
+ Set<LinkedBlockingQueue<Result>> subscribers = Collections.synchronizedSet(new HashSet(new LinkedBlockingQueue<>()));
+ Set<IMbotEvent> externalSubscribers = Collections.synchronizedSet(new HashSet<>());
+ private volatile boolean running = true;
+ private Thread thread;
+
+ // #define GET 1
+ // #define RUN 2
+ // #define RESET 4
+ // #define START 5
+
+ public MbotClient(CommPortIdentifier portid) throws PortInUseException, IOException, UnsupportedCommOperationException {
+ serialPort = (SerialPort) portid.open("asdf", 3500);
+ serialPort.setFlowControlMode(SerialPort.FLOWCONTROL_NONE);
+ serialPort.setSerialPortParams(115200, SerialPort.DATABITS_8, SerialPort.STOPBITS_1,
+ SerialPort.PARITY_NONE);
+ serialPort.disableReceiveFraming();
+ serialPort.disableReceiveThreshold();
+ serialPort.disableReceiveTimeout();
+ portIS = serialPort.getInputStream();
+ portOS = serialPort.getOutputStream();
+ thread = new Thread(this);
+ thread.setName("Serial port read thread");
+ thread.start();
+ }
+
+ @Override
+ public void close() {
+ running = false;
+ thread.interrupt();
+ serialPort.close();
+ }
+
+ public void addListener(IMbotEvent eventif) {
+ externalSubscribers.add(eventif);
+ }
+
+
+ public void reset() throws IOException {
+ waitForResult(TYPE_RESET, DeviceType.NONE, new byte[]{}, Void.class);
+ }
+
+ public float readUltraSonic(int port) throws IOException {
+ Result r = waitForResult(TYPE_READ, DeviceType.ULTRASONIC_SENSOR, new byte[]{(byte) port}, Float.class);
+ return (float) r.result;
+ }
+
+ public float readLineFollower(int port) throws IOException {
+ Result r = waitForResult(TYPE_READ, DeviceType.LINEFOLLOWER, new byte[]{(byte) port}, Float.class);
+ return (float) r.result;
+ }
+
+ /**
+ * @param port light sensor port Internal: Nr. 6
+ * @return intensity (0..1024)
+ * @throws IOException
+ */
+ public float readLightSensor(int port) throws IOException {
+ byte[] payload = new byte[1];
+ payload[0] = (byte) (port & 0xFF);
+ Result r = waitForResult(TYPE_READ, DeviceType.LIGHT_SENSOR, payload, Float.class);
+ return (float) r.result;
+ }
+
+ public float readLightSensorOnboard() throws IOException {
+ return readLightSensor(6);
+ }
+
+ public float readSoundSensor(int port) throws IOException {
+ byte[] payload = new byte[1];
+ payload[0] = (byte) (port & 0xFF);
+ Result r = waitForResult(TYPE_READ, DeviceType.SOUND_SENSOR, payload, Float.class);
+ return (float) r.result;
+ }
+
+ public float readPotentiometer(int port) throws IOException {
+ byte[] payload = new byte[1];
+ payload[0] = (byte) (port & 0xFF);
+ Result r = waitForResult(TYPE_READ, DeviceType.POTENTIONMETER, payload, Float.class);
+ return (float) r.result;
+ }
+
+ public void motorLeft(int speed) throws IOException {
+ speed = -speed;
+ byte[] payload = new byte[3];
+ payload[0] = 0x09; //left motor port
+ payload[1] = (byte) (speed & 0xFF);
+ payload[2] = (byte) (speed >> 8 & 0xFF);
+ Result r = waitForResult(TYPE_WRITE, DeviceType.MOTOR, payload, Void.class);
+ }
+
+
+ public void motorRight(int speed) throws IOException {
+ byte[] payload = new byte[3];
+ payload[0] = 0x0a; //right motor
+ payload[1] = (byte) (speed & 0xFF);
+ payload[2] = (byte) (speed >> 8 & 0xFF);
+ Result r = waitForResult(TYPE_WRITE, DeviceType.MOTOR, payload, Void.class);
+ }
+
+ /**
+ * @param idx (1: left led, 2: right led, 0: both)
+ * @param r red (0..255)
+ * @param g green (0..255)
+ * @param b blue(0..255)
+ * @throws IOException
+ */
+ public void rgbLEDOnboard(int idx, int r, int g, int b) throws IOException {
+ rgbLED(0x07, idx, r, g, b);
+ }
+
+ public void rgbLED(int port, int idx, int r, int g, int b) throws IOException {
+ byte[] payload = new byte[6];
+ payload[0] = (byte) (port & 0xFF); //port
+ payload[1] = (byte) (MbotClient.RGB_INTERNAL_IDX & 0xFF); //slot
+ payload[2] = (byte) (idx & 0xFF); //idx
+ payload[3] = (byte) (r & 0xFF);
+ payload[4] = (byte) (g & 0xFF);
+ payload[5] = (byte) (b & 0xFF);
+ Result rr = waitForResult(TYPE_WRITE, DeviceType.RGBLED, payload, Void.class);
+ }
+
+ public void tone(int hz, int tone_time) throws IOException {
+ byte[] payload = new byte[3];
+ payload[0] = (byte) (hz & 0xFF);
+ payload[1] = (byte) (0);
+ payload[2] = (byte) (tone_time & 0xFF);
+ Result r = waitForResult(TYPE_WRITE, DeviceType.TONE, payload, Void.class);
+ }
+
+ public void ledMatrixString(int port, int x, int y, String s) throws IOException {
+ byte[] bstring = s.getBytes();
+ byte[] payload = new byte[5 + bstring.length];
+ payload[0] = (byte) (port & 0xFF);
+ payload[1] = (byte) (0x01);
+ payload[2] = (byte) (x);
+ payload[3] = (byte) (y);
+ payload[4] = (byte) (bstring.length & 0xFF);
+ for (int i = 0; i < bstring.length; i++)
+ payload[5 + i] = bstring[i];
+ Result r = waitForResult(TYPE_WRITE, DeviceType.LEDMATRIX, payload, Void.class);
+ }
+
+ public void ledMatrixBitmap(int port, int x, int y, byte[] bitmap) throws IOException {
+ byte[] payload = new byte[5 + bitmap.length];
+ payload[0] = (byte) (port & 0xFF);
+ payload[1] = (byte) (0x02);
+ payload[2] = (byte) (x);
+ payload[3] = (byte) (y);
+ for (int i = 0; i < bitmap.length; i++)
+ payload[5 + i] = bitmap[i];
+
+ Result r = waitForResult(TYPE_WRITE, DeviceType.LEDMATRIX, payload, Void.class);
+ }
+
+ public byte readIRCmd() {
+ byte[] payload = new byte[1];
+ payload[0] = 0x00;
+ Result r = waitForResult(TYPE_READ, DeviceType.IRREMOTECODE, payload, Byte.class);
+ return (byte) r.result;
+ }
+
+ public void irSend(String s) {
+ byte[] bstring = s.getBytes();
+ byte[] payload = new byte[1 + bstring.length];
+ payload[0] = (byte) (bstring.length + 3 & 0xFF);
+ for (int i = 0; i < bstring.length; i++)
+ payload[1 + i] = bstring[i];
+ Result r = waitForResult(TYPE_WRITE, DeviceType.IR, payload, Void.class);
+ }
+
+
+ public float readPirMotion(int port) {
+ byte[] payload = new byte[1];
+ payload[0] = (byte) (port & 0xFF);
+ Result r = waitForResult(TYPE_READ, DeviceType.PIRMOTION, payload, Float.class);
+ return (float) r.result;
+ }
+
+ public float readJoystick(int port, int slot) {
+ byte[] payload = new byte[2];
+ payload[0] = (byte) (port & 0xFF);
+ payload[1] = (byte) (slot & 0xFF);
+ Result r = waitForResult(TYPE_READ, DeviceType.JOYSTICK, payload, Float.class);
+ return (float) r.result;
+ }
+
+
+ public byte readHumiture(int port, int idx) {
+ byte[] payload = new byte[2];
+ payload[0] = (byte) (port & 0xFF);
+ payload[1] = (byte) (idx & 0xFF);
+ Result r = waitForResult(TYPE_READ, DeviceType.HUMITURE, payload, Byte.class);
+ return (byte) r.result;
+ }
+
+ public float readGyro(int axis) {
+ byte[] payload = new byte[2];
+ payload[0] = (byte) (0 & 0xFF);
+ payload[1] = (byte) (axis & 0xFF);
+
+ Result r = waitForResult(TYPE_READ, DeviceType.GYRO, payload, Float.class);
+ return (float) r.result;
+ }
+
+ public void stepper(Port port, int maxspeed, int distance) throws IOException {
+ byte[] payload = new byte[7];
+ payload[0] = (byte) port.getId();
+ payload[1] = (byte) (maxspeed & 0xFF);
+ payload[2] = (byte) (maxspeed >> 8 & 0xFF);
+ payload[3] = (byte) (distance & 0xFF);
+ payload[4] = (byte) (distance >> 8 & 0xFF);
+ payload[5] = (byte) (distance >> 16 & 0xFF);
+ payload[6] = (byte) (distance >> 24 & 0xFF);
+ Result r = waitForResult(TYPE_WRITE, DeviceType.STEPPER, payload, Void.class);
+ }
+
+ public void servo(Port port, Port slot, int degree) throws IOException {
+ byte[] payload = new byte[3];
+ payload[0] = port.getId();
+ payload[1] = slot.getId();
+ payload[2] = (byte) (degree);
+ Result r = waitForResult(TYPE_WRITE, DeviceType.SERVO, payload, Void.class);
+ }
+
+ private void send(byte type, DeviceType deviceType, byte[] payload) throws IOException {
+ byte[] data = new byte[6 + payload.length];
+ data[0] = (byte) (0xFF);
+ data[1] = (byte) (0x55);
+ data[2] = (byte) (payload.length + 3);
+ data[3] = (byte) (0); //index
+ data[4] = (byte) (type); //action
+ data[5] = (byte) deviceType.getId(); //device
+ System.arraycopy(payload, 0, data, 6, payload.length);
+ portOS.write(data);
+ portOS.flush();
+ }
+
+ final protected static char[] hexArray = "0123456789ABCDEF".toCharArray();
+ public static String bytesToHex(byte[] bytes) {
+ char[] hexChars = new char[bytes.length * 3];
+ for (int j = 0; j < bytes.length; j++) {
+ int v = bytes[j] & 0xFF;
+ hexChars[j * 3] = hexArray[v >>> 4];
+ hexChars[j * 3 + 1] = hexArray[v & 0x0F];
+ hexChars[j * 3 + 2] = ',';
+ }
+ return new String(hexChars);
+ }
+
+ private Result waitForResult(byte typeRW, DeviceType deviceType, byte[] payload, Class type) {
+ LinkedBlockingQueue<Result> q = new LinkedBlockingQueue<>();
+ subscribers.add(q);
+ try {
+ while (true) {
+ send(typeRW, deviceType, payload);
+ while (true) {
+ Result r = q.poll(200, TimeUnit.MILLISECONDS);
+ if (r == null)
+ break; //send again
+
+ if (r.result == null) {
+ if (type == Void.class)
+ return r;
+ } else {
+ if (type.isAssignableFrom(r.result.getClass())) {
+ return r;
+ }
+ }
+ }
+ }
+ } catch (Exception e) {
+ throw new RuntimeException(e);
+ } finally {
+ subscribers.remove(q);
+ }
+ }
+
+ //@NotNull
+ private Result readResponse() throws IOException{
+ byte[] sync = new byte[2];
+ while (true) {
+ sync[0] = sync[1];
+ sync[1] = (byte) portIS.read();
+ if (Arrays.equals(sync, new byte[]{(byte) 0xFF, (byte) 0x55}))
+ break;
+ }
+
+ byte[] first = new byte[2];
+ first[0] = (byte) portIS.read();
+ first[1] = (byte) portIS.read();
+ if (Arrays.equals(first, new byte[]{(byte) 0x0d, (byte) 0x0a})) { //OK reply (Motors, ..?)
+ Result r = new Result<Void>();
+ return r;
+ } else if (Arrays.equals(first, new byte[]{(byte) 0x80, (byte) 0x01})) { //device button press
+ Result r = new Result<Boolean>();
+ r.deviceType = DeviceType.BUTTON_INNER;
+ r.result = portIS.read() == 1 ? true : false;
+ readSuffix();
+ return r;
+ } else {
+ byte index = first[0];
+ byte type = first[1];
+ switch (type) {
+ case MB_MCU_DATATYPE_FLOAT: {//float
+ byte[] data = new byte[4];
+ portIS.read(data);
+ Result r = new Result<Float>();
+ r.result = ByteBuffer.wrap(data).order(ByteOrder.LITTLE_ENDIAN).getFloat();
+ r.deviceType = DeviceType.getById(type);
+ readSuffix();
+ return r;
+ }
+ case MB_MCU_DATATYPE_BYTE: {//byte
+ byte res = (byte) portIS.read();
+ Result r = new Result<Byte>();
+ r.result = res;
+ r.deviceType = DeviceType.getById(type);
+ readSuffix();
+ return r;
+ }
+ default:
+ log.warn("Unknows Type: " + type);
+ Result r = new Result<Void>();
+ return r;
+ }
+ }
+ }
+
+ private void readSuffix() throws IOException {
+ byte[] suffix = new byte[2]; //od 0a
+ portIS.read(suffix);
+ }
+
+ @Override
+ public void run() {
+ try {
+ while (running) {
+ Result r = readResponse();
+ for (LinkedBlockingQueue<Result> q : subscribers) {
+ q.put(r);
+ }
+
+ for (IMbotEvent s : externalSubscribers) {
+ if (r.deviceType == DeviceType.BUTTON_INNER) {
+ s.onButton((Boolean) r.result);
+ }
+ }
+ }
+ } catch (InterruptedException|IOException e) {
+ running = false;
+ log.info("Serialport Thread terminating");
+ }
+ }
+}
diff --git a/src/main/java/org/omilab/omirob/mbot/client/Port.java b/src/main/java/org/omilab/omirob/mbot/client/Port.java
new file mode 100644
index 0000000000000000000000000000000000000000..3df61059c2bd9702b21ad1f93dc7680159b03a77
--- /dev/null
+++ b/src/main/java/org/omilab/omirob/mbot/client/Port.java
@@ -0,0 +1,52 @@
+package org.omilab.omirob.mbot.client;
+
+/**
+ * Port and Slot IDs as defined in MePort.h
+ *
+ * @author Martin Kunz <martin.michael.kunz@gmail.com>
+ */
+public enum Port {
+ NC((byte)0),
+ PORT_1((byte)0x01),
+ PORT_2((byte)0x02),
+ PORT_3((byte)0x03),
+ PORT_4((byte)0x04),
+ PORT_5((byte)0x05),
+ PORT_6((byte)0x06),
+ PORT_7((byte)0x07),
+ PORT_8((byte)0x08),
+ PORT_9((byte)0x09),
+ PORT_10((byte)0x0a),
+ PORT_11((byte)0x0b),
+ PORT_12((byte)0x0c),
+ PORT_13((byte)0x0d),
+ PORT_14((byte)0x0e),
+ PORT_15((byte)0x0f),
+ PORT_16((byte)0x10),
+ M1((byte)0x09),
+ M2((byte)0x09),
+ PORT_RGB((byte)0x05),
+ PORT_LightSensor((byte)0x05),
+ SLOT1((byte)1),
+ SLOT2((byte)1),
+ SLOT3((byte)1),
+ SLOT4((byte)1)
+ ;
+
+ private final byte id;
+
+ Port(final byte id) {
+ this.id = id;
+ }
+
+ public byte getId() {
+ return id;
+ }
+
+ public static Port getById(int id) {
+ for(Port e : values()) {
+ if(e.id==id) return e;
+ }
+ return null;
+ }
+}
diff --git a/src/main/java/org/omilab/omirob/mbot/client/Result.java b/src/main/java/org/omilab/omirob/mbot/client/Result.java
new file mode 100644
index 0000000000000000000000000000000000000000..3b625cfa9d6c3524308b27a6625ccbf345adf3a7
--- /dev/null
+++ b/src/main/java/org/omilab/omirob/mbot/client/Result.java
@@ -0,0 +1,11 @@
+package org.omilab.omirob.mbot.client;
+
+/**
+ * @author Martin Kunz <martin.michael.kunz@gmail.com>
+ */
+public class Result<T> {
+ public int index;
+ public T result;
+ public DeviceType deviceType;
+
+}
diff --git a/src/main/resources/mbotController.fxml b/src/main/resources/mbotController.fxml
new file mode 100644
index 0000000000000000000000000000000000000000..50de686f2fa975a93c1c4d655e93bd184b9e919d
--- /dev/null
+++ b/src/main/resources/mbotController.fxml
@@ -0,0 +1,81 @@
+<?xml version="1.0" encoding="UTF-8"?>
+
+<?import javafx.scene.control.Button?>
+<?import javafx.scene.control.Label?>
+<?import javafx.scene.control.TextArea?>
+<?import javafx.scene.control.TextField?>
+<?import javafx.scene.layout.AnchorPane?>
+<?import javafx.scene.layout.ColumnConstraints?>
+<?import javafx.scene.layout.GridPane?>
+<?import javafx.scene.layout.RowConstraints?>
+<?import javafx.scene.text.Font?>
+
+<AnchorPane maxHeight="584.0" maxWidth="800.0" minHeight="400.0" minWidth="800.0" prefHeight="551.0" prefWidth="800.0" xmlns="http://javafx.com/javafx/8.0.111" xmlns:fx="http://javafx.com/fxml/1" fx:controller="org.omilab.omirob.mbot.balancer.MbotController">
+ <children>
+ <GridPane hgap="20.0" layoutX="23.0" layoutY="40.0" prefHeight="480.0" prefWidth="760.0" vgap="20.0" AnchorPane.leftAnchor="20.0" AnchorPane.rightAnchor="20.0" AnchorPane.topAnchor="40.0">
+ <columnConstraints>
+ <ColumnConstraints hgrow="SOMETIMES" maxWidth="188.0" minWidth="-Infinity" prefWidth="188.0" />
+ <ColumnConstraints hgrow="SOMETIMES" maxWidth="577.5" minWidth="-Infinity" prefWidth="552.0" />
+ </columnConstraints>
+ <rowConstraints>
+ <RowConstraints maxHeight="55.428558349609375" minHeight="32.0" prefHeight="38.28570556640625" vgrow="SOMETIMES" />
+ <RowConstraints maxHeight="88.0" minHeight="30.5" prefHeight="31.0" vgrow="SOMETIMES" />
+ <RowConstraints maxHeight="201.5" minHeight="150.0" prefHeight="201.0" vgrow="NEVER" />
+ <RowConstraints minHeight="150.0" prefHeight="30.0" vgrow="NEVER" />
+ </rowConstraints>
+ <children>
+ <Label alignment="TOP_LEFT" text="COM Port*" GridPane.rowIndex="1" GridPane.valignment="TOP">
+ <font>
+ <Font size="20.0" />
+ </font></Label>
+ <Label alignment="TOP_LEFT" text="Fuzzy Set File .fcl*">
+ <font>
+ <Font size="20.0" />
+ </font>
+ </Label>
+ <TextField fx:id="txtFCLFile" prefHeight="33.0" prefWidth="505.0" GridPane.columnIndex="1">
+ <font>
+ <Font size="14.0" />
+ </font></TextField>
+ <TextArea fx:id="txtStatus" prefHeight="92.0" prefWidth="530.0" GridPane.columnIndex="1" GridPane.rowIndex="2" />
+ <GridPane GridPane.columnIndex="1" GridPane.rowIndex="3">
+ <columnConstraints>
+ <ColumnConstraints hgrow="SOMETIMES" maxWidth="1.7976931348623157E308" minWidth="10.0" prefWidth="100.0" />
+ <ColumnConstraints hgrow="SOMETIMES" maxWidth="1.7976931348623157E308" minWidth="10.0" prefWidth="100.0" />
+ </columnConstraints>
+ <rowConstraints>
+ <RowConstraints minHeight="10.0" prefHeight="30.0" vgrow="SOMETIMES" />
+ </rowConstraints>
+ <children>
+ <Button fx:id="btnStart" mnemonicParsing="false" onAction="#startBot" prefHeight="112.0" prefWidth="258.0" text="Start">
+ <font>
+ <Font name="System Bold" size="39.0" />
+ </font>
+ </Button>
+ <Button fx:id="btnStop" mnemonicParsing="false" onAction="#stopBot" prefHeight="112.0" prefWidth="265.0" text="Stop" GridPane.columnIndex="1">
+ <font>
+ <Font name="System Bold" size="39.0" />
+ </font>
+ </Button>
+ </children>
+ </GridPane>
+ <TextField fx:id="txtComPort" prefHeight="33.0" prefWidth="505.0" GridPane.columnIndex="1" GridPane.rowIndex="1">
+ <font>
+ <Font size="14.0" />
+ </font></TextField>
+ <Label alignment="TOP_LEFT" text="Status" GridPane.rowIndex="2">
+ <font>
+ <Font size="20.0" />
+ </font>
+ </Label>
+ </children>
+ </GridPane>
+ <GridPane>
+ <rowConstraints>
+ <RowConstraints minHeight="10.0" prefHeight="30.0" vgrow="SOMETIMES" />
+ <RowConstraints minHeight="10.0" prefHeight="30.0" vgrow="SOMETIMES" />
+ <RowConstraints minHeight="10.0" prefHeight="30.0" vgrow="SOMETIMES" />
+ </rowConstraints>
+ </GridPane>
+ </children>
+</AnchorPane>