Ted Parrott / Mbed 2 deprecated InverseKinematics2

NA

Dependencies:   AX12 mbed

Fork of InverseKinematics2 by Martin Smith

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?

UserRevisionLine numberNew 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 }