Diff: Walk_Forward.cpp

Revision:

0:0965dacb3caf

diff -r 000000000000 -r 0965dacb3caf Walk_Forward.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Walk_Forward.cpp	Sun Aug 21 08:25:00 2011 +0000
@@ -0,0 +1,61 @@
+#include "ERIC.h"
+
+#define Z_OFFSET 150            // Offset for Z-axis
+#define RATE 2                  // Time in seconds to complete one step
+#define ELLIPSE_HGT 20          // b constant for ellipse calculation
+
+//Serial pc(USBTX,USBRX);         // To communicate with TeraTerm
+
+void Walk_Forward(int stride) {
+    Timer t;                    // Used to control the ellipse
+
+    float RHS_Y, RHS_Z;         // Variables for the right leg
+    float LHS_Y, LHS_Z;         // Variables for the left leg
+    float a = stride / 2;       // The a constant for ellipse calculation
+    float angle = 360 / RATE;   // The constant to make algorithm calculate for full 360°
+    angle = angle * PI / 180;   // Convert into radians
+    float time = 0;             // Variable to hold the timer value.
+        
+    // Create variables to hold the joint angles & current set points
+    Joint_Angles LHS, RHS;      // Variables for joint angles
+
+    t.start();                  // Start the timer
+
+    while (t < RATE) {
+        time = t.read();        // Read the timer
+
+        // For the right leg...
+        RHS_Y = sin(time*angle);            // Calculate the Y position
+        RHS_Y = a * RHS_Y;
+        RHS_Z = cos(time*angle);            // Calculate the Z position and correct for Z-axis offset
+        RHS_Z = ELLIPSE_HGT * RHS_Z;
+        RHS_Z = RHS_Z - Z_OFFSET;
+
+        // For the left leg...
+        LHS_Y = sin((time+RATE/2)*angle);   // Calculate the Y position
+        LHS_Y = a * LHS_Y;
+        LHS_Z = cos((time+RATE/2)*angle);   // Calculate the Z position and correct for Z-axis offset
+        LHS_Z = ELLIPSE_HGT * LHS_Z;
+        LHS_Z = LHS_Z - Z_OFFSET;
+
+        // Calculate the required joint angles...
+        RHS = IK_Engine(70,RHS_Y,RHS_Z);
+        LHS = IK_Engine(-70,LHS_Y,LHS_Z);
+        
+        // Translate the joint angles into Servo Angles
+        RHS.lateral = 165 - RHS.lateral;
+        RHS.hip = 340 - RHS.hip;
+        RHS.knee = 320 - RHS.knee;
+        LHS.lateral = LHS.lateral + 135;
+        LHS.hip = LHS.hip - 40;
+        LHS.knee = LHS.knee - 20;
+
+        // And finally move the leg to the current set point
+        Right_Hip_Lateral.SetGoal((int)RHS.lateral);
+        Right_Hip.SetGoal((int)RHS.hip);
+        Right_Knee.SetGoal((int)RHS.knee);
+        Left_Hip_Lateral.SetGoal((int)LHS.lateral);
+        Left_Hip.SetGoal((int)LHS.hip);
+        Left_Knee.SetGoal((int)LHS.knee);     
+    }
+}