Isabella Waayer / Mbed 2 deprecated template_biorobotics_InverseKinematics

Test voor Inverse Kinematics (afgeleid van schrift berekeningen)

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

RKI/InverseKinematics.cpp@2:703501924009, 2019-10-18 (annotated)

Committer:
s1961438
Date:
Fri Oct 18 12:07:54 2019 +0000
Revision:
2:703501924009
Test_InverseKinematics

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1961438 2:703501924009 1 #include "...."
s1961438 2:703501924009 2 #include "math.h"
s1961438 2:703501924009 3
s1961438 2:703501924009 4 int Theta1(float Angle)
s1961438 2:703501924009 5 {
s1961438 2:703501924009 6 int Compare;
s1961438 2:703501924009 7 int min_comp=1450;
s1961438 2:703501924009 8 int max_comp=6500;
s1961438 2:703501924009 9 int min_angle=0;
s1961438 2:703501924009 10 int max_angle= 180;
s1961438 2:703501924009 11 Compare=((max_comp-min_comp)/(max_angle-min_angle))*(Angle-min_angle)+
s1961438 2:703501924009 12 return Compare;
s1961438 2:703501924009 13 }
s1961438 2:703501924009 14
s1961438 2:703501924009 15 int Theta2(float Angle)
s1961438 2:703501924009 16 {
s1961438 2:703501924009 17 int Compare;
s1961438 2:703501924009 18 int min_comp=1500;
s1961438 2:703501924009 19 int max_comp=6900;
s1961438 2:703501924009 20 int min_angle=-90;
s1961438 2:703501924009 21 int max_angle= 90;
s1961438 2:703501924009 22 Compare=((max_comp-min_comp)/(max_angle-min_angle))*(Angle-min_angle)+
s1961438 2:703501924009 23 return Compare;
s1961438 2:703501924009 24 }
s1961438 2:703501924009 25
s1961438 2:703501924009 26 int main(void)
s1961438 2:703501924009 27 {
s1961438 2:703501924009 28 float X=1.0; //desired X position of the end effector in cm
s1961438 2:703501924009 29 float Y=1.0; //desired Y position of the end effector in cm
s1961438 2:703501924009 30 float r1=0.0;
s1961438 2:703501924009 31 float phi1=0.0;
s1961438 2:703501924009 32 float phi2=0.0;
s1961438 2:703501924009 33 float phi3=0.0;
s1961438 2:703501924009 34 float a2=6.0;
s1961438 2:703501924009 35 float a4=5.5:
s1961438 2:703501924009 36 float T1=0.0; //T1 is theta 1 in radians
s1961438 2:703501924009 37 float T2=0.0; //T2 is theta 2 in radians
s1961438 2:703501924009 38
s1961438 2:703501924009 39 r1=sqrt(X*X+Y*Y); //eguation 1
s1961438 2:703501924009 40 phi1=acos(((a4*a4)-(a2*a2)-(r1*r1))/(-2.0*a2*r1)); //Equation2
s1961438 2:703501924009 41 phi2=atan(Y/X); //Eguation 3
s1961438 2:703501924009 42 T1=phi2-phi1;
s1961438 2:703501924009 43 phi3=acos(((r1*r1)-(a2*a2)-(a4*a4))/(-2.0*a2*a4)); //Equation 5
s1961438 2:703501924009 44 T2=3.14159-phi3; //Equation 6
s1961438 2:703501924009 45
s1961438 2:703501924009 46 PWM_1_Start();
s1961438 2:703501924009 47 for(;;)
s1961438 2:703501924009 48 {
s1961438 2:703501924009 49 PWM_1_WriteCompare1(Theta1((T1/3.14159)*180.0));
s1961438 2:703501924009 50 PWM_1_WriteCompare2(Theta2((T2/3.14159)*180.0));
s1961438 2:703501924009 51 CyDelay(2000);
s1961438 2:703501924009 52
s1961438 2:703501924009 53