Better document SmarterWalkAnimation, remove DumbWalkAnimation

The arm rotation angles are now based on the feet position. Better
explain rationale instead of writing the implementation in comments.

2 files changed, 43 insertions, 91 deletions

diff --git a/src/DumbWalkAnimation.java b/src/DumbWalkAnimation.java
deleted file mode 100644
index 5dc2ac1..0000000
--- a/src/DumbWalkAnimation.java
+++ /dev/null
@@ -1,65 +0,0 @@
-
-/**
- * A WalkAnimation that does not result in a smooth transition. The formula and
- * constants are chosen at random.
- *
- * @author Peter Wu
- */
-public class DumbWalkAnimation implements WalkAnimation {
-
-    private double robot_pos_meters;
-    private final double legLength;
-
-    DumbWalkAnimation(float legTopLength, float legBottomLength) {
-        this.legLength = legTopLength + legBottomLength;
-    }
-
-    /**
-     * Sets the new position for the robot.
-     */
-    @Override
-    public void updatePosition(double pos) {
-        this.robot_pos_meters = pos;
-    }
-
-    private double getTime() {
-        return robot_pos_meters / 5;
-    }
-
-    @Override
-    public double getLegAngleLeft() {
-        return Math.sin(-getTime()) * 30.0;
-    }
-
-    @Override
-    public double getLegAngleRight() {
-        return -getLegAngleLeft();
-    }
-
-    @Override
-    public double getKneeAngleLeft() {
-        return 75.0 + Math.abs(Math.cos(getTime()) * 90.0);
-    }
-
-    @Override
-    public double getKneeAngleRight() {
-        return getKneeAngleLeft();
-    }
-
-    @Override
-    public double getArmAngleLeft() {
-        // static non-moving arms.
-        return 0;
-    }
-
-    @Override
-    public double getArmAngleRight() {
-        // static non-moving arms.
-        return 0;
-    }
-
-    @Override
-    public double getBottomOffset() {
-        return legLength;
-    }
-}
diff --git a/src/SmarterWalkAnimation.java b/src/SmarterWalkAnimation.java
index bf4e72e..64f3626 100644
--- a/src/SmarterWalkAnimation.java
+++ b/src/SmarterWalkAnimation.java
@@ -62,47 +62,64 @@ public class SmarterWalkAnimation implements WalkAnimation {
         double t = pos % 1.0;
         assert t >= 0.0 : "Time went negative?!";
 
-        /**
-         * At t=0, the body is at x=0; the right foot at x=-1/4 and the left
-         * foot at x=1/4. During the transition from t=0 to t=1/2, the right
-         * foot accelerates to x=3/4 while the left foot stays at x=-1/4
-         * (something has to support the robot while it is moving...).
-         */
+        // The animation is modeled with a transition from t=0 to t=2.
+        // (Actually, the period t=0 to t=1 and t=1 to t=2 have the same
+        // transitions, but with the Right foot and Left foot swapped.)
+        // Picture of half a cycle (t=0.5 to t=1.0 is similar, swapping L and R):
+        // t=  0    0.5
+        //     =     =
+        //    R L   L R
+        //   R   L L   R
+        //  =     =     =
+        //  A     B     C
+        //
+        // In this picture, the left foot stays at position B ("t=0.25") while
+        // the right foot moves from point A ("t=-1/4") to point C ("t=0.75").
+        // As you may have noticed, the body moves twice as slow as the foot.
+        // (hence body.x=t/2 below). The coordinates of body, foot_l and foot_r
+        // are in unit length (0.0 to 1.0). The body y position has yet to be
+        // determined, but assume that it is located near y=1.
         Point body = new Point(t / 2.0, 1.0);
-        Point foot_l = new Point(.25, 0.0);
-        Point foot_r = new Point(.25, 0.0);
-        double support_dist;
+        Point foot_l, foot_r;
+        Point supporting_foot = new Point(.25, 0.0);
+        Point moving_foot = new Point(-.25 + t,
+                                      FOOT_MAX_LIFT * sin(t * PI));
+        // Use Pythagoras to calculate the body bottom position from the
+        // leg length (1.0) and body to foot distance
+        body.y = sqrt(1.0 - pow(body.x - supporting_foot.x, 2));
+
         if (supported_by_left) {
-            foot_r.x = -.25 + t;
-            foot_r.y = FOOT_MAX_LIFT * sin(t * PI);
-            support_dist = foot_r.x - body.x;
+            // supported by left, so the right foot is moving.
+            foot_l = supporting_foot;
+            foot_r = moving_foot;
         } else {
-            foot_l.x = -.25 + t;
-            foot_l.y = FOOT_MAX_LIFT * sin(t * PI);
-            support_dist = foot_l.x - body.x;
+            foot_r = supporting_foot;
+            foot_l = moving_foot;
         }
-        // base the robot body bottom position on the foot that is supporting
-        // the robot.
-        body.y = sqrt(1.0 - support_dist * support_dist);
 
         // distance between a bent leg and the body
         double dist_leg_r, dist_leg_l;
         dist_leg_l = legLength * distance(body, foot_l);
         dist_leg_r = legLength * distance(body, foot_r);
 
+        double foot_angle_right, foot_angle_left;
         // base rotation for legs
-        leg_angle_right = calcAngle(body, foot_r);
-        leg_angle_left = calcAngle(body, foot_l);
+        foot_angle_right = calcAngle(body, foot_r);
+        foot_angle_left = calcAngle(body, foot_l);
         // bend the knees
-
         knee_angle_right = cosineRule(legTopLength, legBottomLength, dist_leg_r);
         knee_angle_left = cosineRule(legTopLength, legBottomLength, dist_leg_l);
-        // correct the foot position due to the bent leg
+
+        // calculate leg rotations (and consider the bent knee to fit the foot
+        // above the floor)
+        leg_angle_left = foot_angle_left;
+        leg_angle_right = foot_angle_right;
         leg_angle_left += cosineRule(legTopLength, dist_leg_l, legBottomLength);
         leg_angle_right += cosineRule(legTopLength, dist_leg_r, legBottomLength);
 
-        arm_angle_left = MAX_ARM_ANGLE_DEG * cos(pos / 2 * PI);
-        arm_angle_right = MAX_ARM_ANGLE_DEG * sin(pos / 2 * PI);
+        // to balance the robot, rotate the arms in the opposite direction.
+        arm_angle_left = -foot_angle_left;
+        arm_angle_right = -foot_angle_right;
 
         bodyOffset = legLength * body.y;
     }
@@ -129,12 +146,12 @@ public class SmarterWalkAnimation implements WalkAnimation {
 
     @Override
     public double getArmAngleLeft() {
-        return arm_angle_left;
+        return arm_angle_left * 180 / PI;
     }
 
     @Override
     public double getArmAngleRight() {
-        return arm_angle_right;
+        return arm_angle_right * 180 / PI;
     }
 
     @Override