Implement randomized speed

2 files changed, 38 insertions, 6 deletions

diff --git a/src/Robot.java b/src/Robot.java
index b0bca1a..d050b6f 100644
--- a/src/Robot.java
+++ b/src/Robot.java
@@ -482,6 +482,7 @@ class Robot extends BetterBase {
      * Set the speed (meters per second) for this robot.
      */
     public void setSpeed(double speed) {
+        assert speed >= 0 : "Speed must be positive!";
         this.speed = speed;
     }
 
diff --git a/src/RobotRace.java b/src/RobotRace.java
index 622f82b..cc48694 100644
--- a/src/RobotRace.java
+++ b/src/RobotRace.java
@@ -17,6 +17,7 @@ import static java.lang.Math.*;
 import java.net.URI;
 import java.net.URISyntaxException;
 import java.util.Locale;
+import java.util.Random;
 
 /**
  * Handles all of the RobotRace graphics functionality,
@@ -136,6 +137,11 @@ public class RobotRace extends Base {
     private final static double TARGET_ROBOT_SPEED = .03;
 
     /**
+     * Used for randomizing robot speed.
+     */
+    private final Random random = new Random();
+
+    /**
      * Constructs this robot race by initializing robots, camera, track, and
      * terrain.
      */
@@ -382,16 +388,41 @@ public class RobotRace extends Base {
             }
         }
 
+        // periodically calculate a new speed
         double last_speed_update_t_diff = current_t - last_speed_update;
         if (last_speed_update_t_diff >= SPEED_RECALC_INTERVAL) {
+            // (expected) position
+            double avg_pos = current_t * TARGET_ROBOT_SPEED;
+
             for (Robot robot : robots) {
-                // smaller than 0 means that robot is faster than usual, equal
-                // to 0 means robot is matching expected and great means that
-                // we have a turtle.
-                double jitter = current_t - robot.getTimePos();
-                // TODO: do not use static speed
-                robot.setSpeed(TARGET_ROBOT_SPEED);
+                double old_speed = robot.getSpeed();
+                // initially the speed of robots is unknown, assume target speed
+                if (old_speed <= 0) {
+                    old_speed = TARGET_ROBOT_SPEED;
+                }
+                /* Speed is the sum of:
+                 * 20% previous speed
+                 * 30% minimum speed (100% target speed = 30%)
+                 * 50% random speed (max. 200% target speed = 100%, since it is
+                 * random, the expected random speed is 50%.)
+                 */
+                double speedup_factor = 2.0;
+                // if a robot is lagging behind too much, then give it a higher
+                // chance to walk faster. Similarly, if it is too fast, make
+                // it slow down a bit. The difference is at most 60% for the
+                // random speed (expected: 30%, final effect: 15%).
+                double pos_diff = (avg_pos - robot.getTimePos()) / TARGET_ROBOT_SPEED;
+                pos_diff = Math.max(-0.6, Math.min(pos_diff, 0.6));
+                speedup_factor += pos_diff;
+
+                double rnd = random.nextFloat();
+                double speed = .2 * old_speed;
+                speed += .3 * TARGET_ROBOT_SPEED;
+                speed += .5 * rnd * speedup_factor * TARGET_ROBOT_SPEED;
+                robot.setSpeed(speed);
             }
+            // DEBUG: average speed to robot 1 for showing expected position
+            //robots[0].setSpeed(TARGET_ROBOT_SPEED);
             last_speed_update = current_t;
         }
         double t_diff = current_t - last_t;