![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Test voor Inverse Kinematics (afgeleid van schrift berekeningen)
Dependencies: mbed QEI HIDScope biquadFilter MODSERIAL FastPWM
Diff: RKI/InverseKinematics.cpp
- Revision:
- 2:703501924009
diff -r b862262a9d14 -r 703501924009 RKI/InverseKinematics.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/RKI/InverseKinematics.cpp Fri Oct 18 12:07:54 2019 +0000 @@ -0,0 +1,53 @@ +#include "...." +#include "math.h" + +int Theta1(float Angle) +{ + int Compare; + int min_comp=1450; + int max_comp=6500; + int min_angle=0; + int max_angle= 180; + Compare=((max_comp-min_comp)/(max_angle-min_angle))*(Angle-min_angle)+ + return Compare; +} + +int Theta2(float Angle) +{ + int Compare; + int min_comp=1500; + int max_comp=6900; + int min_angle=-90; + int max_angle= 90; + Compare=((max_comp-min_comp)/(max_angle-min_angle))*(Angle-min_angle)+ + return Compare; +} + +int main(void) +{ + float X=1.0; //desired X position of the end effector in cm + float Y=1.0; //desired Y position of the end effector in cm + float r1=0.0; + float phi1=0.0; + float phi2=0.0; + float phi3=0.0; + float a2=6.0; + float a4=5.5: + float T1=0.0; //T1 is theta 1 in radians + float T2=0.0; //T2 is theta 2 in radians + + r1=sqrt(X*X+Y*Y); //eguation 1 + phi1=acos(((a4*a4)-(a2*a2)-(r1*r1))/(-2.0*a2*r1)); //Equation2 + phi2=atan(Y/X); //Eguation 3 + T1=phi2-phi1; + phi3=acos(((r1*r1)-(a2*a2)-(a4*a4))/(-2.0*a2*a4)); //Equation 5 + T2=3.14159-phi3; //Equation 6 + + PWM_1_Start(); + for(;;) + { + PWM_1_WriteCompare1(Theta1((T1/3.14159)*180.0)); + PWM_1_WriteCompare2(Theta2((T2/3.14159)*180.0)); + CyDelay(2000); + + \ No newline at end of file