Diff: main.cpp

Revision:

18:8806d24809c2

Parent:

16:cc1ae2a289ee

Child:

19:efba54b23912

--- a/main.cpp	Mon May 27 03:31:58 2013 +0000
+++ b/main.cpp	Mon May 27 20:08:03 2013 +0000
@@ -32,6 +32,8 @@
 
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
 
 
 
@@ -80,18 +82,10 @@
 
 
 
-int deadzone(int input, int zone)
-{
-    if (input > zone) return input;
-    else if (input < -zone) return input;
-    else return 0;
-} // deadzone()
+void setupLegs();
+float calcStability(vector3 p1, vector3 p2);
 
-
-
-void setupLegs();
-
-
+int counter; //debug
 
 int main()
 {
@@ -114,15 +108,15 @@
     setupLegs();
     
     // Create matrices to change base from robot coordinates to leg coordinates
-    QA.translate(vector3(0.508f, 0.508f, 0.0f));
+    QA.translate(vector3(0.0508f, 0.0508f, 0.0f));
     PA = QA.inverse();
-    QB.translate(vector3(-0.508f, -0.508f, 0.0f));
+    QB.translate(vector3(-0.0508f, -0.0508f, 0.0f));
     QB.a11 = -1.0f; QB.a22 = -1.0f;
     PB = QB.inverse();
-    QC.translate(vector3(-0.508f, 0.508f, 0.0f));
+    QC.translate(vector3(-0.0508f, 0.0508f, 0.0f));
     QC.a11 = -1.0f;
     PC = QC.inverse();
-    QD.translate(vector3(0.508f, -0.508f, 0.0f));
+    QD.translate(vector3(0.0508f, -0.0508f, 0.0f));
     QD.a22 = -1.0f;
     PD = QD.inverse();
     
@@ -154,7 +148,7 @@
         }
         
         deltaTimer.reset();
-        dataLog.push(deltaTime);
+        //dataLog.push(deltaTime);
         
         // Compute delta movement vector and delta angle
         v.x = -xaxis;
@@ -166,10 +160,81 @@
         // 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
+        
+        switch(leastIndex)
+        {
+        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;
+        }
+        
+        
+        
+        stability = calcStability(point1, point2);
+        bool stepEarly = stability < margin && stability > 0.0f;
+        
+        // debug stuff
+        if ((counter++%50)==0)
+        {
+            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 (stability < 0.0f)
+        {
+            // 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();
+        }
+        
+        // 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;
         
-        if (!legA.update(PA*T*QA))
+        if (!freeA || (0 == leastIndex && stepEarly))
         {
             if (stepping) lockup = true;
             else 
@@ -178,7 +243,7 @@
                 stepping = true;
             }
         }
-        if (!legB.update(PB*T*QB))
+        if (!freeB || (1 == leastIndex && stepEarly))
         {
             if (stepping) lockup = true;
             else 
@@ -187,7 +252,7 @@
                 stepping = true;
             }
         }
-        if (!legC.update(PC*T*QC))
+        if (!freeC || (2 == leastIndex && stepEarly))
         {
             if (stepping) lockup = true;
             else 
@@ -196,7 +261,7 @@
                 stepping = true;
             }
         }
-        if (!legD.update(PD*T*QD))
+        if (!freeD || (3 == leastIndex && stepEarly))
         {
             if (stepping) lockup = true;
             else 
@@ -275,3 +340,17 @@
     legC.reset(0.4f);
     legD.reset(0.9f);
 }
+
+
+
+float calcStability(vector3 p1, vector3 p2)
+{
+    float lx, ly, vx, vy;
+    lx = p2.x - p1.x;
+    ly = p2.y - p1.y;
+    vx = -p1.x;
+    vy = -p1.y;
+    
+    return (ly*vx - lx*vy)/sqrt(lx*lx + ly*ly);
+}
+