Revision 22:9cf770fb12f8, committed 2014-09-30

Comitter:

pclary

Date:

Tue Sep 30 07:31:07 2014 +0000

Parent:

21:c00567cbe6cc

Commit message:

Hacked together a better demonstration

Changed in this revision

--- a/RobotLeg.cpp	Tue May 28 04:20:23 2013 +0000
+++ b/RobotLeg.cpp	Tue Sep 30 07:31:07 2014 +0000
@@ -9,9 +9,9 @@
     setAngleOffsets(0.0f, 0.0f, 0.0f);
     
     state = neutral;
-    stepTime = 0.4f;
+    stepTime = 0.35f;
     stepDelta = 3.141593f / stepTime;
-    stepHeight = 0.05f;
+    stepHeight = 0.06f;
 }
 
 
@@ -175,7 +175,7 @@
         {
             newPosition.x = stepA.x + (stepB.x - stepA.x)*0.5f*(1 - cos(stepDelta*t));
             newPosition.y = stepA.y + (stepB.y - stepA.y)*0.5f*(1 - cos(stepDelta*t));
-            newPosition.z = stepA.z + (stepB.z - stepA.z)*stepDelta*t + stepHeight*sin(stepDelta*t);
+            newPosition.z = stepA.z + (stepB.z - stepA.z)*stepDelta*t + stepHeight*pow(sin(stepDelta*t), 0.1f);
         }
         else
         {

--- a/main.cpp	Tue May 28 04:20:23 2013 +0000
+++ b/main.cpp	Tue Sep 30 07:31:07 2014 +0000
@@ -17,7 +17,7 @@
 #define DIM_D 0.0275f
 #define CIRCLE_X 0.095f
 #define CIRCLE_Y 0.095f
-#define CIRCLE_Z -0.12f
+#define CIRCLE_Z -0.125f
 #define CIRCLE_R 0.09f
 #define PERIOD 0.005f
 
@@ -90,6 +90,16 @@
 void resetLegs();
 float calcStability(vector3 p1, vector3 p2);
 
+enum MovementState {
+    ms_none,
+    ms_forward,
+    ms_backward,
+    ms_right,
+    ms_left,
+    ms_ccw,
+    ms_cw
+};
+
 
 
 int main()
@@ -119,6 +129,8 @@
     
     matrix4 TMat;
     
+    MovementState mstate = ms_none;
+    
     // Start timer
     deltaTimer.start();
     
@@ -131,6 +143,70 @@
         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);
         
+        // Quantize controller input into movement states
+        MovementState mstatenew = mstate;
+        const float xythresh = 0.3f;
+        const float turnthresh = 0.3f;
+        if (xaxis > xythresh && abs(xaxis) >= abs(yaxis))
+            mstatenew = ms_forward;
+        else if (xaxis < -xythresh && abs(xaxis) >= abs(yaxis))
+            mstatenew = ms_backward;
+        else if (yaxis > xythresh)
+            mstatenew = ms_right;
+        else if (yaxis < -xythresh)
+            mstatenew = ms_left;
+        else if (turnaxis > turnthresh)
+            mstatenew = ms_ccw;
+        else if (turnaxis < -turnthresh)
+            mstatenew = ms_cw;
+        else
+            mstatenew = ms_none;
+            
+        switch (mstatenew) {
+        case ms_none:
+            xaxis = 0.f;
+            yaxis = 0.f;
+            turnaxis = 0.f;
+            break;
+        case ms_forward:
+            xaxis = 1.f;
+            yaxis = 0.f;
+            turnaxis = 0.f;
+            break;
+        case ms_backward:
+            xaxis = -1.f;
+            yaxis = 0.f;
+            turnaxis = 0.f;
+            break;
+        case ms_right:
+            xaxis = 0.f;
+            yaxis = 1.f;
+            turnaxis = 0.f;
+            break;
+        case ms_left:
+            xaxis = 0.f;
+            yaxis = -1.f;
+            turnaxis = 0.f;
+            break;
+        case ms_ccw:
+            xaxis = 0.f;
+            yaxis = 0.f;
+            turnaxis = 1.f;
+            break;
+        case ms_cw:
+            xaxis = 0.f;
+            yaxis = 0.f;
+            turnaxis = -1.f;
+            break;
+        }
+        
+        // Don't move while stepping
+        if (legA.getStepping() || legB.getStepping() || legC.getStepping() || legD.getStepping()) {
+            xaxis = 0.f;
+            yaxis = 0.f;
+            turnaxis = 0.f;
+        }
+        
         // Reset legs to sane positions when 'A' button is pressed
         if ((radio.rx_controller>>25)&0x1) resetLegs();
         
@@ -147,6 +223,11 @@
         
         processMovement(TMat);
         
+        if (mstatenew != mstate && mstatenew != ms_none)
+            resetLegs();
+            
+        mstate = mstatenew;
+        
     } // while (true)
 } // main()
 
@@ -193,22 +274,22 @@
             }
             else 
             {
-                if (stability[i] > borderMin)
+                //if (stability[i] > borderMin)
                 {
                     // If stable, step
-                    leg[i]->reset(0.8);
+                    leg[i]->reset(1.0f);
                     stepping = true;
                 }
-                else
+                /*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;
+//                    n = n.unit() * -MAXSPEED * PERIOD;
 //                    TMat.identity().translate(n).inverse();
 //                    return false;
-                }
+                }*/
             }
         }
     }
@@ -238,7 +319,7 @@
 //        vector3 n;
 //        n.x = point2[next].y - point1[next].y;
 //        n.y = point1[next].x - point2[next].x;
-//        n = n.unit() * MAXSPEED * PERIOD;
+//        n = n.unit() * -MAXSPEED * PERIOD;
 //        TMat.identity().translate(n).inverse();
 //        return false;
     }
@@ -249,10 +330,14 @@
     }
     
     // Debug info
-    led1 = stability[0] > borderMin;
-    led2 = stability[1] > borderMin;
-    led3 = stability[2] > borderMin;
-    led4 = stability[3] > borderMin;
+//    led1 = stability[0] > borderMin;
+//    led2 = stability[1] > borderMin;
+//    led3 = stability[2] > borderMin;
+//    led4 = stability[3] > borderMin;
+    led1 = legA.getStepping();
+    led2 = legB.getStepping();
+    led3 = legC.getStepping();
+    led4 = legD.getStepping();
     
     return true;
 }
@@ -300,7 +385,7 @@
     legA.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
     legB.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
     legC.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
-    legD.setAngleOffsets(0.7853982f, 0.0f, 0.0f);
+    legD.setAngleOffsets(0.7853982f, 0.1f, 0.0f);
     legA.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R);
     legB.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R);
     legC.setStepCircle(CIRCLE_X, CIRCLE_Y, CIRCLE_Z, CIRCLE_R);