Diff: main.cpp

Revision:

10:dc1ba352667e

Parent:

9:a6d1502f0f20

Child:

11:9ee0214bd410

--- a/main.cpp	Thu Jan 31 23:51:15 2013 +0000
+++ b/main.cpp	Tue Apr 09 01:36:50 2013 +0000
@@ -43,7 +43,6 @@
 RobotLeg legD(p23, p24, p25, false);
 state_t state;
 legstate_t legState;
-float stepDistance, stepDistanceTarget;
 
 
 
@@ -154,8 +153,6 @@
 {
     state = walk;
     legState = A;
-    stepDistanceTarget = CIRCLE_R / 2.0f;
-    stepDistance = 0;
 } // walkLegs()
 
 
@@ -163,7 +160,7 @@
 int main()
 {
     Timer deltaTimer;
-    float xaxis, yaxis, turnaxis, speed, angle;
+    float xaxis, yaxis, turnaxis, angle;
     float deltaTime, cycleTime;
     vector3 v;
     matrix4 T;
@@ -172,7 +169,6 @@
     matrix4 PC, QC; 
     matrix4 PD, QD;
     Terminal terminal;
-    bool freeA, freeB, freeC, freeD;
     
     terminal.addCommand("log", &log);
     terminal.addCommand("read", &read);
@@ -291,74 +287,39 @@
             turnaxis = -0.0078125f * deadzone((int8_t)((radio.rx_controller>>16)&0xff), 8);
             
             // Compute delta movement vector and delta angle
-            speed = sqrt(xaxis*xaxis + yaxis*yaxis) * MAXSPEED;
             v.x = -xaxis;
             v.y = -yaxis;
             v.z = 0;
             v = v * MAXSPEED * deltaTime;
             angle = -turnaxis * MAXTURN * deltaTime;
-            stepDistance += deltaTime * (speed + fabs(angle * 0.141f));
             
             // Compute movement transformation in robot coordinates
             T.identity().rotateZ(angle).translate(v).inverse();
             
             // Update legs
-            freeA = legA.update(PA*T*QA);
-            freeB = legB.update(PB*T*QB);
-            freeC = legC.update(PC*T*QC);
-            freeD = legD.update(PD*T*QD);
-            
-            // Update state
-            switch (legState)
+            if (!legA.update(PA*T*QA))
             {
-            case A:
-                if (!freeB || !freeC || !freeD) resetLegs();
-                else if (!freeA || stepDistance > stepDistanceTarget)
-                {
-                    legA.reset(1.0f);
-                    legState = B;
-                    stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance;
-                    stepDistance = 0;
-                }
-                break;
+                legA.reset(1.0f);
+                legState = A;
+            }
             
-            case B:
-                if (!freeA || !freeC || !freeD) resetLegs();
-                else if (!freeB || stepDistance > stepDistanceTarget)
-                {
-                    legB.reset(1.0f);
-                    legState = C;
-                    stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance;
-                    stepDistance = 0;
-                }
-                break;
-            
-            case C:
-                if (!freeA || !freeB || !freeD) resetLegs();
-                else if (!freeC || stepDistance > stepDistanceTarget)
-                {
-                    legC.reset(1.0f);
-                    legState = D;
-                    stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance;
-                    stepDistance = 0;
-                }
-                break;
-            
-            case D:
-                if (!freeA || !freeB || !freeC) resetLegs();
-                else if (!freeD || stepDistance > stepDistanceTarget)
-                {
-                    legD.reset(1.0f);
-                    legState = A;
-                    stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance;
-                    stepDistance = 0;
-                }
-                break;
+            if (!legB.update(PB*T*QB))
+            {
+                legB.reset(1.0f);
+                legState = B;
             }
             
-            dataLog.push(stepDistanceTarget);
+            if (!legC.update(PC*T*QC))
+            {
+                legC.reset(1.0f);
+                legState = C;
+            }
             
-            
+            if (!legD.update(PD*T*QD))
+            {
+                legD.reset(1.0f);
+                legState = D;
+            }
             
             break; // case walk:
             
@@ -403,10 +364,10 @@
             
             T.identity();
             
-            freeA = legA.update(T);
-            freeB = legB.update(T);
-            freeC = legC.update(T);
-            freeD = legD.update(T);
+            legA.update(T);
+            legB.update(T);
+            legC.update(T);
+            legD.update(T);
             
             break; // case reset:
         } // switch (state)