File content as of revision 0:fdc9474c08b9:

#include "ServoAngles.h"

int main() {
    AX12 AX12_Hip(p9, p10, 1), AX12_Knee(p9, p10, 2);
    Timer t;
    float Time;
    int x,y;                            // x & y positions for the foot
    LegAngles Answer;                   // Struct containing joint angles

    t.start();
    while (1) {
        Time = t.read();
        if (Time>=2){                   // Work on a 4 second cycle
            t.reset();
            Time = 0;                   // Reset Time
        }
        if (Time<=1) {                // Leg on it's forward stroke
            x = (200 * Time) - 100;
            y = 200;
        } else {                         // Leg on it's backward stroke
            x = (200 * (2 - Time)) - 100;
            y = 195;
        }
        Answer = GetAngles(x,y);       // Work out the joint angles
        
        // Print the answers to terminal to debug
        PC.printf("\n\rX = %d, Hip = %.2f, Knee = %.2f",x,Answer.Hip,Answer.Knee);

        AX12_Hip.SetGoal(Answer.Hip);
        AX12_Knee.SetGoal(Answer.Knee);
    }
}