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