Revision 10:d78f5d556cf7, committed 2018-10-31

Comitter:

Jopper0575

Date:

Wed Oct 31 08:12:31 2018 +0000

Parent:

9:466dff9ae128

Commit message:

Foward & Backward kinematics

Changed in this revision

--- a/main.cpp	Fri Oct 05 12:51:06 2018 +0000
+++ b/main.cpp	Wed Oct 31 08:12:31 2018 +0000
@@ -29,6 +29,7 @@
 const float sample_time = 0.05; //s
 const float sample_frequency = 200; //hz
 
+
 double countstoangle(int counts){
     double angle;
     int counts_abs = abs(counts);
@@ -42,6 +43,50 @@
     }
 }
 
+
+// KINEMATICS
+// Variables
+const int L1 = 300; // Length arm 1 in mm
+const int L2 = 482; // Length arm 2 in mm
+
+// Gets current postition of end effector | FOWARD KINEMATICS
+double getX1pos(double q1){ // X-pos of joint
+    return x1_c = L1*cosd(q1);
+}
+double getY1pos(double q1){ // Y-pos of joint
+    return y1_c = L1*sind(q1);
+}
+double getX2pos(double q1, double q2){ // X-pos of end-effector
+    double x1_c = getX1pos(q1)
+    return x2_c = x1_c + L2*cosd(q1+q2);
+}
+double getY2pos(double q1, double q2){ // Y-pos of end-effector
+    double y1_c = getY1pos(q1)
+    return y2_c = y1_c + L2*sind(q1+q2);
+}
+
+// Calculate new position | BACKWARD KINEMATICS
+double getNewq2(double x2_d, double y2_d){ // requires desired x and y position of end-effector
+    double frac = ( pow(x2_d,2) + pow(y2_d,2) - pow(L1,2) - pow(L2,2) ) / ( 2 * L1 * L2 ); // define kinematic cosine fraction for easier calculation
+    double q2_d = atan2d( -sqrt(1 - pow(frac,2)) , frac ); // new desired q2 joint angle
+    return q2_d;
+}
+double getNewq1(double x2_d, double y2_d){ // requires desired x and y position of end-effector
+    double q2_d = getNewq2(x2_d, y2_d); // first calculate q2
+    double k1 = L1 + L2*cosd(q2_d); // kinematic relation 1
+    double k2 = L2*sind(q2_d); // kinematic relation 2
+    double q1_d = atan2d( y2_d, x2_d ) - atan2d( k2, k1); // new desired q1 joint angle
+    return q1_d;
+}
+double getNewq3(double x2_d, double y2_d){ // Cup holder
+    double q3_d = getNewq2(x2_d, y2_d) + getNewq1(x2_d, y2_d); // q2_d+q1_d
+}
+
+
+
+
+
+
 int main()
 {
     ledr = 1;