Diff: RobotLeg.cpp

Revision:

19:efba54b23912

Parent:

18:8806d24809c2

Child:

20:bf46c0400b10

--- a/RobotLeg.cpp	Mon May 27 20:08:03 2013 +0000
+++ b/RobotLeg.cpp	Mon May 27 23:20:14 2013 +0000
@@ -170,37 +170,32 @@
     case stepping:
         // Compute new position along step trajectory
         t = stepTimer.read();
-        newPosition.x = stepA.x + (stepB.x - stepA.x)*0.5f*(1 - cos(stepDelta*t));
-        newPosition.y = stepA.y + (stepB.y - stepA.y)*0.5f*(1 - cos(stepDelta*t));
-        newPosition.z = stepA.z + (stepB.z - stepA.z)*stepDelta*t + stepHeight*sin(stepDelta*t);
         
         if (t < stepTime)
         {
-            move(newPosition);
+            newPosition.x = stepA.x + (stepB.x - stepA.x)*0.5f*(1 - cos(stepDelta*t));
+            newPosition.y = stepA.y + (stepB.y - stepA.y)*0.5f*(1 - cos(stepDelta*t));
+            newPosition.z = stepA.z + (stepB.z - stepA.z)*stepDelta*t + stepHeight*sin(stepDelta*t);
         }
         else
         {
-            move(stepB);
+            newPosition = stepB;
             state = neutral;
             stepTimer.stop();
-            newPosition = position;
         }
         
         return true;
+        
+    default:
+        return false;
     }
-    
-    // Should never reach here
-    return false;
 }
 
 
 
 void RobotLeg::apply()
 {
-    if (neutral == state)
-    {
-        move(newPosition);
-    }
+    move(newPosition);
 }