From 682a93f8abeac502db39684f262143d7488810c9 Mon Sep 17 00:00:00 2001 From: dhmudalige Date: Wed, 28 Feb 2024 20:46:15 +0530 Subject: [PATCH 1/6] Update CompassSensor.java --- .../swarm/robot/sensors/CompassSensor.java | 70 +++++++++++++++++-- 1 file changed, 66 insertions(+), 4 deletions(-) diff --git a/src/main/java/swarm/robot/sensors/CompassSensor.java b/src/main/java/swarm/robot/sensors/CompassSensor.java index 5565415..77c4798 100644 --- a/src/main/java/swarm/robot/sensors/CompassSensor.java +++ b/src/main/java/swarm/robot/sensors/CompassSensor.java @@ -1,22 +1,84 @@ 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.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 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); + } + + /** + * Get the emulated compass sensor reading from the simulator + * + * @return heading as double + * @throws SensorException + */ + 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 0; + return heading; } } From 340096870d8a111ebec8c1c15c17b71f31585bcf Mon Sep 17 00:00:00 2001 From: dhmudalige Date: Tue, 5 Mar 2024 22:09:11 +0530 Subject: [PATCH 2/6] Update project updated the virtual compass reading technique --- src/main/java/Robots/MyTestRobot.java | 2 +- src/main/java/swarm/robot/Robot.java | 2 +- .../swarm/robot/sensors/CompassSensor.java | 19 +++++++++++++++++-- 3 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/main/java/Robots/MyTestRobot.java b/src/main/java/Robots/MyTestRobot.java index d4b249a..6a4b66c 100644 --- a/src/main/java/Robots/MyTestRobot.java +++ b/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); } } diff --git a/src/main/java/swarm/robot/Robot.java b/src/main/java/swarm/robot/Robot.java index e6823bf..5314e69 100644 --- a/src/main/java/swarm/robot/Robot.java +++ b/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); } diff --git a/src/main/java/swarm/robot/sensors/CompassSensor.java b/src/main/java/swarm/robot/sensors/CompassSensor.java index 77c4798..fbc84b5 100644 --- a/src/main/java/swarm/robot/sensors/CompassSensor.java +++ b/src/main/java/swarm/robot/sensors/CompassSensor.java @@ -4,6 +4,7 @@ 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; @@ -49,7 +50,8 @@ private void subscribe(CompassSensor.mqttTopic key, String topic) { */ @Override public void handleSubscription(Robot robot, MqttMsg m) throws RuntimeException{ - heading = Integer.parseInt(m.message); + heading = Integer.parseInt(m.message); // physical robot's heading directly from the MQTT subscription + System.out.println(" Received topic: " + m.topic + " = "+ heading); } /** @@ -59,6 +61,19 @@ public void handleSubscription(Robot robot, MqttMsg m) throws RuntimeException{ * @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(); @@ -80,5 +95,5 @@ public double readCompass() throws Exception { } return heading; - } + } */ } From cfdf83a2be54c8842bfabf84b0ff3ab82817fbb6 Mon Sep 17 00:00:00 2001 From: dhmudalige Date: Tue, 5 Mar 2024 22:10:21 +0530 Subject: [PATCH 3/6] Test implementation tested with ObstacleAvoidRobot --- src/main/java/Robots/ObstacleAvoidRobot.java | 63 ++++++++++++++++++++ src/main/java/swarm/App.java | 3 +- 2 files changed, 65 insertions(+), 1 deletion(-) create mode 100644 src/main/java/Robots/ObstacleAvoidRobot.java diff --git a/src/main/java/Robots/ObstacleAvoidRobot.java b/src/main/java/Robots/ObstacleAvoidRobot.java new file mode 100644 index 0000000..05c0d72 --- /dev/null +++ b/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); + } + } + + } +} diff --git a/src/main/java/swarm/App.java b/src/main/java/swarm/App.java index f2f58f9..79227a9 100644 --- a/src/main/java/swarm/App.java +++ b/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 From 45ded4f88098938c814a79242b847e2313d3507d Mon Sep 17 00:00:00 2001 From: dhmudalige Date: Sat, 16 Mar 2024 16:06:53 +0530 Subject: [PATCH 4/6] Update project updating compass directions V1 --- src/main/java/Robots/ObstacleAvoidRobot.java | 8 +++-- src/main/java/swarm/App.java | 2 +- src/main/java/swarm/robot/Robot.java | 2 +- .../swarm/robot/sensors/CompassSensor.java | 33 ++++++++++++++++--- 4 files changed, 37 insertions(+), 8 deletions(-) diff --git a/src/main/java/Robots/ObstacleAvoidRobot.java b/src/main/java/Robots/ObstacleAvoidRobot.java index 05c0d72..b4995d1 100644 --- a/src/main/java/Robots/ObstacleAvoidRobot.java +++ b/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()); diff --git a/src/main/java/swarm/App.java b/src/main/java/swarm/App.java index 79227a9..dceba01 100644 --- a/src/main/java/swarm/App.java +++ b/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 diff --git a/src/main/java/swarm/robot/Robot.java b/src/main/java/swarm/robot/Robot.java index 5314e69..b833d20 100644 --- a/src/main/java/swarm/robot/Robot.java +++ b/src/main/java/swarm/robot/Robot.java @@ -82,7 +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); + compassSensor = new CompassSensor(this, coordinates.getHeading(), robotMqttClient); neoPixel = new NeoPixel(this, robotMqttClient); diff --git a/src/main/java/swarm/robot/sensors/CompassSensor.java b/src/main/java/swarm/robot/sensors/CompassSensor.java index f920215..7de5fce 100644 --- a/src/main/java/swarm/robot/sensors/CompassSensor.java +++ b/src/main/java/swarm/robot/sensors/CompassSensor.java @@ -21,10 +21,17 @@ private enum mqttTopic { } private HashMap topicsSub = new HashMap<>(); - private double heading; + private double reading; - public CompassSensor(Robot robot, RobotMqttClient mqttClient) { + private double bearing; + + 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 + + public CompassSensor(Robot robot, double initialBearing, RobotMqttClient mqttClient) { super(robot, mqttClient); + this.bearing = initialBearing; subscribe(CompassSensor.mqttTopic.COMPASS_OUT, "sensor/compass/" + robotId + "/?"); } @@ -65,7 +72,15 @@ 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(); + if (Math.abs(robotHead - bearing) < TOLERANCE) { // 'robotHead' is approximately equal to 'bearing' + reading = Math.abs(NORTH) + bearing; + } else { + reading = convertToNorth(robotHead, bearing); + bearing = reading; + } + } else { robot.handleSubscribeQueue(); } @@ -73,7 +88,7 @@ public double readCompass() { e.printStackTrace(); } - return heading; + return reading; } /** @@ -90,4 +105,14 @@ public void sendCompass(double compass) { robotMqttClient.publish("sensor/compass/", obj.toJSONString()); } + private double convertToNorth(double rotatedAngle, double currentOrientation) { + // Normalize the current orientation to ensure it's within [0, 360) + currentOrientation = (currentOrientation + 360) % 360; + + double bearing = Math.abs(NORTH) + currentOrientation + rotatedAngle; + + bearing = (bearing + 360) % 360; + + return bearing; + } } From 39fc967d32282eef71e7e475feaa0b160b335e12 Mon Sep 17 00:00:00 2001 From: dhmudalige Date: Sat, 16 Mar 2024 17:00:52 +0530 Subject: [PATCH 5/6] Update project updating compass directions V2 --- src/main/java/swarm/robot/Robot.java | 2 +- .../swarm/robot/sensors/CompassSensor.java | 26 ++++++------------- 2 files changed, 9 insertions(+), 19 deletions(-) diff --git a/src/main/java/swarm/robot/Robot.java b/src/main/java/swarm/robot/Robot.java index b833d20..5314e69 100644 --- a/src/main/java/swarm/robot/Robot.java +++ b/src/main/java/swarm/robot/Robot.java @@ -82,7 +82,7 @@ public void setup() { distSensor = new DistanceSensor(this, robotMqttClient); proximitySensor = new ProximitySensor(this, robotMqttClient); colorSensor = new ColorSensor(this, robotMqttClient); - compassSensor = new CompassSensor(this, coordinates.getHeading(), robotMqttClient); + compassSensor = new CompassSensor(this, robotMqttClient); neoPixel = new NeoPixel(this, robotMqttClient); diff --git a/src/main/java/swarm/robot/sensors/CompassSensor.java b/src/main/java/swarm/robot/sensors/CompassSensor.java index 7de5fce..5ae4cea 100644 --- a/src/main/java/swarm/robot/sensors/CompassSensor.java +++ b/src/main/java/swarm/robot/sensors/CompassSensor.java @@ -23,15 +23,12 @@ private enum mqttTopic { private HashMap topicsSub = new HashMap<>(); private double reading; - private double bearing; +// private static final double TOLERANCE = 0.000001; // Value should be closer to zero - 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) - private static final double NORTH = 0.0; // Should measure relative to a reference - - public CompassSensor(Robot robot, double initialBearing, RobotMqttClient mqttClient) { + public CompassSensor(Robot robot, RobotMqttClient mqttClient) { super(robot, mqttClient); - this.bearing = initialBearing; subscribe(CompassSensor.mqttTopic.COMPASS_OUT, "sensor/compass/" + robotId + "/?"); } @@ -74,12 +71,7 @@ public double readCompass() { if (robot instanceof VirtualRobot) { double robotHead = robot.coordinates.getHeading(); - if (Math.abs(robotHead - bearing) < TOLERANCE) { // 'robotHead' is approximately equal to 'bearing' - reading = Math.abs(NORTH) + bearing; - } else { - reading = convertToNorth(robotHead, bearing); - bearing = reading; - } + reading = convertToNorth(robotHead); } else { robot.handleSubscribeQueue(); @@ -105,14 +97,12 @@ public void sendCompass(double compass) { robotMqttClient.publish("sensor/compass/", obj.toJSONString()); } - private double convertToNorth(double rotatedAngle, double currentOrientation) { - // Normalize the current orientation to ensure it's within [0, 360) - currentOrientation = (currentOrientation + 360) % 360; - - double bearing = Math.abs(NORTH) + currentOrientation + rotatedAngle; + 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; } -} +} \ No newline at end of file From 0aa2252caaa1ae00eda9b71987bcc4beaa47a69a Mon Sep 17 00:00:00 2001 From: dhmudalige Date: Mon, 25 Mar 2024 07:59:05 +0530 Subject: [PATCH 6/6] Update CompassSensor.java changed NORTH reference value --- src/main/java/swarm/robot/sensors/CompassSensor.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/swarm/robot/sensors/CompassSensor.java b/src/main/java/swarm/robot/sensors/CompassSensor.java index 5ae4cea..e757c2e 100644 --- a/src/main/java/swarm/robot/sensors/CompassSensor.java +++ b/src/main/java/swarm/robot/sensors/CompassSensor.java @@ -25,7 +25,7 @@ private enum mqttTopic { // 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) + private static final double NORTH = 90.0; // Should measure relative to a reference (here, to the horizontal) public CompassSensor(Robot robot, RobotMqttClient mqttClient) { super(robot, mqttClient);