Diff: Step_Forward.cpp

Revision:

0:8a7c1e92d067

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Step_Forward.cpp	Sun Oct 16 16:01:08 2011 +0000
@@ -0,0 +1,101 @@
+#include "FK.h"
+
+#define Z_OFFSET 190            // Offset for Z-axis
+#define RATE 4                  // Time in seconds to complete one step
+#define b 50                    // b constant for ellipse calculation
+
+void Step_Forward(int stride) {
+    Timer t;                    // Used to control the ellipse
+
+    float FL_Y, FL_Z;           // Variables for the fore left leg
+    float FR_Y, FR_Z;           // Variables for the fore right leg
+    float HL_Y, HL_Z;           // Variables for the hind left leg
+    float HR_Y, HR_Z;           // Variables for the hind right leg
+    float a = stride / 2;       // The a constant for ellipse calculation
+    float cadence = 2*PI/RATE;  // The constant to make algorithm calculate for full 360�
+    float leg_timer[4];
+    float time;
+    float master_time;
+    
+    // Create structs to hold joint positions
+    Joint_Angles Hind_left, Hind_right;                 // Variables for joint angles
+    Joint_Angles Fore_left, Fore_right;                 // Variables for joint angles
+
+    while (t < RATE) {
+        master_time = t.read()*cadence;         // Read the timer and scale
+
+        for ( int i = 0; i < 4; i++) {
+            time = master_time + (PI/2 * i);    // Add a phase shift
+            if (time > 2*PI) {
+                time -= 2*PI;                   // Stop time going out of bounds
+            }
+            if (time < PI*3/2) {
+                leg_timer [i] = time * 2/3;
+            } else {
+                leg_timer [i] = (time * 2) - 2*PI;
+            }
+        }
+
+        // For the fore left leg...
+        FL_Y = cos(leg_timer [0]);      // Calculate the Y position
+        FL_Y = a * FL_Y;
+        FL_Z = sin(-1 * leg_timer [0]); // Calculate the Z position and correct for Z-axis offset
+        FL_Z = b * FL_Z;
+        FL_Z = FL_Z - Z_OFFSET;
+
+        // For the hind right leg...
+        HR_Y = cos(leg_timer [1]);      // Calculate the Y position
+        HR_Y = a * HR_Y;
+        HR_Z = sin(-1 * leg_timer [1]); // Calculate the Z position and correct for Z-axis offset
+        HR_Z = b * HR_Z;
+        HR_Z = HR_Z - Z_OFFSET +20;
+           
+        // For the fore right leg...
+        FR_Y = cos(leg_timer [2]);      // Calculate the Y position
+        FR_Y = a * FR_Y;
+        FR_Z = sin(-1 * leg_timer [2]); // Calculate the Z position and correct for Z-axis offset
+        FR_Z = b * FR_Z;
+        FR_Z = FR_Z - Z_OFFSET;
+           
+        // For the hind left leg...
+        HL_Y = cos(leg_timer [3]);      // Calculate the Y position
+        HL_Y = a * HL_Y;
+        HL_Z = sin(-1 * leg_timer [3]); // Calculate the Z position and correct for Z-axis offset
+        HL_Z = b * HL_Z;
+        HL_Z = HL_Z - Z_OFFSET +20;
+           
+        // Calculate the required joint angles...
+        Fore_right = IK_Engine(70,FR_Y,FR_Z,FORE);
+        Fore_left = IK_Engine(-70,FL_Y,FL_Z,FORE);
+        Hind_right = IK_Engine(70,HR_Y,HR_Z,HIND);
+        Hind_left = IK_Engine(-70,HL_Y,HL_Z,HIND);
+        
+         // Translate the joint angles into Servo Angles       
+        Fore_right.lateral = Fore_right.lateral + 135;
+        Fore_right.hip = 355 - Fore_right.hip;
+        Fore_right.knee = 305 - Fore_right.knee;
+        Fore_left.lateral = 165 - Fore_left.lateral;
+        Fore_left.hip = Fore_left.hip - 55;
+        Fore_left.knee = Fore_left.knee - 5;  
+        Hind_right.lateral = 165 - Hind_right.lateral;
+        Hind_right.hip = 340 - Hind_right.hip;
+        Hind_right.knee = 320 - Hind_right.knee;
+        Hind_left.lateral = Hind_left.lateral + 135;
+        Hind_left.hip = Hind_left.hip - 40;
+        Hind_left.knee = Hind_left.knee - 20;
+        
+        // And finally move the leg to the current set point
+        Fore_Right_Hip_Lateral.SetGoal((int)Fore_right.lateral);
+        Fore_Right_Hip.SetGoal((int)Fore_right.hip);
+        Fore_Right_Knee.SetGoal((int)Fore_right.knee);
+        Fore_Left_Hip_Lateral.SetGoal((int)Fore_left.lateral);
+        Fore_Left_Hip.SetGoal((int)Fore_left.hip);
+        Fore_Left_Knee.SetGoal((int)Fore_left.knee);
+        Hind_Right_Hip_Lateral.SetGoal((int)Hind_right.lateral);       
+        Hind_Right_Hip.SetGoal((int)Hind_right.hip);
+        Hind_Right_Knee.SetGoal((int)Hind_right.knee);
+        Hind_Left_Hip_Lateral.SetGoal((int)Hind_left.lateral);
+        Hind_Left_Hip.SetGoal((int)Hind_left.hip);
+        Hind_Left_Knee.SetGoal((int)Hind_left.knee);        
+    }
+}
\ No newline at end of file