Compass Sensor Implementation

src/main/java/Robots/MyTestRobot.java

@@ -17,7 +17,7 @@ public void loop() throws Exception {
         super.loop();
 
         if (state == robotState.RUN) {
-            System.out.println("Test");
+            System.out.println("\t\t Compass reading: " + compassSensor.readCompass());
             delay(1000);
         }
     }

src/main/java/Robots/ObstacleAvoidRobot.java

@@ -0,0 +1,63 @@
+// This robot will move freely, avoiding obstacles 
+// Written by Nuwan Jaliyagoda 
+
+package Robots;
+
+import swarm.robot.VirtualRobot;
+
+public class ObstacleAvoidRobot extends VirtualRobot {
+
+    // The minimum distance that robot tries to keep with the obstacles
+    private int distanceThreshold = 15;
+
+    // The default movement speed
+    private int defaultMoveSpeed = 100;
+
+    public ObstacleAvoidRobot(int id, double x, double y, double heading) {
+        super(id, x, y, heading);
+    }
+
+    public void setup() {
+        super.setup();
+    }
+
+    @Override
+    public void loop() throws Exception {
+        super.loop();
+
+        if (state == robotState.RUN) {
+            double dist = distSensor.getDistance();
+
+            if (dist < distanceThreshold) {
+                // Generate a random number in [-1000,1000] range
+                // if even, rotate CW, otherwise rotate CCW an angle depends on the random
+                // number
+                int random = -1000 + ((int) ((Math.random() * 2000)));
+                int sign = (random % 2 == 0) ? 1 : -1;
+
+                System.out.println("\t Wall detected, go back and rotate " + ((sign > 0) ? "CW" : "CCW"));
+
+                // Go back a little
+                motion.move(-100, -100, 900);
+
+                // rotate
+                int loopCount = 0;
+                while (distSensor.getDistance() < distanceThreshold && loopCount < 5) {
+                    // Maximum 5 tries to rotate and find a obstale free path
+                    motion.rotate((int) (defaultMoveSpeed * 0.75 * sign), 1000);
+                    loopCount++;
+                }
+
+                // rotate a little more
+                motion.rotate((int) (defaultMoveSpeed * 0.75 * sign), 500);
+
+
+                System.out.println("\t\t Compass reading: " + compassSensor.readCompass());
+
+            } else {
+                motion.move(defaultMoveSpeed, defaultMoveSpeed, 1000);
+            }
+        }
+
+    }
+}

src/main/java/swarm/App.java

@@ -34,7 +34,8 @@ public static void main(String[] args) {
             reader.close();
 
             // Start a single robot
-            Robot robot = new MyTestRobot(10, 0, 0, 90);
+//            Robot robot = new MyTestRobot(4, 18, 6, 180);
+            Robot robot = new ObstacleAvoidRobot(4, 18, 6, 180);
             new Thread(robot).start();
 
             // // Start a swarm of robots

src/main/java/swarm/robot/Robot.java

@@ -13,6 +13,7 @@
 import swarm.robot.indicator.NeoPixel;
 import swarm.robot.sensors.ColorSensor;
 import swarm.robot.sensors.DistanceSensor;
+import swarm.robot.sensors.CompassSensor;
 import swarm.robot.sensors.ProximitySensor;
 
 /**
@@ -26,6 +27,7 @@ public abstract class Robot implements Runnable, IRobotState {
     public DistanceSensor distSensor;
     public ProximitySensor proximitySensor;
     public ColorSensor colorSensor;
+    public CompassSensor compassSensor;
 
     // Communication -----------------------------------------------------
     public SimpleCommunication simpleComm;
@@ -80,6 +82,7 @@ public void setup() {
         distSensor = new DistanceSensor(this, robotMqttClient);
         proximitySensor = new ProximitySensor(this, robotMqttClient);
         colorSensor = new ColorSensor(this, robotMqttClient);
+        compassSensor = new CompassSensor(this, robotMqttClient);
 
         neoPixel = new NeoPixel(this, robotMqttClient);
 
@@ -155,6 +158,9 @@ public final void handleSubscribeQueue() throws ParseException {
                         } else if (m.topicGroups[1].equals("proximity")) {
                             // System.out.println("proximity sensor message received");
                             proximitySensor.handleSubscription(this, m);
+                        } else if (m.topicGroups[1].equals("compass")) {
+                             System.out.println("compass sensor message received");
+                            compassSensor.handleSubscription(this, m);
                         }
 
                         break;

src/main/java/swarm/robot/sensors/CompassSensor.java

@@ -0,0 +1,93 @@
+package swarm.robot.sensors;
+
+import org.json.simple.JSONObject;
+import org.json.simple.parser.ParseException;
+import swarm.mqtt.MqttMsg;
+import swarm.mqtt.RobotMqttClient;
+import swarm.robot.Robot;
+import swarm.robot.VirtualRobot;
+import swarm.robot.exception.SensorException;
+
+import java.util.HashMap;
+
+/**
+ * Compass Sensors Emulator class
+ * 
+ * @author Dinuka Mudalige
+ */
+public class CompassSensor extends AbstractSensor {
+    private enum mqttTopic {
+        COMPASS_OUT
+    }
+
+    private HashMap<CompassSensor.mqttTopic, String> topicsSub = new HashMap<>();
+    private double heading;
+
+    public CompassSensor(Robot robot, RobotMqttClient mqttClient) {
+        super(robot, mqttClient);
+        subscribe(CompassSensor.mqttTopic.COMPASS_OUT, "sensor/compass/" + robotId + "/?");
+    }
+
+    /**
+     * Subscribe to a MQTT topic
+     *
+     * @param key   Subscription topic key
+     * @param topic Subscription topic value
+     */
+    private void subscribe(CompassSensor.mqttTopic key, String topic) {
+        topicsSub.put(key, topic);
+        robotMqttClient.subscribe(topic);
+    }
+
+    /**
+     * Handle compassSensor related MQTT subscriptions
+     *
+     * @param robot Robot object
+     * @param m     Subscription topic received object
+     */
+    @Override
+    public void handleSubscription(Robot robot, MqttMsg m) throws RuntimeException {
+        String topic = m.topic, msg = m.message;
+
+        if (topic.equals(topicsSub.get(mqttTopic.COMPASS_OUT))) {
+            sendCompass(readCompass());
+        } else {
+            System.out.println("Received (unknown): " + topic + "> " + msg);
+        }
+    }
+
+    /**
+     * Get the emulated compass sensor reading from the simulator
+     *
+     * @return heading as double
+     * @throws SensorException
+     */
+    public double readCompass() {
+        try {
+            if (robot instanceof VirtualRobot) {
+                heading = robot.coordinates.getHeading();
+            } else {
+                robot.handleSubscribeQueue();
+            }
+        } catch (ParseException e) {
+            e.printStackTrace();
+        }
+
+        return heading;
+    }
+
+    /**
+     * Send the compass reading to simulation server
+     * 
+     * @param compass compass reading
+     */
+    public void sendCompass(double compass) {
+
+        JSONObject obj = new JSONObject();
+        obj.put("id", robotId);
+        obj.put("compass", compass);
+
+        robotMqttClient.publish("sensor/compass/", obj.toJSONString());
+    }
+
+}