Small changes

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

@@ -16,7 +16,7 @@
 import swarm.robot.sensors.ProximitySensor;
 
 /**
- * Abstract class implementation for Robot
+ * Abstract Class implementation for Robot
  * 
  * @author Nuwan Jaliyagoda
  */
@@ -49,10 +49,10 @@ public abstract class Robot implements Runnable, IRobotState {
      * Abstract Robot class
      * 
      * @param id      robot Id
-     * @param x       coordinate as double
-     * @param y       coordinate as double
-     * @param heading direction in degrees, as double
-     * @param reality reality, currently supports only 'V'
+     * @param x       X coordinate as double
+     * @param y       Y coordinate as double
+     * @param heading Heading direction in degrees, as double
+     * @param reality Reality of the robot, currently only support 'V'
      */
     public Robot(int id, double x, double y, double heading, char reality) {
 
@@ -130,7 +130,7 @@ public final void run() {
     /**
      * Handle MQTT subscriptions
      */
-    public void handleSubscribeQueue() throws ParseException {
+    public final void handleSubscribeQueue() throws ParseException {
 
         while (!robotMqttClient.inQueue.isEmpty()) {
             MqttMsg m = robotMqttClient.inQueue.poll();
@@ -203,7 +203,7 @@ private void handlePublishQueue() {
     }
 
     /**
-     * Method which handle START command
+     * Method which handles START command
      */
     @Override
     public void start() {
@@ -212,7 +212,7 @@ public void start() {
     }
 
     /**
-     * Method which handle STOP command
+     * Method which handles STOP command
      */
     @Override
     public void stop() {
@@ -221,7 +221,7 @@ public void stop() {
     }
 
     /**
-     * Method which handle RESET command
+     * Method which handles RESET command
      */
     @Override
     public void reset() {

src/main/java/swarm/robot/VirtualRobot.java

@@ -1,7 +1,7 @@
 package swarm.robot;
 
 /**
- * VirtualRobot implementation for Robot
+ * VirtualRobot implementation of the Robot
  * 
  * @author Nuwan Jaliyagoda
  */
@@ -11,24 +11,24 @@ public class VirtualRobot extends Robot {
      * VirtualRobot class
      * 
      * @param id      robot Id
-     * @param x       coordinate as double
-     * @param y       coordinate as double
-     * @param heading direction in degrees, as double
+     * @param x       X coordinate as double
+     * @param y       Y coordinate as double
+     * @param heading Heading direction in degrees, as double
      */
     public VirtualRobot(int id, double x, double y, double heading) {
         super(id, x, y, heading, 'V');
     }
 
     /**
-     * Handle the event loop of the robot
+     * Handles the event loop of the robot
      */
     @Override
     public void loop() throws Exception {
 
     }
 
     /**
-     * Handle sensorInterrupt triggers of the robot
+     * Handles sensorInterrupt triggers of the robot
      */
     @Override
     public void sensorInterrupt(String sensor, String value) {

src/main/java/swarm/robot/communication/DirectedCommunication.java

@@ -21,8 +21,8 @@ public DirectedCommunication(int robotId, RobotMqttClient m) {
     }
 
     private void subscribe(mqttTopic key, String topic) {
-        topicsSub.put(key, topic); // Put to the queue
-        robotMqttClient.subscribe(topic); // Subscribe through MqttHandler
+        topicsSub.put(key, topic);
+        robotMqttClient.subscribe(topic);
     }
 
     public void sendMessage(String msg) {

src/main/java/swarm/robot/communication/SimpleCommunication.java

@@ -21,8 +21,8 @@ public SimpleCommunication(int robotId, RobotMqttClient m) {
     }
 
     private void subscribe(mqttTopic key, String topic) {
-        topicsSub.put(key, topic); // Put to the queue
-        robotMqttClient.subscribe(topic); // Subscribe through MqttHandler
+        topicsSub.put(key, topic);
+        robotMqttClient.subscribe(topic);
     }
 
     public void sendMessage(String msg) {

src/main/java/swarm/robot/helpers/Coordinate.java

@@ -34,9 +34,9 @@ private enum mqttTopic {
      * Coordinate class
      * 
      * @param robotId
-     * @param x          coordinate as double
-     * @param y          coordinate as double
-     * @param heading    direction in degrees, as double
+     * @param x          X coordinate as double
+     * @param y          Y coordinate as double
+     * @param heading    Heading direction in degrees, as double
      * @param mqttClient mqttClient object
      */
     public Coordinate(int robotId, double x, double y, double heading, RobotMqttClient mqttClient) {
@@ -50,14 +50,14 @@ public Coordinate(int robotId, double x, double y, double heading, RobotMqttClie
     }
 
     /**
-     * subscribe to a MQTT topic
+     * Subscribe to a MQTT topic
      * 
      * @param key   Subscription topic key
      * @param topic Subscription topic value
      */
     private void subscribe(mqttTopic key, String topic) {
-        topicsSub.put(key, topic); // Put to the queue
-        robotMqttClient.subscribe(topic); // Subscribe through MqttHandler
+        topicsSub.put(key, topic);
+        robotMqttClient.subscribe(topic);
     }
 
     /**
@@ -153,8 +153,8 @@ public void setHeadingRad(double heading) {
     /**
      * Set robot's current (x,y) coordinate
      * 
-     * @param x coordinate as a double
-     * @param y coordinate as a double
+     * @param x X coordinate as a double
+     * @param y Y coordinate as a double
      */
     public void setCoordinate(double x, double y) {
         setX(x);
@@ -164,9 +164,9 @@ public void setCoordinate(double x, double y) {
     /**
      * Set robot's current (x,y) coordinate and heading direction
      * 
-     * @param x       coordinate as a double
-     * @param y       coordinate as a double
-     * @param heading in radians, [-PI, PI]
+     * @param x       X coordinate as a double
+     * @param y       Y coordinate as a double
+     * @param heading Heading in radians, [-PI, PI]
      */
     public void setCoordinate(double x, double y, double heading) {
         setCoordinate(x, y);
@@ -182,7 +182,7 @@ public void print() {
     }
 
     /**
-     * Returns robot's current (x,y,heading) values
+     * Returns robot's current (x,y,heading) values as a string
      * 
      * @return String
      */

src/main/java/swarm/robot/helpers/MotionController.java

@@ -4,27 +4,36 @@
 import swarm.robot.exception.MotionControllerException;
 
 /**
- * The class that calculate the robot motions
+ * The class that calculate the robot motions using mathematical models
  * 
  * @author Nuwan Jaliyagoda
  */
 public class MotionController {
 
     /**
-     * This is the maximum duration allowed to coordinate calculation, smaller
-     * values increase the smoothness of the movement, but more CPU time
+     * This is the maximum duration allowed to coordinate calculation, larger
+     * values increase the smoothness of the movement, but less CPU time
      */
     static final private int maxDuration = 100;
 
+    final int CM_2_MM = 10; // centimeters to milimeters
+    final int SEC_2_MS = 1000; // seconds to miliseconds
+
+    /*
+     * This will add necessary additional delays for the simulation time
+     */
+    final int ADDITIONAL_DELAY = 500;
+
     /* This is to be match with cm/s speed */
-    static final private double speedFactor = 0.1;
+    static final private double SPEED_FACTOR = 0.1;
 
     private final Coordinate c;
 
     /**
      * MotionController class
      * 
-     * @param c Coordinate object
+     * @param coordinate Coordinate object
+     * @see Coordinate
      */
     public MotionController(Coordinate coordinate) {
         this.c = coordinate;
@@ -82,7 +91,7 @@ public void rotate(int speed, double duration) {
     }
 
     /**
-     * Rotate to a given reltive angle in radians, with a given speed
+     * Rotate to a given relative angle in radians, with a given speed
      * 
      * @param speed  the value for speed, [ROBOT_SPEED_MIN, ROBOT_SPEED_MAX]
      * @param degree the relative degree to be rotate, positive means CW, [-180,
@@ -94,7 +103,7 @@ public void rotateRadians(int speed, float radians) {
     }
 
     /**
-     * Rotate to a given reltive angle in degrees, with a given speed
+     * Rotate to a given relative angle in degrees, with a given speed
      * 
      * @param speed  the value for speed, [ROBOT_SPEED_MIN, ROBOT_SPEED_MAX]
      * @param degree the relative degree to be rotate, positive means CW, [-180,
@@ -104,14 +113,14 @@ public void rotateRadians(int speed, float radians) {
     public void rotateDegree(int speed, float degree) {
         try {
             if (degree == 0 || degree < -180 || degree > 180)
-                throw new MotionControllerException("Degree should in range [-180, 180]");
+                throw new MotionControllerException("Degree should in range [-180, 180], except 0");
 
             if (speed < RobotSettings.ROBOT_SPEED_MIN)
                 throw new MotionControllerException("Speed should be greater than ROBOT_SPEED_MIN");
 
             int sign = (int) (degree / Math.abs(degree));
-            double distance = (2 * Math.PI * RobotSettings.ROBOT_RADIUS * (Math.abs(degree) / 360)) * 10;
-            float duration = (float) (distance / Math.abs(speed)) * 1000;
+            double distance = (2 * Math.PI * RobotSettings.ROBOT_RADIUS * (Math.abs(degree) / 360)) * CM_2_MM;
+            float duration = (float) (distance / Math.abs(speed)) * SEC_2_MS;
             debug("Sign: " + sign + " Distance: " + distance + " Duration: " + duration);
 
             rotate(sign * speed, duration);
@@ -136,21 +145,31 @@ public void moveDistance(int speed, int distance) {
      * Move the robot by given speed for a given distance
      * 
      * @param speed    the value for speed, [±ROBOT_SPEED_MIN, ±ROBOT_SPEED_MAX]
-     * @param distance the distance as double
+     * @param distance the distance as double, > 0
      * @throws MotionControllerException
      */
     public void moveDistance(int speed, double distance) {
-        // distance is relative displacement (in cm) from the current position
-        double duration = (distance * 10 / Math.abs(speed)) * 1000; // in ms
-        move(speed, speed, duration);
+        try {
+            if (speed <= RobotSettings.ROBOT_SPEED_MIN)
+                throw new MotionControllerException("Speed should be greater than ROBOT_SPEED_MIN");
+
+            if (distance <= 0)
+                throw new MotionControllerException("Distance should be greater than 0");
+
+            double duration = (distance * CM_2_MM / Math.abs(speed)) * SEC_2_MS;
+            move(speed, speed, duration);
+
+        } catch (MotionControllerException e) {
+            e.printStackTrace();
+        }
     }
 
     /**
-     * The core move method
+     * The core move implementation, using Dead Reckoning algorithm
      * 
-     * @param leftSpeed  left motor speed
-     * @param rightSpeed right motor speed
-     * @param duration   the duration, in ms
+     * @param leftSpeed  Left motor speed
+     * @param rightSpeed Right motor speed
+     * @param duration   Duration, in ms
      * @throws MotionControllerException
      */
     public void move(int leftSpeed, int rightSpeed, double duration) {
@@ -162,8 +181,8 @@ public void move(int leftSpeed, int rightSpeed, double duration) {
             debug("Calculate movement using " + steps + " steps, each has " + stepInterval + " intervals", 0);
 
             for (int j = 0; j < steps; j++) {
-                double dL = leftSpeed * speedFactor * (stepInterval / 1000.0);
-                double dR = rightSpeed * speedFactor * (stepInterval / 1000.0);
+                double dL = leftSpeed * SPEED_FACTOR * (stepInterval / 1000.0);
+                double dR = rightSpeed * SPEED_FACTOR * (stepInterval / 1000.0);
                 double d = (dL + dR) / 2.0;
                 double h = c.getHeadingRad();
 
@@ -174,11 +193,11 @@ public void move(int leftSpeed, int rightSpeed, double duration) {
                 c.setCoordinate(x, y, Math.toDegrees(heading));
 
                 cumulativeInterval += stepInterval;
-                if (cumulativeInterval >= 500) {
-                    debug("Adding extra delay of " + cumulativeInterval);
-                    delay(cumulativeInterval - 500);
+                if (cumulativeInterval >= ADDITIONAL_DELAY) {
+                    debug("Adding extra delay of " + ADDITIONAL_DELAY);
+                    delay(ADDITIONAL_DELAY);
                     c.publishCoordinate();
-                    cumulativeInterval -= 500;
+                    cumulativeInterval -= ADDITIONAL_DELAY;
                 }
             }
 
@@ -199,7 +218,7 @@ public void move(int leftSpeed, int rightSpeed, double duration) {
     /**
      * Validate the robot move speed
      * 
-     * @param speed speed, [ROBOT_SPEED_MIN, ROBOT_SPEED_MAX]
+     * @param speed Speed, [ROBOT_SPEED_MIN, ROBOT_SPEED_MAX]
      * @return True if the speed in the allowed range
      */
     private boolean isSpeedInRange(int speed) {
@@ -214,7 +233,7 @@ private boolean isSpeedInRange(int speed) {
     /**
      * Internal delay method
      * 
-     * @param duration duration in ms
+     * @param duration Duration in ms
      * @throws InterruptedException
      */
     private void delay(int duration) {
@@ -238,7 +257,7 @@ private static void debug(String msg) {
     /**
      * Debug support function
      * 
-     * @param msg   Debug Message
+     * @param msg   Debug message
      * @param level Debug level
      * @throws InterruptedException
      */