Ted Parrott
/
InverseKinematics2
NA
Fork of InverseKinematics2 by
GetAngles.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 | LegAngles GetAngles(int X, int Y) { |
ms523 | 0:fdc9474c08b9 | 4 | // Begin by working out L |
ms523 | 0:fdc9474c08b9 | 5 | float L = (X*X) + (Y*Y); |
ms523 | 0:fdc9474c08b9 | 6 | L = sqrt(L); |
ms523 | 0:fdc9474c08b9 | 7 | |
ms523 | 0:fdc9474c08b9 | 8 | // Work out the Knee angle |
ms523 | 0:fdc9474c08b9 | 9 | float Knee = (FEMUR*FEMUR)+(TIBIA*TIBIA)-(L*L); |
ms523 | 0:fdc9474c08b9 | 10 | Knee = Knee / (2*FEMUR*TIBIA); |
ms523 | 0:fdc9474c08b9 | 11 | Knee = acos(Knee); |
ms523 | 0:fdc9474c08b9 | 12 | |
ms523 | 0:fdc9474c08b9 | 13 | // Work out Alpha |
ms523 | 0:fdc9474c08b9 | 14 | float Alpha = (FEMUR*FEMUR)+(L*L)-(TIBIA*TIBIA); |
ms523 | 0:fdc9474c08b9 | 15 | Alpha = Alpha / (2*FEMUR*L); |
ms523 | 0:fdc9474c08b9 | 16 | Alpha = acos(Alpha); |
ms523 | 0:fdc9474c08b9 | 17 | |
ms523 | 0:fdc9474c08b9 | 18 | // Work out Beta |
ms523 | 0:fdc9474c08b9 | 19 | float Beta = (float) X/(float) Y; |
ms523 | 0:fdc9474c08b9 | 20 | Beta = atan(Beta); |
ms523 | 0:fdc9474c08b9 | 21 | |
ms523 | 0:fdc9474c08b9 | 22 | // Finally work out the Hip angle |
ms523 | 0:fdc9474c08b9 | 23 | float Hip = PI - Alpha - Beta; |
ms523 | 0:fdc9474c08b9 | 24 | |
ms523 | 0:fdc9474c08b9 | 25 | // Convert the angles to degrees... |
ms523 | 0:fdc9474c08b9 | 26 | LegAngles Angles; |
ms523 | 0:fdc9474c08b9 | 27 | Angles.Knee = Knee * 180 / PI - 30; |
ms523 | 0:fdc9474c08b9 | 28 | Angles.Hip = 360 - (Hip * 180 / PI) - 30; |
ms523 | 0:fdc9474c08b9 | 29 | |
ms523 | 0:fdc9474c08b9 | 30 | // Return the hip and knee angles... |
ms523 | 0:fdc9474c08b9 | 31 | return(Angles); |
ms523 | 0:fdc9474c08b9 | 32 | } |