Diff: main.cpp

Revision:

19:efba54b23912

Parent:

18:8806d24809c2

Child:

20:bf46c0400b10

--- a/main.cpp	Mon May 27 20:08:03 2013 +0000
+++ b/main.cpp	Mon May 27 23:20:14 2013 +0000
@@ -83,42 +83,43 @@
 
 
 void setupLegs();
+void resetLegs();
 float calcStability(vector3 p1, vector3 p2);
 
-int counter; //debug
+
 
 int main()
 {
     Timer deltaTimer;
-    float xaxis, yaxis, turnaxis, angle;
-    float deltaTime;
-    vector3 v;
-    matrix4 T;
-    matrix4 PA, QA;
-    matrix4 PB, QB; 
-    matrix4 PC, QC; 
-    matrix4 PD, QD;
     Terminal terminal;
     
     terminal.addCommand("log", &log);
     terminal.addCommand("leg", &legpos);
     
     radio.reset();
+    setupLegs();
     
-    setupLegs();
+    // Wrap legs up in an array for convenience
+    RobotLeg* leg[4] = { &legA, &legB, &legC, &legD };
     
     // Create matrices to change base from robot coordinates to leg coordinates
-    QA.translate(vector3(0.0508f, 0.0508f, 0.0f));
-    PA = QA.inverse();
-    QB.translate(vector3(-0.0508f, -0.0508f, 0.0f));
-    QB.a11 = -1.0f; QB.a22 = -1.0f;
-    PB = QB.inverse();
-    QC.translate(vector3(-0.0508f, 0.0508f, 0.0f));
-    QC.a11 = -1.0f;
-    PC = QC.inverse();
-    QD.translate(vector3(0.0508f, -0.0508f, 0.0f));
-    QD.a22 = -1.0f;
-    PD = QD.inverse();
+    matrix4 QMat[4];
+    matrix4 PMat[4];
+    
+    QMat[0].translate(vector3(0.0508f, 0.0508f, 0.0f));
+    QMat[1].translate(vector3(-0.0508f, -0.0508f, 0.0f));
+    QMat[1].a11 = -1.0f; QMat[1].a22 = -1.0f;
+    QMat[2].translate(vector3(-0.0508f, 0.0508f, 0.0f));
+    QMat[2].a11 = -1.0f;
+    QMat[3].translate(vector3(0.0508f, -0.0508f, 0.0f));
+    QMat[3].a22 = -1.0f;
+    
+    PMat[0] = QMat[0].inverse();
+    PMat[1] = QMat[1].inverse();
+    PMat[2] = QMat[2].inverse();
+    PMat[3] = QMat[3].inverse();
+    
+    matrix4 TMat;
     
     // Start timer
     deltaTimer.start();
@@ -128,161 +129,159 @@
         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);
+        float xaxis = 0.0078125f * deadzone((int8_t)((radio.rx_controller>>0)&0xff), 8); // Convert to +/-1.0f range
+        float yaxis = -0.0078125f * deadzone((int8_t)((radio.rx_controller>>8)&0xff), 8);
+        float turnaxis = -0.0078125f * deadzone((int8_t)((radio.rx_controller>>16)&0xff), 8);
         
         // Get delta-time
-        deltaTime = deltaTimer.read();
+        float deltaTime = deltaTimer.read();
         
-        if ((radio.rx_controller>>25)&0x1) // reset
-        {
-            legA.reset(-0.6f);
-            while (legA.getStepping()) legA.update(T);
-            legB.reset(-0.1f);
-            while (legB.getStepping()) legB.update(T);
-            legC.reset(0.4f);
-            while (legC.getStepping()) legC.update(T);
-            legD.reset(0.9f);
-            while (legD.getStepping()) legD.update(T);
-        }
+        // Reset legs to sane positions when 'A' button is pressed
+        if ((radio.rx_controller>>25)&0x1) resetLegs();
         
         deltaTimer.reset();
-        //dataLog.push(deltaTime);
+        dataLog.push(deltaTime);
         
         // Compute delta movement vector and delta angle
-        v.x = -xaxis;
-        v.y = -yaxis;
-        v.z = 0.0f;
+        vector3 v(-xaxis, -yaxis, 0.0f);
         v = v * MAXSPEED * deltaTime;
-        angle = -turnaxis * MAXTURN * deltaTime;
+        float angle = -turnaxis * MAXTURN * deltaTime;
         
         // Compute movement transformation in robot coordinates
-        T.identity().rotateZ(angle).translate(v).inverse();
-        
-        bool freeA = legA.update(PA*T*QA);
-        bool freeB = legB.update(PB*T*QB);
-        bool freeC = legC.update(PC*T*QC);
-        bool freeD = legD.update(PD*T*QD);
-        
-        // Predict unstable step events and step early in response
-        float distA = legA.getStepDistance();
-        float distB = legB.getStepDistance();
-        float distC = legC.getStepDistance();
-        float distD = legD.getStepDistance();
-        
-        int leastIndex = least(distA, distB, distC, distD);
-        float stability;
-        vector3 point1, point2;
-        const float margin = 0.015f; // radius of support base in meters
+        TMat.identity().rotateZ(angle).translate(v).inverse();
         
-        switch(leastIndex)
+        // Get points used to calculate stability
+        vector3 point1[4];
+        vector3 point2[4];
+        point1[0] = QMat[2]*leg[2]->getPosition();
+        point1[1] = QMat[3]*leg[3]->getPosition();
+        point1[2] = QMat[1]*leg[1]->getPosition();
+        point1[3] = QMat[0]*leg[0]->getPosition();
+        point2[0] = QMat[3]*leg[3]->getPosition();
+        point2[1] = QMat[2]*leg[2]->getPosition();
+        point2[2] = QMat[0]*leg[0]->getPosition();
+        point2[3] = QMat[1]*leg[1]->getPosition();
+        
+        // Check if each leg can perform this motion, find the next leg to step, and calculate stability of each leg
+        bool legFree[4];
+        float stepDist[4];
+        float stability[4];
+        for (int i = 0; i < 4; ++i)
         {
-        case 0:
-            point1 = QC*legC.getPosition();
-            point2 = QD*legD.getPosition();
-            break;
-        case 1:
-            point1 = QD*legD.getPosition();
-            point2 = QC*legC.getPosition();
-            break;
-        case 2:
-            point1 = QB*legB.getPosition();
-            point2 = QA*legA.getPosition();
-            break;
-        case 3:
-            point1 = QA*legA.getPosition();
-            point2 = QB*legB.getPosition();
-            break;
+            legFree[i] = leg[i]->update(PMat[i]*TMat*QMat[i]);
+            stepDist[i] = leg[i]->getStepDistance();
+            stability[i] = calcStability(point1[i], point2[i]);
         }
         
-        
+        // Check if each leg needs to step, and then check if it's stable before stepping
+        bool stepping = leg[0]->getStepping() || leg[1]->getStepping() || leg[2]->getStepping() || leg[3]->getStepping();
+        bool changedMotion = false;
+        const float borderMax = 0.015f; // radius of support base in meters
+        const float borderMin = 0.007f;
         
-        stability = calcStability(point1, point2);
-        bool stepEarly = stability < margin && stability > 0.0f;
-        
-        // debug stuff
-        if ((counter++%50)==0)
+        for (int i = 0; i < 4; ++i)
         {
-            char buf[256];
-            char vbuf[64];
-            char vbuf2[64];
-            point1.print(vbuf, 64);
-            point2.print(vbuf2, 64);
-            snprintf(buf, 256, "n = %d; p1 = %s; p1 = %s; s = %f\n", leastIndex, vbuf, vbuf2, stability); 
-            terminal.write(buf);
+            if (!legFree[i])
+            {
+                if (stepping)
+                {
+                    TMat.identity();
+                    changedMotion = true;
+                }
+                else 
+                {
+                    if (stability[i] > borderMin)
+                    {
+                        // If stable, step
+                        leg[i]->reset(0.8);
+                        stepping = true;
+                    }
+                    else
+                    {
+                        // If unstable, move towards a stable position
+                        vector3 n;
+                        n.x = point2[i].y - point1[i].y;
+                        n.y = point1[i].x - point2[i].x;
+                        n = n.unit() * MAXSPEED * deltaTime;
+                        TMat.identity().translate(n).inverse();
+                        changedMotion = true;
+                    }
+                }
+            }
         }
         
-        if (stability < 0.0f)
+        if (!changedMotion)
         {
-            // Move towards stable zone
-            vector3 n;
-            n.x = point2.y - point1.y;
-            n.y = point1.x - point2.x;
-            n = n.unit() * MAXSPEED * deltaTime;
-            T.identity().translate(v).inverse();
+            // Check if the next leg to step is stable
+            int next = least(stepDist[0], stepDist[1], stepDist[2], stepDist[3]);
+            if (stepDist[next] > borderMax)
+            {
+                // Continue to carry out step as normal
+            }
+            else if (stepDist[next] > borderMin)
+            {
+                if (stepping)
+                {
+                    TMat.identity();
+                    changedMotion = true;
+                }
+                else 
+                {
+                    leg[next]->reset(0.8);
+                    stepping = true;
+                }
+            }
+            else
+            {
+                // If unstable, move towards a stable position
+                vector3 n;
+                n.x = point2[next].y - point1[next].y;
+                n.y = point1[next].x - point2[next].x;
+                n = n.unit() * MAXSPEED * deltaTime;
+                TMat.identity().translate(n).inverse();
+                changedMotion = true;
+            }
+        }
+        
+        if (changedMotion)
+        {
+            for (int i = 0; i < 4; ++i)
+            {
+                leg[i]->update(PMat[i]*TMat*QMat[i]);
+            }
+        }
+        
+        for (int i = 0; i < 4; ++i)
+        {
+            leg[i]->apply();
         }
         
         // Debug info
-        dataLog.push(stability);
-        led1 = calcStability(QC*legC.getPosition(), QD*legD.getPosition()) > 0.0f;
-        led2 = calcStability(QD*legD.getPosition(), QC*legC.getPosition()) > 0.0f;
-        led3 = calcStability(QB*legB.getPosition(), QA*legA.getPosition()) > 0.0f;
-        led4 = calcStability(QA*legA.getPosition(), QB*legB.getPosition()) > 0.0f;
-        
-        // Prevent multiple legs from stepping at the same time
-        bool stepping = legA.getStepping() || legB.getStepping() || legC.getStepping() || legD.getStepping();
-        bool lockup = false;
+        led1 = stability[0] > borderMin;
+        led2 = stability[1] > borderMin;
+        led3 = stability[2] > borderMin;
+        led4 = stability[3] > borderMin;
         
-        if (!freeA || (0 == leastIndex && stepEarly))
-        {
-            if (stepping) lockup = true;
-            else 
-            {
-                legA.reset(0.8);
-                stepping = true;
-            }
-        }
-        if (!freeB || (1 == leastIndex && stepEarly))
-        {
-            if (stepping) lockup = true;
-            else 
-            {
-                legB.reset(0.8f);
-                stepping = true;
-            }
-        }
-        if (!freeC || (2 == leastIndex && stepEarly))
-        {
-            if (stepping) lockup = true;
-            else 
-            {
-                legC.reset(0.8f);
-                stepping = true;
-            }
-        }
-        if (!freeD || (3 == leastIndex && stepEarly))
-        {
-            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 resetLegs()
+{
+    matrix4 T;
+    legA.reset(-0.6f);
+    while (legA.getStepping()) legA.update(T);
+    legB.reset(-0.1f);
+    while (legB.getStepping()) legB.update(T);
+    legC.reset(0.4f);
+    while (legC.getStepping()) legC.update(T);
+    legD.reset(0.9f);
+    while (legD.getStepping()) legD.update(T);
+}
+
+
+
 void setupLegs()
 {
     // Set leg parameters
@@ -335,10 +334,7 @@
     legC.psi.enable(); wait(0.1f);
     legD.psi.enable(); wait(0.1f);
     wait(0.4f);
-    legA.reset(-0.6f);
-    legB.reset(-0.1f);
-    legC.reset(0.4f);
-    legD.reset(0.9f);
+    resetLegs();
 }