Diff: main.cpp

Revision:

11:9ee0214bd410

Parent:

10:dc1ba352667e

Child:

12:a952bd74d363

diff -r dc1ba352667e -r 9ee0214bd410 main.cpp
--- a/main.cpp	Tue Apr 09 01:36:50 2013 +0000
+++ b/main.cpp	Wed May 15 17:22:22 2013 +0000
@@ -19,30 +19,16 @@
 #define CIRCLE_Y 0.095f
 #define CIRCLE_Z -0.12f
 #define CIRCLE_R 0.09f
-
-enum state_t
-{
-    walk,
-    reset
-};
+#define PERIOD 0.002f
 
-enum legstate_t
-{
-    A,
-    B,
-    C,
-    D
-};
+
 
 CircularBuffer<float,16> dataLog;
 Radio radio(p5, p6, p7, p16, p17, p18);
-Timer stepTimer;
-RobotLeg legA(p26, p29, p30, false); // Start the legs disabled
+RobotLeg legA(p26, p29, p30, false);
 RobotLeg legB(p13, p14, p15, false);
-RobotLeg legC(p12, p11, p8, false);
-RobotLeg legD(p23, p24, p25, false);
-state_t state;
-legstate_t legState;
+RobotLeg legC(p19, p11, p8, false);
+RobotLeg legD(p25, p24, p23, false);
 
 
 
@@ -73,60 +59,6 @@
 
 
 
-CmdHandler* read(Terminal* terminal, const char* input)
-{
-    char output[256];
-    uint32_t data;
-
-    data = radio.rx_controller;
-    sprintf(output, "%d%d%d%d%d%d%d%d %d%d%d%d%d%d%d%d %d%d%d%d%d%d%d%d %d%d%d%d%d%d%d%d : %4d %4d %4d %4d", 
-    (data>>31)&1, (data>>30)&1, (data>>29)&1, (data>>28)&1, (data>>27)&1, (data>>26)&1, (data>>25)&1, (data>>24)&1, 
-    (data>>23)&1, (data>>22)&1, (data>>21)&1, (data>>20)&1, (data>>19)&1, (data>>18)&1, (data>>17)&1, (data>>16)&1, 
-    (data>>15)&1, (data>>14)&1, (data>>13)&1, (data>>12)&1, (data>>11)&1, (data>>10)&1, (data>>9)&1, (data>>8)&1, 
-    (data>>7)&1, (data>>6)&1, (data>>5)&1, (data>>4)&1, (data>>3)&1, (data>>2)&1, (data>>1)&1, (data>>0)&1, 
-    (int8_t)((data>>24)&0xff), (int8_t)((data>>16)&0xff), (int8_t)((data>>8)&0xff), (int8_t)((data)&0xff));
-    terminal->write(output);
-
-    return NULL;
-} // read()
-
-
-
-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;
@@ -138,22 +70,15 @@
 
 void resetLegs()
 {
-    legA.reset(-0.5f);
-    legB.reset(0.0f);
-    legC.reset(0.5f);
-    legD.reset(1.0f);
-    stepTimer.reset();
-    state = reset;
-    legState = D;
+    legA.reset(-0.6f);
+    legB.reset(-0.1f);
+    legC.reset(0.4f);
+    legD.reset(0.9f);
 } // resetLegs()
 
 
 
-void walkLegs()
-{
-    state = walk;
-    legState = A;
-} // walkLegs()
+void setupLegs();
 
 
 
@@ -161,7 +86,7 @@
 {
     Timer deltaTimer;
     float xaxis, yaxis, turnaxis, angle;
-    float deltaTime, cycleTime;
+    float deltaTime;
     vector3 v;
     matrix4 T;
     matrix4 PA, QA;
@@ -171,19 +96,105 @@
     Terminal terminal;
     
     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();
     
+    setupLegs();
+    
+    // Create matrices to change base from robot coordinates to leg coordinates
+    QA.translate(vector3(0.1f, 0.1f, 0.0f));
+    PA = QA.inverse();
+    QB.translate(vector3(-0.1f, -0.1f, 0.0f));
+    QB.a11 = -1.0f; QB.a22 = -1.0f;
+    PB = QB.inverse();
+    QC.translate(vector3(0.1f, -0.1f, 0.0f));
+    QC.a11 = -1.0f;
+    PC = QC.inverse();
+    QD.translate(vector3(-0.1f, 0.1f, 0.0f));
+    QD.a22 = -1.0f;
+    PD = QD.inverse();
+    
+    // Start timer
+    deltaTimer.start();
+    
+    while(true)
+    {
+        while (deltaTimer.read() < PERIOD);
+        
+        // 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);
+        
+        // Get delta-time
+        deltaTime = deltaTimer.read();
+        deltaTimer.reset();
+        dataLog.push(deltaTime);
+        
+        // Compute delta movement vector and delta angle
+        v.x = -xaxis;
+        v.y = -yaxis;
+        v.z = 0;
+        v = v * MAXSPEED * deltaTime;
+        angle = -turnaxis * MAXTURN * deltaTime;
+        
+        // Compute movement transformation in robot coordinates
+        T.identity().rotateZ(angle).translate(v).inverse();
+        
+        bool stepping = legA.getStepping() || legB.getStepping() || legC.getStepping() || legD.getStepping();
+        bool lockup = false;
+        
+        if (!legA.update(PA*T*QA))
+        {
+            if (stepping) lockup = true;
+            else 
+            {
+                legA.reset(0.8);
+                stepping = true;
+            }
+        }
+        if (!legB.update(PB*T*QB))
+        {
+            if (stepping) lockup = true;
+            else 
+            {
+                legB.reset(0.8f);
+                stepping = true;
+            }
+        }
+        if (!legC.update(PC*T*QC))
+        {
+            if (stepping) lockup = true;
+            else 
+            {
+                legC.reset(0.8f);
+                stepping = true;
+            }
+        }
+        if (!legD.update(PD*T*QD))
+        {
+            if (stepping) lockup = true;
+            else 
+            {
+                legD.reset(0.8f);
+                stepping = true;
+            }
+        }
+        
+        if (!lockup)
+        {
+            legA.apply();
+            legB.apply();
+            legC.apply();
+            legD.apply();
+        }
+    } // while (true)
+} // main()
+
+
+
+void setupLegs()
+{
     // Set leg parameters
     legA.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D);
     legB.setDimensions(DIM_A, DIM_B, DIM_C, DIM_D);
@@ -197,41 +208,24 @@
     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);
-    legB.theta.calibrate(1000, 2000, 45.0f, -45.0f);
-    legB.phi.calibrate(1000, 2000, 70.0f, -45.0f);
-    legB.psi.calibrate(2000, 1000, 70.0f, -60.0f);
+    legA.theta.calibrate(1130, 2060, 45.0f, -45.0f);
+    legA.phi.calibrate(1200, 2050, 70.0f, -45.0f);
+    legA.psi.calibrate(2020, 1050, 70.0f, -60.0f);
+    legB.theta.calibrate(980, 1930, 45.0f, -45.0f);
+    legB.phi.calibrate(1120, 2030, 70.0f, -45.0f);
+    legB.psi.calibrate(2070, 1170, 70.0f, -60.0f);
     legC.theta.calibrate(2000, 1000, 45.0f, -45.0f);
-    legC.phi.calibrate(2000, 1000, 70.0f, -45.0f);
-    legC.psi.calibrate(1000, 2000, 70.0f, -60.0f);
-    legD.theta.calibrate(2000, 1000, 45.0f, -45.0f);
-    legD.phi.calibrate(2000, 1000, 70.0f, -45.0f);
-    legD.psi.calibrate(1000, 2000, 70.0f, -60.0f);
+    legC.phi.calibrate(1930, 1050, 70.0f, -45.0f);
+    legC.psi.calibrate(1100, 2000, 70.0f, -60.0f);
+    legD.theta.calibrate(2000, 1070, 45.0f, -45.0f);
+    legD.phi.calibrate(1930, 1050, 70.0f, -45.0f);
+    legD.psi.calibrate(1020, 1900, 70.0f, -60.0f);
     
     // Initialize leg position deltas
     legA.nDeltaPosition = vector3(0.0f, 0.01f, 0.0f);
     legB.nDeltaPosition = vector3(0.0f, -0.01f, 0.0f);
     legC.nDeltaPosition = vector3(0.0f, 0.01f, 0.0f);
     legD.nDeltaPosition = vector3(0.0f, -0.01f, 0.0f);
-
-    // Create matrices to change base from robot coordinates to leg coordinates
-    QA.translate(vector3(0.1f, 0.1f, 0.0f));
-    PA = QA.inverse();
-    QB.translate(vector3(-0.1f, -0.1f, 0.0f));
-    QB.a11 = -1.0f; QB.a22 = -1.0f;
-    PB = QB.inverse();
-    QC.translate(vector3(0.1f, -0.1f, 0.0f));
-    QC.a11 = -1.0f;
-    PC = QC.inverse();
-    QD.translate(vector3(-0.1f, 0.1f, 0.0f));
-    QD.a22 = -1.0f;
-    PD = QD.inverse();
-    
-    // Start timers
-    deltaTimer.start();
-    stepTimer.start();
     
     // Go to initial position
     legA.move(vector3(0.15f, 0.15f, 0.05f));
@@ -252,124 +246,4 @@
     legD.psi.enable(); wait(0.1f);
     wait(0.4f);
     resetLegs();
-    
-    /*
-    // 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); 
-    */
-    
-    
-    while(true)
-    {
-        switch (state)
-        {
-        case walk:
-            // 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();
-        
-            // 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.z = 0;
-            v = v * MAXSPEED * deltaTime;
-            angle = -turnaxis * MAXTURN * deltaTime;
-            
-            // Compute movement transformation in robot coordinates
-            T.identity().rotateZ(angle).translate(v).inverse();
-            
-            // Update legs
-            if (!legA.update(PA*T*QA))
-            {
-                legA.reset(1.0f);
-                legState = A;
-            }
-            
-            if (!legB.update(PB*T*QB))
-            {
-                legB.reset(1.0f);
-                legState = B;
-            }
-            
-            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:
-            
-            
-            
-        case reset:
-            // 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)
-            {
-                legA.reset(-0.5f);
-                legState = A;
-            }
-            else if ((cycleTime > RESET_STEP_TIME) && legState == A)
-            {
-                legB.reset(0.0f);
-                legState = B;
-            }
-            else if ((cycleTime > RESET_STEP_TIME * 2) && legState == B)
-            {
-                legC.reset(0.5f);
-                legState = C;
-            }
-            else if ((cycleTime > RESET_STEP_TIME * 3) && legState == C)
-            {
-                legD.reset(1.0f);
-                legState = D;
-            }
-            else if ((cycleTime > RESET_STEP_TIME * 4) && legState == D)
-            {
-                walkLegs();
-            }
-            
-            T.identity();
-            
-            legA.update(T);
-            legB.update(T);
-            legC.update(T);
-            legD.update(T);
-            
-            break; // case reset:
-        } // switch (state)
-    } // while (true)
-} // main()
+}