Diff: main.cpp

Revision:

8:db453051f3f4

Parent:

7:aac5f901bd76

Child:

9:a6d1502f0f20

--- a/main.cpp	Sun Jan 13 20:43:28 2013 +0000
+++ b/main.cpp	Thu Jan 17 18:34:09 2013 +0000
@@ -9,7 +9,15 @@
 
 #define MAXSPEED 0.1f
 #define MAXTURN 1.0f
-#define STEPTIME 0.4f
+#define RESET_STEP_TIME 0.4f
+#define DIM_A 0.125f
+#define DIM_B 0.11f
+#define DIM_C 0.0025f
+#define DIM_D 0.025f
+#define CIRCLE_X 0.095f
+#define CIRCLE_Y 0.095f
+#define CIRCLE_Z -0.12f
+#define CIRCLE_R 0.09f
 
 enum state_t
 {
@@ -27,14 +35,16 @@
 };
 
 CircularBuffer<float,16> dataLog;
-RobotLeg legA(p26, p29, p30);
-RobotLeg legB(p13, p14, p15);
-RobotLeg legC(p12, p11, p8);
-RobotLeg legD(p23, p24, p25);
 Radio radio(p5, p6, p7, p19, p20, p17);
-Timer cycleTimer;
+Timer deltaTimer;
+Timer RESET_STEP_TIMEr;
+RobotLeg legA(p26, p29, p30, false); // Start the legs disabled
+RobotLeg legB(p13, p14, p15, false);
+RobotLeg legC(p12, p11, p8, false);
+RobotLeg legD(p23, p24, p25, false);
 state_t state;
 legstate_t legState;
+float RESET_STEP_TIMEThreshold;
 
 
 
@@ -61,7 +71,7 @@
     }  
     
     return NULL;
-}
+} // log()
 
 
 
@@ -80,7 +90,7 @@
     terminal->write(output);
 
     return NULL;
-}
+} // read()
 
 
 
@@ -89,7 +99,7 @@
     if (input > zone) return input;
     else if (input < -zone) return input;
     else return 0;
-}
+} // deadzone()
 
 
 
@@ -99,17 +109,32 @@
     legB.reset(0.0f);
     legC.reset(0.5f);
     legD.reset(1.0f);
-    cycleTimer.start();
+    RESET_STEP_TIMEr.start();
     state = reset;
     legState = D;
+} // resetLegs()
+
+
+
+void walkLegs()
+{
+    float xaxis, yaxis, speed;
+    
+    state = walk;
+    legState = A;
+    RESET_STEP_TIMEr.reset();
+    xaxis = 0.0078125f * deadzone((int8_t)((radio.rx_controller>>0)&0xff), 8);
+    yaxis = -0.0078125f * deadzone((int8_t)((radio.rx_controller>>8)&0xff), 8);
+    speed = sqrt(xaxis*xaxis + yaxis*yaxis) * MAXSPEED * deltaTimer.read();
+    RESET_STEP_TIMEThreshold = CIRCLE_R / 2.0f / speed;
 }
 
 
 
 int main()
 {
-    Timer deltaTimer;
     float xaxis, yaxis, turnaxis, speed, angle;
+    float deltaTime, cycleTime;
     vector3 v;
     matrix4 T;
     matrix4 PA, QA;
@@ -125,18 +150,18 @@
     radio.reset();
     
     // Set leg parameters
-    legA.setDimensions(0.125f, 0.11f, 0.0025f, 0.025f);
-    legB.setDimensions(0.125f, 0.11f, 0.0025f, 0.025f);
-    legC.setDimensions(0.125f, 0.11f, 0.0025f, 0.025f);
-    legD.setDimensions(0.125f, 0.11f, 0.0025f, 0.025f);
+    legA.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D);
+    legB.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D);
+    legC.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D);
+    legD.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D);
     legA.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
     legB.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
     legC.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
     legD.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
-    legA.setStepCircle(0.095f, 0.095f, -0.12f, 0.09f);
-    legB.setStepCircle(0.095f, 0.095f, -0.12f, 0.09f);
-    legC.setStepCircle(0.095f, 0.095f, -0.12f, 0.09f);
-    legD.setStepCircle(0.095f, 0.095f, -0.12f, 0.09f);
+    legA.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R);
+    legB.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R);
+    legC.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R);
+    legD.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R);
     legA.theta.calibrate(1000, 2000, 45.0f, -45.0f);
     legA.phi.calibrate(1000, 2000, 70.0f, -45.0f);
     legA.psi.calibrate(2000, 1000, 70.0f, -60.0f);
@@ -168,41 +193,63 @@
     QD.translate(vector3(-0.1f, 0.1f, 0.0f));
     QD.a22 = -1.0f;
     PD = QD.inverse();
-
+    
+    // Start timers
+    deltaTimer.start();
+    RESET_STEP_TIMEr.start();
+    
     // Go to initial position
+    legA.move(vector3(0.2f, 0.2f, 0.05f));
+    legB.move(vector3(0.2f, 0.2f, 0.05f));
+    legC.move(vector3(0.2f, 0.2f, 0.05f));
+    legD.move(vector3(0.2f, 0.2f, 0.05f));
+    legA.theta.enable(); wait(0.1f);
+    legB.theta.enable(); wait(0.1f);
+    legC.theta.enable(); wait(0.1f);
+    legD.theta.enable(); wait(0.1f);
+    legA.phi.enable(); wait(0.1f);
+    legB.phi.enable(); wait(0.1f);
+    legC.phi.enable(); wait(0.1f);
+    legD.phi.enable(); wait(0.1f);
+    legA.psi.enable(); wait(0.1f);
+    legB.psi.enable(); wait(0.1f);
+    legC.psi.enable(); wait(0.1f);
+    legD.psi.enable(); wait(0.1f);
     resetLegs();
     
-    deltaTimer.start();
-    
     /*
     // Dump debug info 
     sprintf(output, "T =\t%f\t%f\t%f\t%f\n\t%f\t%f\t%f\t%f\n\t%f\t%f\t%f\t%f\n\t0\t\t0\t\t0\t\t1\n",
             T.a11, T.a12, T.a13, T.a14,
             T.a21, T.a22, T.a23, T.a24,
             T.a31, T.a32, T.a33, T.a34);
-    terminal.write(output); */
+    terminal.write(output); 
+    */
     
     
     while(true)
     {
+        dataLog.push((float)state * 10 + legState);
+    
         switch (state)
         {
         case walk:
         case step:
+            deltaTime = deltaTimer.read();
+            deltaTimer.reset();
+        
             // Read controller input
             xaxis = 0.0078125f * deadzone((int8_t)((radio.rx_controller>>0)&0xff), 8); // Convert to +/-1.0f range
             yaxis = -0.0078125f * deadzone((int8_t)((radio.rx_controller>>8)&0xff), 8);
             turnaxis = -0.0078125f * deadzone((int8_t)((radio.rx_controller>>16)&0xff), 8);
             
             // Compute delta movement vector and delta angle
-            v.x = xaxis;
-            v.y = yaxis;
+            v.x = -xaxis;
+            v.y = -yaxis;
             v.z = 0;
-            v = v * MAXSPEED * deltaTimer.read();
+            v = v * MAXSPEED * deltaTime;
             speed = sqrt(v.x*v.x + v.y*v.y);
-            angle = turnaxis * MAXTURN * deltaTimer.read();
-            dataLog.push(turnaxis);
-            deltaTimer.reset();
+            angle = -turnaxis * MAXTURN * deltaTime;
             
             // Compute movement transformation in robot coordinates
             T.identity().rotateZ(angle).translate(v).inverse();
@@ -218,38 +265,45 @@
             {
             case A:
                 if (!freeB || !freeC || !freeD) resetLegs();
-                else if (!freeA || cycleTimer.read() > 0.055f / speed) // 0.055/speed is 1/4 the gait period (0.22m/speed/4)
+                else if (!freeA || RESET_STEP_TIMEr.read() > RESET_STEP_TIMEThreshold)
                 {
                     legA.reset(1.0f);
                     legState = B;
+                    RESET_STEP_TIMEThreshold = CIRCLE_R / speed - RESET_STEP_TIMEr.read();
+                    RESET_STEP_TIMEr.reset();
                 }
                 break;
             
             case B:
                 if (!freeA || !freeC || !freeD) resetLegs();
-                else if (!freeB || cycleTimer.read() > 0.11f / speed)
+                else if (!freeB || RESET_STEP_TIMEr.read() > RESET_STEP_TIMEThreshold)
                 {
                     legB.reset(1.0f);
                     legState = C;
+                    RESET_STEP_TIMEThreshold = CIRCLE_R / speed - RESET_STEP_TIMEr.read();
+                    RESET_STEP_TIMEr.reset();
                 }
                 break;
             
             case C:
                 if (!freeA || !freeB || !freeD) resetLegs();
-                else if (!freeC || cycleTimer.read() > 0.165f / speed)
+                else if (!freeC || RESET_STEP_TIMEr.read() > RESET_STEP_TIMEThreshold)
                 {
                     legC.reset(1.0f);
                     legState = D;
+                    RESET_STEP_TIMEThreshold = CIRCLE_R / speed - RESET_STEP_TIMEr.read();
+                    RESET_STEP_TIMEr.reset();
                 }
                 break;
             
             case D:
                 if (!freeA || !freeB || !freeC) resetLegs();
-                else if (!freeD || cycleTimer.read() > 0.22f / speed)
+                else if (!freeD || RESET_STEP_TIMEr.read() > RESET_STEP_TIMEThreshold)
                 {
                     legD.reset(1.0f);
                     legState = A;
-                    cycleTimer.reset();
+                    RESET_STEP_TIMEThreshold = CIRCLE_R / speed - RESET_STEP_TIMEr.read();
+                    RESET_STEP_TIMEr.reset();
                 }
                 break;
             }
@@ -257,36 +311,39 @@
             break; // case walk:, case step:
             
         case reset:
-            if (cycleTimer.read() < STEPTIME && legState != A)
+            cycleTime = RESET_STEP_TIMEr.read();
+        
+            if ((cycleTime <= RESET_STEP_TIME) && legState != A)
             {
                 legA.reset(-0.5f);
                 legState = A;
             }
-            else if (cycleTimer.read() < STEPTIME * 2 && legState == A)
+            else if ((cycleTime > RESET_STEP_TIME) && legState == A)
             {
                 legB.reset(0.0f);
                 legState = B;
             }
-            else if (cycleTimer.read() < STEPTIME * 3 && legState == B)
+            else if ((cycleTime > RESET_STEP_TIME * 2) && legState == B)
             {
                 legC.reset(0.5f);
                 legState = C;
             }
-            else if (cycleTimer.read() < STEPTIME * 4 && legState == C)
+            else if ((cycleTime > RESET_STEP_TIME * 3) && legState == C)
             {
                 legD.reset(1.0f);
                 legState = D;
             }
-            else if (cycleTimer.read() >= STEPTIME * 4 && legState == D)
+            else if ((cycleTime > RESET_STEP_TIME * 4) && legState == D)
             {
-                state = walk;
-                legState = A;
-                cycleTimer.reset();
+                walkLegs();
             }
-            else
-            {
-                resetLegs();
-            }
+            
+            T.identity();
+            
+            freeA = legA.update(T);
+            freeB = legB.update(T);
+            freeC = legC.update(T);
+            freeD = legD.update(T);
             
             break; // case reset:
         } // switch (state)