Compass Sensor Implementation V1

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

@@ -159,7 +159,7 @@ public final void handleSubscribeQueue() throws ParseException {
                             // 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");
+                             System.out.println("compass sensor message received");
                             compassSensor.handleSubscription(this, m);
                         }
 

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

@@ -1,22 +1,99 @@
 package swarm.robot.sensors;
 
+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 TBD
+ * @author Dinuka Mudalige
  */
 public class CompassSensor extends AbstractSensor {
+    private enum mqttTopic {
+        COMPASS_OUT
+    }
+
+    private final static int MQTT_TIMEOUT = 2000;
+
+    private boolean compassLock = false;
+    private HashMap<CompassSensor.mqttTopic, String> topicsSub = new HashMap<>();
+//    private CompassReadingType compass;
+    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);
     }
 
-    public double readComapss() throws Exception {
-        // To be implemented to return the heading direction of the robot
+    /**
+     * 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{
+        heading = Integer.parseInt(m.message); // physical robot's heading directly from the MQTT subscription
+        System.out.println("<CompassSensor> Received topic: " + m.topic + " = "+ heading);
+    }
 
-        return 0;
+    /**
+     * Get the emulated compass sensor reading from the simulator
+     *
+     * @return heading as double
+     * @throws SensorException
+     */
+    public double readCompass() throws Exception {
+        try {
+            if (robot instanceof VirtualRobot) {
+                heading = robot.coordinates.getHeading();
+            } else {
+                robot.handleSubscribeQueue();
+            }
+        } catch (ParseException e) {
+            e.printStackTrace();
+        }
+        robot.delay(100);
+        return heading;
     }
+    /* public double readCompass() throws Exception {
+        compassLock = true; // Acquire the compass sensor lock
+
+        long startTime = System.currentTimeMillis();
+        boolean timeout = false;
+
+        while (compassLock && !timeout) {
+            try {
+                robot.handleSubscribeQueue();
+            } catch (ParseException e) {
+                e.printStackTrace();
+            }
+
+            robot.delay(100);
+            timeout = (System.currentTimeMillis() - startTime > MQTT_TIMEOUT);
+        }
+
+        if (timeout) {
+            throw new SensorException("Compass sensor timeout");
+        }
+
+        return heading;
+    } */
 }