Revision 1:edac9d449caa, committed 2018-10-29

Comitter:

Duif

Date:

Mon Oct 29 13:59:50 2018 +0000

Parent:

0:30f920202a5c

Commit message:

Inverse Kinematics. Change x and y to what should be the input

Changed in this revision

diff -r 30f920202a5c -r edac9d449caa main.cpp
--- a/main.cpp	Thu Oct 25 11:34:55 2018 +0000
+++ b/main.cpp	Mon Oct 29 13:59:50 2018 +0000
@@ -2,10 +2,10 @@
 #include "MODSERIAL.h"
 #include "math.h"
 
+double q1_diff;
+double q2_diff;
 const double pi = 3.14159265358979323846; //definition of pi
-int sq = 2; //to square numbers
-int ox = 0; //origin
-int oy = 0; //origin
+double sq = 2.0; //to square numbers
 const double x0 = 80.0; //zero x position after homing
 const double y0 = 141.0; //zero y position after homing
 const double L1 = 250.0; //length of the first link
@@ -15,24 +15,47 @@
 double y = 141.0; //y variable that will be set; input
 
 //reference angles of the starting position
-double q2_0 = -acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2*L1*L3));
-double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,2))/(2*L1*sqrt(pow(x0,2)+pow(y0,2))));
+double q2_0 = pi + 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 = q2_0 + q1_0;
 
 //function to calculate the angle from 
-void makeAngle(double x, double y){
+double makeAngleq1(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; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
 
-double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2*L1*L3)); //angle of the second joint in setpoint configuration
+q1_diff = -2.0*(q1-q1_0); //the actual amount of radians that the motor has to turn in total to reach the setpoint
+//q2_diff = 2.0*(q2_motor - q2_0); //the actual amount of radians that the motor has to turn in total to reach the setpoint
 
-double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
+//double q_diff[2] = {q1_diff,q2_diff}; //make a vector so the answer can be returned
+//q_diff[0] = q1_diff and q_diff[1] = q2_diff
+
+
+return q1_diff;
+}
 
-double q2_motor = pi - q2; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
+
+//function to calculate the angle from 
+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 q1_diff = 2*(q1-q1_0); //the actual amount of radians that the motor has to turn in total to reach the setpoint
-double q2_diff = 2*(q2_motor - q2_0); //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
+
+//q1_diff = 2.0*(q1-q1_0); //the actual amount of radians that the motor has to turn in total to reach the setpoint
+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
 
-double q_diff[2] = (q1_diff,q2_diff); //make a vector so the answer can be returned
+//double q_diff[2] = {q1_diff,q2_diff}; //make a vector so the answer can be returned
+//q_diff[0] = q1_diff and q_diff[1] = q2_diff
 
-return q_diff;
+
+return q2_diff;
 }
 
 int main()