File content as of revision 0:8a7c1e92d067:

#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);        
    }
}