Biorobotics / Robot-Software

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed Servo

help_functions/kinematics.h@2:17ed767d8394, 2018-10-22 (annotated)

Committer:
SvenD97
Date:
Mon Oct 22 19:04:51 2018 +0000
Revision:
2:17ed767d8394
Parent:
0:4cb1de41d049
Child:
3:40e0e9404f82
Changed input of forwardkinematics_function to q1 and q2

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 double q1,q2,x,y;
MaikOvermars 0:4cb1de41d049 8
SvenD97 2:17ed767d8394 9 float forwardkinematics_function(double q1, double q2) {
MaikOvermars 0:4cb1de41d049 10 // input are joint angles, output are x and y position of end effector
MaikOvermars 0:4cb1de41d049 11
MaikOvermars 0:4cb1de41d049 12 x = x01 - L1 * sin(q1) + L2 * cos(q1 + q2);
MaikOvermars 0:4cb1de41d049 13 y = y01 + L1 * cos(q1) + L2 * sin(q1 + q2);
MaikOvermars 0:4cb1de41d049 14 return x;
MaikOvermars 0:4cb1de41d049 15 }
MaikOvermars 0:4cb1de41d049 16
MaikOvermars 0:4cb1de41d049 17 float inversekinematics_function(double reference) {
MaikOvermars 0:4cb1de41d049 18 // pseudo inverse jacobian to get joint speeds
MaikOvermars 0:4cb1de41d049 19 // input are desired vx and vy of end effector, output joint angle speeds
MaikOvermars 0:4cb1de41d049 20
MaikOvermars 0:4cb1de41d049 21 return 5.5;
MaikOvermars 0:4cb1de41d049 22 }