Revision 20:bf46c0400b10, committed 2013-05-28

Comitter:

pclary

Date:

Tue May 28 04:11:37 2013 +0000

Parent:

19:efba54b23912

Child:

21:c00567cbe6cc

Commit message:

Currently stays still if a requested move will put it out of balance

Changed in this revision

--- a/RobotLeg.cpp	Mon May 27 23:20:14 2013 +0000
+++ b/RobotLeg.cpp	Tue May 28 04:11:37 2013 +0000
@@ -184,6 +184,8 @@
             stepTimer.stop();
         }
         
+        move(newPosition);
+        
         return true;
         
     default:
@@ -195,7 +197,7 @@
 
 void RobotLeg::apply()
 {
-    move(newPosition);
+    if (stepping != state) move(newPosition);
 }
 
 

--- a/main.cpp	Mon May 27 23:20:14 2013 +0000
+++ b/main.cpp	Tue May 28 04:11:37 2013 +0000
@@ -19,7 +19,7 @@
 #define CIRCLE_Y 0.095f
 #define CIRCLE_Z -0.12f
 #define CIRCLE_R 0.09f
-#define PERIOD 0.002f
+#define PERIOD 0.005f
 
 
 
@@ -29,6 +29,9 @@
 RobotLeg legB(p13, p14, p15, false);
 RobotLeg legC(p19, p11, p8, false);
 RobotLeg legD(p25, p24, p23, false);
+RobotLeg* leg[4] = { &legA, &legB, &legC, &legD };
+matrix4 QMat[4];
+matrix4 PMat[4];
 
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
@@ -82,6 +85,7 @@
 
 
 
+bool processMovement(matrix4& TMat);
 void setupLegs();
 void resetLegs();
 float calcStability(vector3 p1, vector3 p2);
@@ -99,13 +103,7 @@
     radio.reset();
     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
-    matrix4 QMat[4];
-    matrix4 PMat[4];
-    
+    // Initialize matrices to change base from robot coordinates to leg coordinates
     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;
@@ -124,7 +122,7 @@
     // Start timer
     deltaTimer.start();
     
-    while(true)
+    while (true)
     {
         while (deltaTimer.read() < PERIOD);
         
@@ -133,137 +131,131 @@
         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
-        float deltaTime = deltaTimer.read();
-        
         // Reset legs to sane positions when 'A' button is pressed
         if ((radio.rx_controller>>25)&0x1) resetLegs();
         
         deltaTimer.reset();
-        dataLog.push(deltaTime);
+        dataLog.push(deltaTimer.read());
         
         // Compute delta movement vector and delta angle
         vector3 v(-xaxis, -yaxis, 0.0f);
-        v = v * MAXSPEED * deltaTime;
-        float angle = -turnaxis * MAXTURN * deltaTime;
+        v = v * MAXSPEED * PERIOD;
+        float angle = -turnaxis * MAXTURN * PERIOD;
         
         // Compute movement transformation in robot coordinates
         TMat.identity().rotateZ(angle).translate(v).inverse();
         
-        // 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)
-        {
-            legFree[i] = leg[i]->update(PMat[i]*TMat*QMat[i]);
-            stepDist[i] = leg[i]->getStepDistance();
-            stability[i] = calcStability(point1[i], point2[i]);
-        }
+        processMovement(TMat);
         
-        // 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;
-        
-        for (int i = 0; i < 4; ++i)
+    } // while (true)
+} // main()
+
+
+
+bool processMovement(matrix4& TMat)
+{
+    // 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)
+    {
+        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();
+    const float borderMax = 0.015f; // radius of support base in meters
+    const float borderMin = 0.007f;
+    
+    for (int i = 0; i < 4; ++i)
+    {
+        if (!legFree[i])
         {
-            if (!legFree[i])
+            if (stepping)
             {
-                if (stepping)
-                {
-                    TMat.identity();
-                    changedMotion = true;
-                }
-                else 
+                TMat.identity();
+                return false;
+            }
+            else 
+            {
+                if (stability[i] > borderMin)
                 {
-                    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 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 * PERIOD;
+                    TMat.identity().translate(n).inverse();
+                    return false;
                 }
             }
         }
-        
-        if (!changedMotion)
+    }
+    
+    // Check if the next leg to step is stable
+    int next = least(stepDist[0], stepDist[1], stepDist[2], stepDist[3]);
+    if (stability[next] > borderMax)
+    {
+        // Continue to carry out step as normal
+    }
+    else if (stability[next] > borderMin)
+    {
+        if (stepping)
+        {
+            TMat.identity();
+            return false;
+        }
+        else 
         {
-            // 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;
-            }
+            leg[next]->reset(0.8);
+            stepping = 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
-        led1 = stability[0] > borderMin;
-        led2 = stability[1] > borderMin;
-        led3 = stability[2] > borderMin;
-        led4 = stability[3] > borderMin;
-        
-    } // while (true)
-} // main()
+    }
+    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 * PERIOD;
+        TMat.identity().translate(n).inverse();
+        return false;
+    }
+    
+    for (int i = 0; i < 4; ++i)
+    {
+        leg[i]->apply();
+    }
+    
+    // Debug info
+    led1 = stability[0] > borderMin;
+    led2 = stability[1] > borderMin;
+    led3 = stability[2] > borderMin;
+    led4 = stability[3] > borderMin;
+    
+    return true;
+}
 
 
 
@@ -271,13 +263,29 @@
 {
     matrix4 T;
     legA.reset(-0.6f);
-    while (legA.getStepping()) legA.update(T);
+    while (legA.getStepping())
+    {
+        legA.update(T);
+        legA.apply();
+    }
     legB.reset(-0.1f);
-    while (legB.getStepping()) legB.update(T);
+    while (legB.getStepping())
+    {
+        legB.update(T);
+        legB.apply();
+    }
     legC.reset(0.4f);
-    while (legC.getStepping()) legC.update(T);
+    while (legC.getStepping())
+    {
+        legC.update(T);
+        legC.apply();
+    }
     legD.reset(0.9f);
-    while (legD.getStepping()) legD.update(T);
+    while (legD.getStepping())
+    {
+        legD.update(T);
+        legD.apply();
+    }
 }
 
 
@@ -334,7 +342,22 @@
     legC.psi.enable(); wait(0.1f);
     legD.psi.enable(); wait(0.1f);
     wait(0.4f);
-    resetLegs();
+    legA.reset(-0.6f);
+    legB.reset(-0.1f);
+    legC.reset(0.4f);
+    legD.reset(0.9f);
+    matrix4 T;
+    while (legA.getStepping())
+    {
+        legA.update(T);
+        legA.apply();
+        legB.update(T);
+        legB.apply();
+        legC.update(T);
+        legC.apply();
+        legD.update(T);
+        legD.apply();
+    }
 }