Diff: main.cpp

Revision:

0:30f920202a5c

Child:

1:edac9d449caa

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 25 11:34:55 2018 +0000
@@ -0,0 +1,43 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include "math.h"
+
+const double pi = 3.14159265358979323846; //definition of pi
+int sq = 2; //to square numbers
+int ox = 0; //origin
+int oy = 0; //origin
+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
+const double L3 = 350.0; //length of the second link
+
+double x = 500.0; //x variable that will be set; input
+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))));
+
+//function to calculate the angle from 
+void makeAngle(double x, double y){
+
+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
+
+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 q2_motor = pi - q2; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
+
+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 q_diff[2] = (q1_diff,q2_diff); //make a vector so the answer can be returned
+
+return q_diff;
+}
+
+int main()
+{
+    while (true) {
+        
+    }
+}
\ No newline at end of file