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/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 diff --git a/src/main/java/swarm/robot/Robot.java b/src/main/java/swarm/robot/Robot.java index 8b76f39..5314e69 100644 --- a/src/main/java/swarm/robot/Robot.java +++ b/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; diff --git a/src/main/java/swarm/robot/sensors/CompassSensor.java b/src/main/java/swarm/robot/sensors/CompassSensor.java new file mode 100644 index 0000000..f920215 --- /dev/null +++ b/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 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()); + } + +}