Diff: RobotLeg.cpp

Revision:

17:4ec59e8b52a6

Parent:

13:1c5d255835ce

Child:

18:8806d24809c2

--- a/RobotLeg.cpp	Mon May 27 00:42:22 2013 +0000
+++ b/RobotLeg.cpp	Mon May 27 03:31:58 2013 +0000
@@ -1,4 +1,5 @@
 #include "RobotLeg.h"
+#include "utility.h"
 
 
 
@@ -11,8 +12,6 @@
     stepTime = 0.4f;
     stepDelta = 3.141593f / stepTime;
     stepHeight = 0.05f;
-
-    position = getPosition();
 }
 
 
@@ -46,7 +45,9 @@
 
 vector3 RobotLeg::getPosition()
 {
-    const float deg2rad = 0.01745329f;
+    return position;
+    
+    /*const float deg2rad = 0.01745329f;
     vector3 p;
     float L, thetaR, phiR, psiR;
     
@@ -61,7 +62,23 @@
     p.y = c*cos(thetaR + oth) + (L + d)*sin(thetaR + oth);
     p.z = a*sin(phiR + oph) - b*cos(phiR + oph + psiR + ops);
     
-    return p;
+    return p;*/
+}
+
+
+
+float RobotLeg::getStepDistance()
+{
+    // Returns distance to step circle edge in the current direction of movement.
+    float vx, vy, m, cosval;
+    
+    vx = position.x - circleCenter.x;
+    vy = position.y - circleCenter.y;
+    m = sqrt(vx*vx + vy*vy);
+    cosval = (nDeltaPosition.x*vx + nDeltaPosition.y*vy) / 
+        (m * sqrt(nDeltaPosition.x*nDeltaPosition.x + nDeltaPosition.y*nDeltaPosition.y));
+    
+    return m*cosval + sqrt(pos(circleRadius*circleRadius - m*m*(1.0f - cosval*cosval)));
 }
 
 
@@ -72,6 +89,8 @@
     const float rad2deg =  57.2958;
     float L;
     
+    position = dest;
+    
     // Calculate new angles
     L = sqrt(dest.x*dest.x + dest.y*dest.y - c*c) - d;
     thetaAngle = atan2( ((L + d)*dest.y - c*dest.x), ((L + d)*dest.x + c*dest.y) ) - oth;
@@ -105,7 +124,6 @@
 
 void RobotLeg::step(vector3 dest)
 {
-    position = getPosition();
     stepA = position;
     stepB = dest;
 
@@ -165,7 +183,6 @@
             move(stepB);
             state = neutral;
             stepTimer.stop();
-            position = getPosition();
             newPosition = position;
         }
         
@@ -182,8 +199,7 @@
 {
     if (neutral == state)
     {
-        position = newPosition;
-        move(position);
+        move(newPosition);
     }
 }