Diff: main.cpp

Revision:

1:f628ab39b9f4

Parent:

0:f81ca7708f95

diff -r f81ca7708f95 -r f628ab39b9f4 main.cpp
--- a/main.cpp	Thu Nov 01 11:03:18 2018 +0000
+++ b/main.cpp	Thu Nov 01 13:17:13 2018 +0000
@@ -73,7 +73,7 @@
 // Reference angles of the starting position
 double q2_0 = -acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
 double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,sq))/(2.0*L1*sqrt(pow(x0,sq)+pow(y0,sq))));
-double q2_0_enc = (pi-q2_0) + q1_0;
+double q2_0_enc = (pi-q2_0) - q1_0;
 
 // --------------------------------------------------------------------
 // ---------------Read out encoders------------------------------------
@@ -168,8 +168,8 @@
 double makeAngleq2(double x, double y){
     double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); //angle of the second joint in setpoint configuration
     double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
-    double q2_motor = (pi - q2)+q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
-    q2_diff = -(2.0*(q2_motor - q2_0_enc))/(2.0*pi); //the actual amount of radians that the motor has to turn in total to reach the setpoint
+    double q2_motor = (pi - q2) - q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
+    q2_diff = (2.0*(q2_motor - q2_0_enc)); //the actual amount of radians that the motor has to turn in total to reach the setpoint
     return q2_diff;
 }