Revision 1:26bd86929178, committed 2018-10-30

Comitter:

s1737619

Date:

Tue Oct 30 15:21:03 2018 +0000

Parent:

0:d1956779c04c

Commit message:

inverse kinematics

Changed in this revision

diff -r d1956779c04c -r 26bd86929178 main.cpp
--- a/main.cpp	Mon Oct 29 14:55:43 2018 +0000
+++ b/main.cpp	Tue Oct 30 15:21:03 2018 +0000
@@ -15,12 +15,24 @@
     X1 = L1 * cos(THETA1) + L2 * cos(THETA1 + THETA2); // gives x coordinates
     Y1 = L1 * sin(THETA1) + L2 * sin(THETA1 + THETA2); // gives y coordinates
     }*/
-
+/*
 void inverse(double X1, double Y1, double & THETA1, double & THETA2) //inverse kinematics
 {
 THETA1 =  (acos((pow(-L1,2.0)-pow(X1,2.0)-pow(Y1,2.0)+pow(L2,2.0))/(-2.0*L1*sqrt(pow(X1,2.0)+pow(Y1,2.0))))+atan(Y1/X1))*9.0; //gives theta M1 in radians
-THETA2 =  (acos((pow(X1,2.0)+ pow(Y1,2.0)-pow(L1,2.0)-pow(L2,2.0))/(-2.0*L1*L2))-3.1416)*4.0; //gives theta M2 in radians
+//THETA2 =  (acos((pow(X1,2.0)+ pow(Y1,2.0)-pow(L1,2.0)-pow(L2,2.0))/(-2.0*L1*L2))-3.1416)*4.0; //gives theta M2 in radians
+THETA2=  (acos(X1-L1*cos(THETA1))/(L2))*4.0; //gives theta M2 in radians
 }
+*/
+
+void inverse(double X1, double Y1, double & thetaM1, double & thetaM2)
+{
+    double L1 = 40.0; // %define length of arm 1 attached to gear
+    double L3 = sqrt(pow(X1,2.0)+pow(Y1,2.0));
+    double q3 = atan(Y1/X1);
+    double q4 = 2.0*asin(0.5*L3/L1);
+    thetaM1 = (0.5*3.1416-0.5*q4+q3)*9.0;
+    thetaM2 = (3.1416-thetaM1/9.0-q4)*4.0;
+    }
 
 
 double meas_pos1 () //Encoder functie