Diff: help_functions/kinematics.h

Revision:

8:bba05e863b68

Parent:

7:b77f2201b156

Child:

9:8e1112874c12

--- a/help_functions/kinematics.h	Tue Oct 23 05:28:32 2018 +0000
+++ b/help_functions/kinematics.h	Tue Oct 23 05:54:48 2018 +0000
@@ -16,6 +16,15 @@
     // pseudo inverse jacobian to get joint speeds
     // input are desired vx and vy of end effector, output joint angle speeds
     // I assume here that reference is a vector (this should also be global I think)
+    // I also assume that the end effector position is global here and is denoted by x
+    // Assuming the ticker_rate is also set to be global
+    
+    double des_twist[2]; // the desired twist (DETERMINE IF WE WANT TO MAKE THIS STATIC)
+    double q1_star_des; // desired joint velocity of q1_star
+    double q2_star_des; // same as above but then for q2_star
+    
+    q1_star_des = 1/(L1*(-x*sin(q1)-(y+y01)*cos(q1)))*(-1*(-x+L1*cos(q1))*des_twist[0]-x*des_twist[1]);
+    q2_star_des = 1/(L1*(-x*sin(q1)-(y+y01)*cos(q1)))*(-1*(-y+y01+L1*sin(q1))*des_twist[0]+1*(-y+y01))*des_twist[1]);
     
     
     return 5.5;