Martin Smith / Mbed 2 deprecated InverseKinematics2

Dependencies:   mbed

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?

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