Compass Sensor Implementation V2

src/main/java/Robots/ObstacleAvoidRobot.java

@@ -13,6 +13,8 @@ public class ObstacleAvoidRobot extends VirtualRobot {
     // The default movement speed
     private int defaultMoveSpeed = 100;
 
+//    private int[] restrictedAngles = {90, 180};
+
     public ObstacleAvoidRobot(int id, double x, double y, double heading) {
         super(id, x, y, heading);
     }
@@ -34,6 +36,7 @@ public void loop() throws Exception {
                 // number
                 int random = -1000 + ((int) ((Math.random() * 2000)));
                 int sign = (random % 2 == 0) ? 1 : -1;
+//                int angle = restrictedAngles[random % restrictedAngles.length];
 
                 System.out.println("\t Wall detected, go back and rotate " + ((sign > 0) ? "CW" : "CCW"));
 
@@ -43,14 +46,15 @@ public void loop() throws Exception {
                 // rotate
                 int loopCount = 0;
                 while (distSensor.getDistance() < distanceThreshold && loopCount < 5) {
-                    // Maximum 5 tries to rotate and find a obstale free path
+                    // Maximum 5 tries to rotate and find a obstacle free path
                     motion.rotate((int) (defaultMoveSpeed * 0.75 * sign), 1000);
+//                    motion.rotateDegree((int) (defaultMoveSpeed * 0.75 * sign), angle);
                     loopCount++;
                 }
 
                 // rotate a little more
                 motion.rotate((int) (defaultMoveSpeed * 0.75 * sign), 500);
-
+//                motion.rotateDegree((int) (defaultMoveSpeed * 0.75 * sign), angle);
 
                 System.out.println("\t\t Compass reading: " + compassSensor.readCompass());
 

src/main/java/swarm/App.java

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

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

@@ -21,7 +21,11 @@ private enum mqttTopic {
     }
 
     private HashMap<CompassSensor.mqttTopic, String> topicsSub = new HashMap<>();
-    private double heading;
+    private double reading;
+
+//    private static final double TOLERANCE = 0.000001;  // Value should be closer to zero
+
+    private static final double NORTH = 0.0;  // Should measure relative to a reference (here, to the horizontal)
 
     public CompassSensor(Robot robot, RobotMqttClient mqttClient) {
         super(robot, mqttClient);
@@ -65,15 +69,18 @@ public void handleSubscription(Robot robot, MqttMsg m) throws RuntimeException {
     public double readCompass() {
         try {
             if (robot instanceof VirtualRobot) {
-                heading = robot.coordinates.getHeading();
+
+                double robotHead = robot.coordinates.getHeading();
+                reading = convertToNorth(robotHead);
+
             } else {
                 robot.handleSubscribeQueue();
             }
         } catch (ParseException e) {
             e.printStackTrace();
         }
 
-        return heading;
+        return reading;
     }
 
     /**
@@ -90,4 +97,12 @@ public void sendCompass(double compass) {
         robotMqttClient.publish("sensor/compass/", obj.toJSONString());
     }
 
-}
+    private double convertToNorth(double measuredValue) {
+        double bearing = NORTH - measuredValue;
+
+        // Normalize the result to ensure it's within [0, 360)
+        bearing = (bearing + 360) % 360;
+
+        return bearing;
+    }
+}