File content as of revision 0:0965dacb3caf:

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