Ted Parrott
/
InverseKinematics2
NA
Fork of InverseKinematics2 by
Diff: main.cpp
- Revision:
- 0:fdc9474c08b9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Jan 28 19:45:20 2011 +0000 @@ -0,0 +1,32 @@ +#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); + } +} \ No newline at end of file