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@17:1f93c83e211f, 2018-10-25 (annotated)

Committer:
MaikOvermars
Date:
Thu Oct 25 22:01:12 2018 +0000
Branch:
bla
Revision:
17:1f93c83e211f
Parent:
16:0280a604cf7e
Child:
22:31065a83d9e8
Some small bugfixes in kinematics and PID_controller and main. Added the emg processing file. In the main file the processed signals are transformed into velocities.

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 14:4744cc6c90f4 8 void forwardkinematics_function(double& q1, double& q2, double& x, double& y) {
MaikOvermars 0:4cb1de41d049 9 // input are joint angles, output are x and y position of end effector
MaikOvermars 0:4cb1de41d049 10
MaikOvermars 17:1f93c83e211f 11 x = x01 + L1*cos(q1) - L2*cos(q2);
MaikOvermars 17:1f93c83e211f 12 y = y01 + L1*sin(q1) - L2*sin(q2);
MaikOvermars 0:4cb1de41d049 13 }
MaikOvermars 0:4cb1de41d049 14
MaikOvermars 17:1f93c83e211f 15 void inversekinematics_function(double &x, double &y, const double &T, double &qref1, double &qref2, double &q1, double &q2, double &des_vx, double &des_vy) {
MaikOvermars 17:1f93c83e211f 16 // input is desired x and y velocity, output reference angles
MaikOvermars 17:1f93c83e211f 17 // reference angle spees are calculated using the inverse of jacobian
MaikOvermars 17:1f93c83e211f 18 // from the reference angle speeds the reference angles are computed
SvenD97 13:397b7c22475c 19
SvenD97 8:bba05e863b68 20 double q1_star_des; // desired joint velocity of q1_star
SvenD97 8:bba05e863b68 21 double q2_star_des; // same as above but then for q2_star
SvenD97 13:397b7c22475c 22
SvenD97 9:8e1112874c12 23 // The calculation below assumes that the end effector position is calculated before this function is executed
SvenD97 13:397b7c22475c 24 // In our case the determinant will not equal zero, hence no problems with singularies I think.
SvenD97 13:397b7c22475c 25 q1_star_des = 1/(L1*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-x+L1*cos(q1))*des_vx-x*des_vy);
MaikOvermars 16:0280a604cf7e 26 q2_star_des = 1/(L1*(-x*sin(q1)-(-y+y01)*cos(q1)))*(-1*(-y+y01+L1*sin(q1))*des_vx+1*(-y+y01)*des_vy);
SvenD97 7:b77f2201b156 27
SvenD97 13:397b7c22475c 28 qref1 = q1+T*q1_star_des; // Yet to adapt all these equations
SvenD97 13:397b7c22475c 29 qref2 = q2+T*(q2_star_des - q1_star_des);
MaikOvermars 0:4cb1de41d049 30 }