Biorobotics / Robot-Software

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed Servo

Committer:
SvenD97
Date:
Mon Oct 22 19:13:37 2018 +0000
Revision:
6:8ff9566c91e2
Parent:
5:0dd66c757f24
Child:
7:b77f2201b156
Child:
19:e1e18746d98d
Changed type of forwardkinematics_function

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
MaikOvermars 0:4cb1de41d049 11 x = x01 - L1 * sin(q1) + L2 * cos(q1 + q2);
SvenD97 6:8ff9566c91e2 12 y = y01 + L1 * cos(q1) + L2 * sin(q1 + 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
MaikOvermars 0:4cb1de41d049 18
MaikOvermars 0:4cb1de41d049 19 return 5.5;
MaikOvermars 0:4cb1de41d049 20 }