Martin Smith
/
InverseKinematics2
main.cpp
- Committer:
- ms523
- Date:
- 2011-01-28
- Revision:
- 0:fdc9474c08b9
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); } }