Ted Parrott
/
InverseKinematics2
NA
Fork of InverseKinematics2 by
main.cpp@0:fdc9474c08b9, 2011-01-28 (annotated)
- Committer:
- ms523
- Date:
- Fri Jan 28 19:45:20 2011 +0000
- Revision:
- 0:fdc9474c08b9
First test of a robotic leg
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ms523 | 0:fdc9474c08b9 | 1 | #include "ServoAngles.h" |
ms523 | 0:fdc9474c08b9 | 2 | |
ms523 | 0:fdc9474c08b9 | 3 | int main() { |
ms523 | 0:fdc9474c08b9 | 4 | AX12 AX12_Hip(p9, p10, 1), AX12_Knee(p9, p10, 2); |
ms523 | 0:fdc9474c08b9 | 5 | Timer t; |
ms523 | 0:fdc9474c08b9 | 6 | float Time; |
ms523 | 0:fdc9474c08b9 | 7 | int x,y; // x & y positions for the foot |
ms523 | 0:fdc9474c08b9 | 8 | LegAngles Answer; // Struct containing joint angles |
ms523 | 0:fdc9474c08b9 | 9 | |
ms523 | 0:fdc9474c08b9 | 10 | t.start(); |
ms523 | 0:fdc9474c08b9 | 11 | while (1) { |
ms523 | 0:fdc9474c08b9 | 12 | Time = t.read(); |
ms523 | 0:fdc9474c08b9 | 13 | if (Time>=2){ // Work on a 4 second cycle |
ms523 | 0:fdc9474c08b9 | 14 | t.reset(); |
ms523 | 0:fdc9474c08b9 | 15 | Time = 0; // Reset Time |
ms523 | 0:fdc9474c08b9 | 16 | } |
ms523 | 0:fdc9474c08b9 | 17 | if (Time<=1) { // Leg on it's forward stroke |
ms523 | 0:fdc9474c08b9 | 18 | x = (200 * Time) - 100; |
ms523 | 0:fdc9474c08b9 | 19 | y = 200; |
ms523 | 0:fdc9474c08b9 | 20 | } else { // Leg on it's backward stroke |
ms523 | 0:fdc9474c08b9 | 21 | x = (200 * (2 - Time)) - 100; |
ms523 | 0:fdc9474c08b9 | 22 | y = 195; |
ms523 | 0:fdc9474c08b9 | 23 | } |
ms523 | 0:fdc9474c08b9 | 24 | Answer = GetAngles(x,y); // Work out the joint angles |
ms523 | 0:fdc9474c08b9 | 25 | |
ms523 | 0:fdc9474c08b9 | 26 | // Print the answers to terminal to debug |
ms523 | 0:fdc9474c08b9 | 27 | PC.printf("\n\rX = %d, Hip = %.2f, Knee = %.2f",x,Answer.Hip,Answer.Knee); |
ms523 | 0:fdc9474c08b9 | 28 | |
ms523 | 0:fdc9474c08b9 | 29 | AX12_Hip.SetGoal(Answer.Hip); |
ms523 | 0:fdc9474c08b9 | 30 | AX12_Knee.SetGoal(Answer.Knee); |
ms523 | 0:fdc9474c08b9 | 31 | } |
ms523 | 0:fdc9474c08b9 | 32 | } |