Diff: RKI/InverseKinematics.cpp

Revision:

2:703501924009

--- /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