Diff: help_functions/kinematics.h

Revision:

9:8e1112874c12

Parent:

8:bba05e863b68

Child:

12:3c47c7b1d1d7

--- a/help_functions/kinematics.h	Tue Oct 23 05:54:48 2018 +0000
+++ b/help_functions/kinematics.h	Tue Oct 23 06:04:49 2018 +0000
@@ -12,7 +12,7 @@
     y = y01 + L1 * sin(q1) - L2 * sin(q2);    
 }
 
-double inversekinematics_function(double q1, double q2, double reference) {
+double inversekinematics_function() {
     // 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)
@@ -23,9 +23,12 @@
     double q1_star_des; // desired joint velocity of q1_star
     double q2_star_des; // same as above but then for q2_star
     
+    // We have to give a des_twist. If we do EMG we give a x and y velocity, so do we have to use the reference x and y position?
+    
+    // The calculation below assumes that the end effector position is calculated before this function is executed
     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;    
+    q_ref[1] = q_ref[1]+T*q1_star_des;
+    q_ref[2] = q_ref[2]+T*(q2_star_des - q1_star_des);    
 }
\ No newline at end of file