Diff: help_functions/kinematics.h

Branch:

bla

Revision:

17:1f93c83e211f

Parent:

16:0280a604cf7e

Child:

22:31065a83d9e8

--- a/help_functions/kinematics.h	Thu Oct 25 17:50:26 2018 +0000
+++ b/help_functions/kinematics.h	Thu Oct 25 22:01:12 2018 +0000
@@ -8,13 +8,14 @@
 void forwardkinematics_function(double& q1, double& q2, double& x, double& y) {
     // input are joint angles, output are x and y position of end effector
     
-    x = x01 + L1*cos(q1)-L2*cos(q2);
-    y = y01 + L1 * sin(q1) - L2 * sin(q2);    
+    x = x01 + L1*cos(q1) - L2*cos(q2);
+    y = y01 + L1*sin(q1) - L2*sin(q2);    
 }
 
-void inversekinematics_function(double& x, double& y, const double& T, double& qref1, double& qref2, double& q1, double& q2, double& des_vx, double& des_vy) {
-    // pseudo inverse jacobian to get joint speeds
-    // input are desired vx and vy of end effector, output are reference angles
+void inversekinematics_function(double &x, double &y, const double &T, double &qref1, double &qref2, double &q1, double &q2, double &des_vx, double &des_vy) {
+    // input is desired x and y velocity, output reference angles
+    // reference angle spees are calculated using the inverse of jacobian
+    // from the reference angle speeds the reference angles are computed
 
     double q1_star_des; // desired joint velocity of q1_star
     double q2_star_des; // same as above but then for q2_star