Diff: main.cpp

Revision:

9:a6d1502f0f20

Parent:

8:db453051f3f4

Child:

10:dc1ba352667e

--- a/main.cpp	Thu Jan 17 18:34:09 2013 +0000
+++ b/main.cpp	Thu Jan 31 23:51:15 2013 +0000
@@ -4,6 +4,7 @@
 #include "CircularBuffer.h"
 #include "Radio.h"
 #include "Terminal.h"
+#include "utility.h"
 #include <cstring>
 #include <cmath>
 
@@ -22,7 +23,6 @@
 enum state_t
 {
     walk,
-    step,
     reset
 };
 
@@ -35,16 +35,15 @@
 };
 
 CircularBuffer<float,16> dataLog;
-Radio radio(p5, p6, p7, p19, p20, p17);
-Timer deltaTimer;
-Timer RESET_STEP_TIMEr;
+Radio radio(p5, p6, p7, p16, p17, p18);
+Timer stepTimer;
 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;
+float stepDistance, stepDistanceTarget;
 
 
 
@@ -94,6 +93,41 @@
 
 
 
+CmdHandler* resetc(Terminal* terminal, const char* input)
+{
+    char output[256];
+    float f;
+    vector3 v;
+    
+    if (sscanf(input, "reset %f", &f) == 1)
+    {
+        v = legA.reset(f);
+        sprintf(output, "reset: %f %f %f", v.x, v.y, v.z);
+        terminal->write(output);
+    }
+    else
+    {
+        sprintf(output, "syntax error");
+        terminal->write(output);
+    } 
+
+    return NULL;
+} // reset()
+
+
+
+CmdHandler* rndp(Terminal* terminal, const char* input)
+{
+    char output[256];
+    
+    sprintf(output, "%f %f %f", legA.nDeltaPosition.x, legA.nDeltaPosition.y, legA.nDeltaPosition.z);
+    terminal->write(output);
+
+    return NULL;
+} // step()
+
+
+
 int deadzone(int input, int zone)
 {
     if (input > zone) return input;
@@ -109,7 +143,7 @@
     legB.reset(0.0f);
     legC.reset(0.5f);
     legD.reset(1.0f);
-    RESET_STEP_TIMEr.start();
+    stepTimer.reset();
     state = reset;
     legState = D;
 } // resetLegs()
@@ -118,21 +152,17 @@
 
 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;
-}
+    stepDistanceTarget = CIRCLE_R / 2.0f;
+    stepDistance = 0;
+} // walkLegs()
 
 
 
 int main()
 {
+    Timer deltaTimer;
     float xaxis, yaxis, turnaxis, speed, angle;
     float deltaTime, cycleTime;
     vector3 v;
@@ -146,6 +176,15 @@
     
     terminal.addCommand("log", &log);
     terminal.addCommand("read", &read);
+    terminal.addCommand("reset", &resetc);
+    terminal.addCommand("ndp", &rndp);
+    
+    
+    DigitalOut debug1(LED1);
+    DigitalOut debug2(LED2);
+    DigitalOut debug3(LED3);
+    DigitalOut debug4(LED4);
+    
     
     radio.reset();
     
@@ -196,13 +235,13 @@
     
     // Start timers
     deltaTimer.start();
-    RESET_STEP_TIMEr.start();
+    stepTimer.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.move(vector3(0.15f, 0.15f, 0.05f));
+    legB.move(vector3(0.15f, 0.15f, 0.05f));
+    legC.move(vector3(0.15f, 0.15f, 0.05f));
+    legD.move(vector3(0.15f, 0.15f, 0.05f));
     legA.theta.enable(); wait(0.1f);
     legB.theta.enable(); wait(0.1f);
     legC.theta.enable(); wait(0.1f);
@@ -215,6 +254,7 @@
     legB.psi.enable(); wait(0.1f);
     legC.psi.enable(); wait(0.1f);
     legD.psi.enable(); wait(0.1f);
+    wait(0.4f);
     resetLegs();
     
     /*
@@ -229,12 +269,19 @@
     
     while(true)
     {
-        dataLog.push((float)state * 10 + legState);
-    
         switch (state)
         {
         case walk:
-        case step:
+            // Debug stuff
+            switch (legState)
+            {
+            case A: debug1 = 1; debug2 = 0; debug3 = 0; debug4 = 0; break;
+            case B: debug1 = 0; debug2 = 1; debug3 = 0; debug4 = 0; break;
+            case C: debug1 = 0; debug2 = 0; debug3 = 1; debug4 = 0; break;
+            case D: debug1 = 0; debug2 = 0; debug3 = 0; debug4 = 1; break;
+            }
+            
+            // Get delta-time
             deltaTime = deltaTimer.read();
             deltaTimer.reset();
         
@@ -244,12 +291,13 @@
             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;
-            speed = sqrt(v.x*v.x + v.y*v.y);
             angle = -turnaxis * MAXTURN * deltaTime;
+            stepDistance += deltaTime * (speed + fabs(angle * 0.141f));
             
             // Compute movement transformation in robot coordinates
             T.identity().rotateZ(angle).translate(v).inverse();
@@ -265,53 +313,68 @@
             {
             case A:
                 if (!freeB || !freeC || !freeD) resetLegs();
-                else if (!freeA || RESET_STEP_TIMEr.read() > RESET_STEP_TIMEThreshold)
+                else if (!freeA || stepDistance > stepDistanceTarget)
                 {
                     legA.reset(1.0f);
                     legState = B;
-                    RESET_STEP_TIMEThreshold = CIRCLE_R / speed - RESET_STEP_TIMEr.read();
-                    RESET_STEP_TIMEr.reset();
+                    stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance;
+                    stepDistance = 0;
                 }
                 break;
             
             case B:
                 if (!freeA || !freeC || !freeD) resetLegs();
-                else if (!freeB || RESET_STEP_TIMEr.read() > RESET_STEP_TIMEThreshold)
+                else if (!freeB || stepDistance > stepDistanceTarget)
                 {
                     legB.reset(1.0f);
                     legState = C;
-                    RESET_STEP_TIMEThreshold = CIRCLE_R / speed - RESET_STEP_TIMEr.read();
-                    RESET_STEP_TIMEr.reset();
+                    stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance;
+                    stepDistance = 0;
                 }
                 break;
             
             case C:
                 if (!freeA || !freeB || !freeD) resetLegs();
-                else if (!freeC || RESET_STEP_TIMEr.read() > RESET_STEP_TIMEThreshold)
+                else if (!freeC || stepDistance > stepDistanceTarget)
                 {
                     legC.reset(1.0f);
                     legState = D;
-                    RESET_STEP_TIMEThreshold = CIRCLE_R / speed - RESET_STEP_TIMEr.read();
-                    RESET_STEP_TIMEr.reset();
+                    stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance;
+                    stepDistance = 0;
                 }
                 break;
             
             case D:
                 if (!freeA || !freeB || !freeC) resetLegs();
-                else if (!freeD || RESET_STEP_TIMEr.read() > RESET_STEP_TIMEThreshold)
+                else if (!freeD || stepDistance > stepDistanceTarget)
                 {
                     legD.reset(1.0f);
                     legState = A;
-                    RESET_STEP_TIMEThreshold = CIRCLE_R / speed - RESET_STEP_TIMEr.read();
-                    RESET_STEP_TIMEr.reset();
+                    stepDistanceTarget += CIRCLE_R / 2.0f - stepDistance;
+                    stepDistance = 0;
                 }
                 break;
             }
             
-            break; // case walk:, case step:
+            dataLog.push(stepDistanceTarget);
+            
+            
+            
+            break; // case walk:
+            
+            
             
         case reset:
-            cycleTime = RESET_STEP_TIMEr.read();
+            // Debug stuff
+            switch (legState)
+            {
+            case A: debug1 = 0; debug2 = 1; debug3 = 1; debug4 = 1; break;
+            case B: debug1 = 1; debug2 = 0; debug3 = 1; debug4 = 1; break;
+            case C: debug1 = 1; debug2 = 1; debug3 = 0; debug4 = 1; break;
+            case D: debug1 = 1; debug2 = 1; debug3 = 1; debug4 = 0; break;
+            }
+        
+            cycleTime = stepTimer.read();
         
             if ((cycleTime <= RESET_STEP_TIME) && legState != A)
             {