Patrick Clary / Mbed 2 deprecated WalkingRobot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   CircularBuffer Servo Terminal mbed Radio

Diff: RobotLeg.cpp

Revision:
9:a6d1502f0f20
Parent:
8:db453051f3f4
Child:
10:dc1ba352667e
--- a/RobotLeg.cpp	Thu Jan 17 18:34:09 2013 +0000
+++ b/RobotLeg.cpp	Thu Jan 31 23:51:15 2013 +0000
@@ -116,11 +116,12 @@
 
 
 
-void RobotLeg::reset(float f)
+vector3 RobotLeg::reset(float f)
 {
     vector3 newPosition;
     newPosition = circleCenter + nDeltaPosition.unit() * circleRadius * f;
     step(newPosition);
+    return nDeltaPosition;
 }
 
 
@@ -128,13 +129,16 @@
 bool RobotLeg::update(const matrix4& deltaTransform)
 {
     float t;
-    vector3 newPosition;
+    vector3 newPosition, newNDeltaPosition;
+    const float eps = 0.000001f;
 
     switch (state)
     {
     case neutral:
         newPosition = deltaTransform*position;
-        nDeltaPosition = position - newPosition;
+        newNDeltaPosition = position - newPosition;
+        if (fabs(newNDeltaPosition.x) > eps || fabs(newNDeltaPosition.y) > eps || fabs(newNDeltaPosition.z) > eps)
+            nDeltaPosition = newNDeltaPosition;
         position = newPosition;
         if (!move(position))
         {