Biorobotics / Robot-Software_jesse

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of Robot-Software by Biorobotics

help_functions/kinematics.h@7:b77f2201b156, 2018-10-23 (annotated)

Committer:
SvenD97
Date:
Tue Oct 23 05:28:32 2018 +0000
Revision:
7:b77f2201b156
Parent:
6:8ff9566c91e2
Child:
8:bba05e863b68
Changed line 33 from float to double;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MaikOvermars 0:4cb1de41d049 1 #include "mbed.h"
MaikOvermars 0:4cb1de41d049 2
MaikOvermars 0:4cb1de41d049 3 double L1 = 0.5;
MaikOvermars 0:4cb1de41d049 4 double L2 = 0.7;
MaikOvermars 0:4cb1de41d049 5 double x01 = 0.0;
MaikOvermars 0:4cb1de41d049 6 double y01 = 0.2;
MaikOvermars 0:4cb1de41d049 7
SvenD97 6:8ff9566c91e2 8 void forwardkinematics_function(double q1, double q2) {
MaikOvermars 0:4cb1de41d049 9 // input are joint angles, output are x and y position of end effector
MaikOvermars 0:4cb1de41d049 10
SvenD97 7:b77f2201b156 11 x = x01 + L1*cos(q1)-L2*cos(q2);
SvenD97 7:b77f2201b156 12 y = y01 + L1 * sin(q1) - L2 * sin(q2);
MaikOvermars 0:4cb1de41d049 13 }
MaikOvermars 0:4cb1de41d049 14
SvenD97 3:40e0e9404f82 15 double inversekinematics_function(double q1, double q2, double reference) {
MaikOvermars 0:4cb1de41d049 16 // pseudo inverse jacobian to get joint speeds
MaikOvermars 0:4cb1de41d049 17 // input are desired vx and vy of end effector, output joint angle speeds
SvenD97 7:b77f2201b156 18 // I assume here that reference is a vector (this should also be global I think)
SvenD97 7:b77f2201b156 19
MaikOvermars 0:4cb1de41d049 20
MaikOvermars 0:4cb1de41d049 21 return 5.5;
MaikOvermars 0:4cb1de41d049 22 }