Biorobotics / Robot-Software

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed Servo

Branch:
bla
Revision:
12:3c47c7b1d1d7
Parent:
9:8e1112874c12
Child:
13:397b7c22475c
--- a/help_functions/kinematics.h	Tue Oct 23 08:48:06 2018 +0000
+++ b/help_functions/kinematics.h	Tue Oct 23 10:47:19 2018 +0000
@@ -29,6 +29,6 @@
     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]);
     
-    q_ref[1] = q_ref[1]+T*q1_star_des;
-    q_ref[2] = q_ref[2]+T*(q2_star_des - q1_star_des);    
+    qref1 = qref1+T*q1_star_des; // Yet to adapt all these equations
+    qref2 = qref2+T*(q2_star_des - q1_star_des);    
 }
\ No newline at end of file