Revision 0:8a7c1e92d067, committed 2011-10-16

Comitter:

ms523

Date:

Sun Oct 16 16:01:08 2011 +0000

Commit message:

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AX12_Hardware.lib	Sun Oct 16 16:01:08 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/ms523/code/AX12_Hardware/#1dd9cd18975d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FK.h	Sun Oct 16 16:01:08 2011 +0000
@@ -0,0 +1,49 @@
+#include "mbed.h"
+#include "AX12.h"
+
+#define B 45                // Offset dimension
+#define FEMUR 80            // Femur length
+#define TIBIA 140           // Tibia length
+#define ULNA 90             // Ulna length
+#define RADIUS 120          // Radius length
+#define X_OFFSET 25         // Distance from the X-axis -> servo centre
+#define PI 3.14159
+
+#define HIND 0
+#define FORE 1
+
+/********** Global structures **********/
+struct Joint_Angles {
+    float lateral;
+    float hip;
+    float knee;
+};
+
+/**************************************************************************
+Global variables
+**************************************************************************/
+extern AX12 Fore_Right_Hip_Lateral,
+            Fore_Right_Hip,
+            Fore_Right_Knee,
+            Fore_Left_Hip_Lateral,
+            Fore_Left_Hip,
+            Fore_Left_Knee,
+            Hind_Right_Hip_Lateral,
+            Hind_Right_Hip,
+            Hind_Right_Knee,
+            Hind_Left_Hip_Lateral,
+            Hind_Left_Hip,
+            Hind_Left_Knee;
+
+/********** Functions **********/
+void FK_Engine(float*);
+void Set_Pose(const int*, float);
+Joint_Angles IK_Engine(int, int, int, char);
+void Step_Forward(int);
+
+/************ Define Poses ************/
+extern const int Stand[];
+extern const int Sit1[];
+extern const int Sit2[];
+extern const int Sit3[];
+extern const int Sit4[];
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FK_Engine.cpp	Sun Oct 16 16:01:08 2011 +0000
@@ -0,0 +1,190 @@
+#include "FK.h"
+
+void FK_Engine(float *pos){
+
+// Read the ax-12+ Servos and translate into joint angles α, θ and φ for each leg
+    float fore_right_alpha = Fore_Right_Hip_Lateral.GetPosition() - 135;
+    float fore_right_theta = 355 - Fore_Right_Hip.GetPosition();
+    float fore_right_phi = 305 - Fore_Right_Knee.GetPosition();
+    float fore_left_alpha = 165 - Fore_Left_Hip_Lateral.GetPosition();
+    float fore_left_theta = Fore_Left_Hip.GetPosition() + 55;
+    float fore_left_phi = Fore_Left_Knee.GetPosition() + 5;   
+    float hind_right_alpha = 165 - Hind_Right_Hip_Lateral.GetPosition();
+    float hind_right_theta = 340 - Hind_Right_Hip.GetPosition();
+    float hind_right_phi = 320 - Hind_Right_Knee.GetPosition();
+    float hind_left_alpha = Hind_Left_Hip_Lateral.GetPosition() - 135;
+    float hind_left_theta = Hind_Left_Hip.GetPosition() + 40;
+    float hind_left_phi = Hind_Left_Knee.GetPosition() + 20;      
+    float temp;                             // used to store maths
+    
+// Convert into radians for maths
+    hind_right_alpha = hind_right_alpha * PI / 180;
+    hind_right_theta = hind_right_theta  * PI / 180;
+    hind_right_phi = hind_right_phi * PI / 180;
+    hind_left_alpha = hind_left_alpha * PI / 180;
+    hind_left_theta = hind_left_theta  * PI / 180;
+    hind_left_phi = hind_left_phi * PI / 180;
+    fore_right_alpha = fore_right_alpha * PI / 180;
+    fore_right_theta = fore_right_theta  * PI / 180;
+    fore_right_phi = fore_right_phi * PI / 180;
+    fore_left_alpha = fore_left_alpha * PI / 180;
+    fore_left_theta = fore_left_theta  * PI / 180;
+    fore_left_phi = fore_left_phi * PI / 180;
+
+// Next calculate point 1 for all legs
+    // Start with right fore leg
+    float x1_fore_right = sin(fore_right_alpha);
+    x1_fore_right = x1_fore_right * B;
+    float y1_fore_right = 0;
+    float z1_fore_right = cos(fore_right_alpha);
+    z1_fore_right = z1_fore_right * B;
+    // Then left fore leg
+    float x1_fore_left = sin(fore_left_alpha + PI);  //Add PI to invert to get the sign correct
+    x1_fore_left = x1_fore_left * B;            
+    float y1_fore_left = 0;
+    float z1_fore_left = cos(fore_left_alpha);
+    z1_fore_left = z1_fore_left * B;
+    // Then hind right leg
+    float x1_hind_right = sin(hind_right_alpha);
+    x1_hind_right = x1_hind_right * B;
+    float y1_hind_right = 0;
+    float z1_hind_right = cos(hind_right_alpha);
+    z1_hind_right = z1_hind_right * B;
+    // Finally hind left leg
+    float x1_hind_left = sin(hind_left_alpha + PI);  //Add PI to invert to get the sign correct
+    x1_hind_left = x1_hind_left * B;            
+    float y1_hind_left = 0;
+    float z1_hind_left = cos(hind_left_alpha);
+    z1_hind_left = z1_hind_left * B;
+
+// Now calculate point 2 
+    // Aagain start with the fore right leg..
+    float x2_fore_right = cos(fore_right_theta + PI);
+    x2_fore_right = x2_fore_right * ULNA;
+    temp = cos(fore_right_alpha);
+    x2_fore_right = x2_fore_right * temp;
+    x2_fore_right = x1_fore_right + x2_fore_right;
+    float y2_fore_right = sin(fore_right_theta);
+    y2_fore_right = y2_fore_right * ULNA;
+    y2_fore_right = y2_fore_right + y1_fore_right;
+    float z2_fore_right = cos(fore_right_theta);
+    z2_fore_right = z2_fore_right * ULNA;
+    temp = sin(fore_right_alpha);
+    z2_fore_right = z2_fore_right * temp;
+    z2_fore_right = z1_fore_right + z2_fore_right;
+    // Then the fore left leg
+    float x2_fore_left = cos(fore_left_theta + PI);  
+    x2_fore_left = x2_fore_left * ULNA;
+    temp = cos(fore_left_alpha + PI);       //Add PI to invert to get the sign correct
+    x2_fore_left = x2_fore_left * temp;
+    x2_fore_left = x1_fore_left + x2_fore_left;
+    float y2_fore_left = sin(fore_left_theta);
+    y2_fore_left = y2_fore_left * ULNA;
+    y2_fore_left = y2_fore_left + y1_fore_left;
+    float z2_fore_left = cos(fore_left_theta);
+    z2_fore_left = z2_fore_left * ULNA;
+    temp = sin(fore_left_alpha);
+    z2_fore_left = z2_fore_left * temp;
+    z2_fore_left = z1_fore_left + z2_fore_left;
+    // Then the hind right leg
+    float x2_hind_right = cos(hind_right_theta + PI);
+    x2_hind_right = x2_hind_right * FEMUR;
+    temp = cos(hind_right_alpha);
+    x2_hind_right = x2_hind_right * temp;
+    x2_hind_right = x1_hind_right + x2_hind_right;
+    float y2_hind_right = sin(hind_right_theta);
+    y2_hind_right = y2_hind_right * FEMUR;
+    y2_hind_right = y2_hind_right + y1_hind_right;
+    float z2_hind_right = cos(hind_right_theta);
+    z2_hind_right = z2_hind_right * FEMUR;
+    temp = sin(hind_right_alpha);
+    z2_hind_right = z2_hind_right * temp;
+    z2_hind_right = z1_hind_right + z2_hind_right;
+    // Finally the hind left leg...
+    float x2_hind_left = cos(hind_left_theta + PI);  
+    x2_hind_left = x2_hind_left * FEMUR;
+    temp = cos(hind_left_alpha + PI);       //Add PI to invert to get the sign correct
+    x2_hind_left = x2_hind_left * temp;
+    x2_hind_left = x1_hind_left + x2_hind_left;
+    float y2_hind_left = sin(hind_left_theta);
+    y2_hind_left = y2_hind_left * FEMUR;
+    y2_hind_left = y2_hind_left + y1_hind_left;
+    float z2_hind_left = cos(hind_left_theta);
+    z2_hind_left = z2_hind_left * FEMUR;
+    temp = sin(hind_left_alpha);
+    z2_hind_left = z2_hind_left * temp;
+    z2_hind_left = z1_hind_left + z2_hind_left;
+
+// Finally calculate point 3 
+    // Again start with the fore right leg
+    float x3_fore_right = cos(fore_right_theta - fore_right_phi);
+    x3_fore_right = x3_fore_right * RADIUS;
+    temp = cos(fore_right_alpha);
+    x3_fore_right = x3_fore_right * temp;
+    x3_fore_right = x2_fore_right + x3_fore_right;
+    float y3_fore_right = sin(fore_right_theta + PI - fore_right_phi);
+    y3_fore_right = y3_fore_right * RADIUS;
+    y3_fore_right = y2_fore_right + y3_fore_right;
+    float z3_fore_right = cos(fore_right_theta + PI - fore_right_phi);
+    z3_fore_right = z3_fore_right * RADIUS;
+    temp = sin(fore_right_alpha);
+    z3_fore_right = z3_fore_right * temp;
+    z3_fore_right = z2_fore_right + z3_fore_right;
+    // Then the fore left leg
+    float x3_fore_left = cos(fore_left_theta - fore_left_phi);
+    x3_fore_left = x3_fore_left * RADIUS;
+    temp = cos(fore_left_alpha + PI);           //Add PI to invert to get the sign correct
+    x3_fore_left = x3_fore_left * temp;
+    x3_fore_left = x2_fore_left + x3_fore_left;
+    float y3_fore_left = sin(fore_left_theta + PI - fore_left_phi);
+    y3_fore_left = y3_fore_left * RADIUS;
+    y3_fore_left = y2_fore_left + y3_fore_left;
+    float z3_fore_left = cos(fore_left_theta + PI - fore_left_phi);
+    z3_fore_left = z3_fore_left * RADIUS;
+    temp = sin(fore_left_alpha);
+    z3_fore_left = z3_fore_left * temp;
+    z3_fore_left = z2_fore_left + z3_fore_left;    
+    // Then the hind right leg..
+    float x3_hind_right = cos(hind_right_theta - hind_right_phi);
+    x3_hind_right = x3_hind_right * TIBIA;
+    temp = cos(hind_right_alpha);
+    x3_hind_right = x3_hind_right * temp;
+    x3_hind_right = x2_fore_right + x3_hind_right;
+    float y3_hind_right = sin(hind_right_theta + PI - hind_right_phi);
+    y3_hind_right = y3_hind_right * TIBIA;
+    y3_hind_right = y2_hind_right + y3_hind_right;
+    float z3_hind_right = cos(hind_right_theta + PI - hind_right_phi);
+    z3_hind_right = z3_hind_right * TIBIA;
+    temp = sin(hind_right_alpha);
+    z3_hind_right = z3_hind_right * temp;
+    z3_hind_right = z2_hind_right + z3_hind_right;
+    // Finally the hind left leg
+    float x3_hind_left = cos(hind_left_theta - hind_left_phi);
+    x3_hind_left = x3_hind_left * TIBIA;
+    temp = cos(hind_left_alpha + PI);           //Add PI to invert to get the sign correct
+    x3_hind_left = x3_hind_left * temp;
+    x3_hind_left = x2_hind_left + x3_hind_left;
+    float y3_hind_left = sin(hind_left_theta + PI - hind_left_phi);
+    y3_hind_left = y3_hind_left * TIBIA;
+    y3_hind_left = y2_hind_left + y3_hind_left;
+    float z3_hind_left = cos(hind_left_theta + PI - hind_left_phi);
+    z3_hind_left = z3_hind_left * TIBIA;
+    temp = sin(hind_left_alpha);
+    z3_hind_left = z3_hind_left * temp;
+    z3_hind_left = z2_hind_left + z3_hind_left;  
+    
+    
+    // Now updtae the array...   
+    pos[0] = x3_fore_right;
+    pos[1] = y3_fore_right;
+    pos[2] = z3_fore_right;
+    pos[3] = x3_fore_left;
+    pos[4] = y3_fore_left;
+    pos[5] = z3_fore_left;
+    pos[6] = x3_hind_right;
+    pos[7] = y3_hind_right;
+    pos[8] = z3_hind_right;
+    pos[9] = x3_hind_left;
+    pos[10] = y3_hind_left;
+    pos[11] = z3_hind_left;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Globals.cpp	Sun Oct 16 16:01:08 2011 +0000
@@ -0,0 +1,29 @@
+#include "FK.h"
+     
+/****************************************************
+Global AX-12+ Servos
+****************************************************/
+    AX12 Hind_Right_Hip_Lateral(p28, p27, p29, 1),
+         Hind_Right_Hip(p28, p27, p29, 2),
+         Hind_Right_Knee(p28, p27, p29, 3),
+         Hind_Left_Hip_Lateral(p28, p27, p29, 4),
+         Hind_Left_Hip(p28, p27, p29, 5),
+         Hind_Left_Knee(p28, p27, p29, 6),
+         Fore_Right_Hip_Lateral(p28, p27, p29, 7),
+         Fore_Right_Hip(p28, p27, p29, 8),
+         Fore_Right_Knee(p28, p27, p29, 9),
+         Fore_Left_Hip_Lateral(p28, p27, p29, 10),
+         Fore_Left_Hip(p28, p27, p29, 11),
+         Fore_Left_Knee(p28, p27, p29, 12);
+         
+/***********************************************************************************
+Poses - Target servo position data
+format: Right fore leg XYZ, Left fore leg XYZ, Right hind leg XYZ, Left hind leg XYZ
+************************************************************************************/
+//                   Right fore leg| Left fore leg|Right hind leg| Left hind
+//                     X    Y    Z |  X   Y    Z  |  X   Y    Z  |  X    Y    Z
+const int Sit1[]  = {  70, -80,-190, -70, -80,-190,  70,  60,-130, -70,  60,-130};
+const int Sit2[]  = {  70, -80,-190, -70, -80,-190, 150,  60,-130,-150,  60,-130};
+const int Sit3[]  = {  70, -80,-190, -70, -80,-190, 155,  20, -25,-150,  20, -25};
+const int Sit4[]  = {  70,  80,-125, -70,  80,-125, 150, -10, -30,-150, -10, -30};
+const int Stand[] = {  70,  50,-200, -70,  50,-200, 150, -50,-180,-150, -50,-180};

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IK_Engine.cpp	Sun Oct 16 16:01:08 2011 +0000
@@ -0,0 +1,89 @@
+/********************************************************************************
+ This function finds E.R.I.C.'s joint angles for a given foot position
+ Notebook: http://mbed.org/users/ms523/notebook/eric-leg-inverse-kinematics/
+********************************************************************************/
+#include "FK.h"
+
+Joint_Angles IK_Engine(int X, int Y, int Z, char legs){
+
+    // Adjust X to account for the X-axis offset
+        if(X > 0)
+            X = X - X_OFFSET;
+        else
+            X = X + X_OFFSET;     
+            
+    // Calculate Leg length
+        float A = X*X+Z*Z-B*B;
+        A = sqrt(A);
+        
+    // Calculate beta
+        float beta = A/B;
+        beta = atan(beta);
+        beta = beta * 180 / PI;
+        
+    // Calculate C
+        float C = X*X+Z*Z;
+        C = sqrt(C);
+         
+    // Calculate gamma
+        float gamma = (float)Z / C;
+        gamma = acos(gamma);
+        gamma = gamma * 180 / PI;
+        
+    // Calculate alpha (i.e. lateral servo angle)
+        float alpha = gamma - beta;
+
+    // Calculate the length of the leg
+        float D = A*A+Y*Y;
+        D = sqrt(D);
+
+        // Work out phi (i.e. knee servo angle)
+        float phi;
+        if(legs){           // Fore legs selected
+            phi = ULNA*ULNA+RADIUS*RADIUS-D*D;
+            phi = phi / (2*ULNA*RADIUS);
+            phi = acos(phi);
+            phi = phi * 180 / PI;
+            phi = 360 - phi;            // Because hind legs bend backwards
+        }
+        else{               // Hind legs selected
+            phi = FEMUR*FEMUR+TIBIA*TIBIA-D*D;
+            phi = phi / (2*FEMUR*TIBIA);
+            phi = acos(phi);
+            phi = phi * 180 / PI;
+        }
+
+    // Work out epsilon
+        float epsilon;
+        if(legs){           // Fore legs selected
+            epsilon = ULNA*ULNA-RADIUS*RADIUS+D*D;
+            epsilon = epsilon / (2*D*ULNA);
+        }
+        else{               // Hind legs selected
+            epsilon = FEMUR*FEMUR-TIBIA*TIBIA+D*D;
+            epsilon = epsilon / (2*D*FEMUR);
+        }
+        epsilon = acos(epsilon);
+        epsilon = epsilon * 180 / PI;
+        
+    // Work out delta
+        float delta = (float) Y/ A;                   
+        delta = atan(delta);
+        delta = delta * 180 / PI;
+        
+    // Work out theta (i.e. hip servo angle)
+        float theta;
+        if(legs){           // Fore legs selected
+            theta = 180 - delta + epsilon;      // Because fore legs bend backwards
+        }
+        else{               // Hind legs selected
+            theta = 180 - delta - epsilon;
+        }
+        
+    // Return calculated joint angles   
+        Joint_Angles angles;    // Local variable 
+        angles.lateral = alpha;
+        angles.hip = theta;
+        angles.knee = phi;
+        return(angles);   
+ }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Set_Pose.cpp	Sun Oct 16 16:01:08 2011 +0000
@@ -0,0 +1,86 @@
+/**********************************************************************
+ This function puts E.R.I.C. into a standard position
+ regardless of E.R.I.C.'s initial positioning
+ Notebook: http://mbed.org/users/ms523/notebook/eric-initialisation-/
+***********************************************************************/
+#include "FK.h"
+
+void Set_Pose(const int *pose, float duration) {
+    
+    float StartPos[12];         // Array to hold current servo pos
+    FK_Engine(StartPos);        // Get the initial foot positions...
+    
+    // Apply global offsets to allow for hip width
+    StartPos[0] = StartPos[0] + X_OFFSET;       // Fore right leg
+    StartPos[3] = StartPos[3] - X_OFFSET;       // Fore left leg
+    StartPos[6] = StartPos[6] + X_OFFSET;       // Hind right leg
+    StartPos[9] = StartPos[9] - X_OFFSET;       // Hind left leg
+    
+    // Create arrays to hold each servo's distance, velocity and set point
+    int distance[12];
+    float velocity[12];
+    int set_point[12];
+
+    // Calculate the distance to travel in each axis...
+    // Equation: Distance = End Position - Start Position
+    for(int i=0;i<12;i++){
+        distance[i] = pose[i] - StartPos[i];        // Calculate the move distance
+        velocity[i] = distance[i] / duration;       // Calculate the move velocity
+    }
+
+    // Create a timer to control the movement
+    Timer Movement_timer;
+
+    // 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
+
+    Movement_timer.start();                             // Start the movement timer
+    float time = 0;                                     // Create a variable to read the time
+
+    while (time < duration) {
+        // Get the current time
+        time = Movement_timer.read();
+        if (time > duration)
+            time = duration;
+
+        // Calculate current set points
+        for(int i=0;i<12;i++){
+            set_point[i] = (int)(time * velocity[i]) + StartPos[i];
+        }
+   
+        // Calculate the required joint angles...
+        Fore_right = IK_Engine(set_point[0],set_point[1],set_point[2],FORE);
+        Fore_left = IK_Engine(set_point[3],set_point[4],set_point[5],FORE);
+        Hind_right = IK_Engine(set_point[6],set_point[7],set_point[8],HIND);
+        Hind_left = IK_Engine(set_point[9],set_point[10],set_point[11],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

--- /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

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Oct 16 16:01:08 2011 +0000
@@ -0,0 +1,12 @@
+#include "FK.h"
+
+Serial pc(USBTX,USBRX);
+Timer t;
+
+int main() {
+    
+    Step_Forward(100);
+    Step_Forward(100);
+    Step_Forward(100);
+   
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun Oct 16 16:01:08 2011 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912