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@8:bba05e863b68, 2018-10-23 (annotated)

Committer:
SvenD97
Date:
Tue Oct 23 05:54:48 2018 +0000
Revision:
8:bba05e863b68
Parent:
7:b77f2201b156
Child:
9:8e1112874c12
Made a constant double T global that indicates the time between two calculation loops (needed for inverse kinematics);

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 8:bba05e863b68 19 // I also assume that the end effector position is global here and is denoted by x
SvenD97 8:bba05e863b68 20 // Assuming the ticker_rate is also set to be global
SvenD97 8:bba05e863b68 21
SvenD97 8:bba05e863b68 22 double des_twist[2]; // the desired twist (DETERMINE IF WE WANT TO MAKE THIS STATIC)
SvenD97 8:bba05e863b68 23 double q1_star_des; // desired joint velocity of q1_star
SvenD97 8:bba05e863b68 24 double q2_star_des; // same as above but then for q2_star
SvenD97 8:bba05e863b68 25
SvenD97 8:bba05e863b68 26 q1_star_des = 1/(L1*(-x*sin(q1)-(y+y01)*cos(q1)))*(-1*(-x+L1*cos(q1))*des_twist[0]-x*des_twist[1]);
SvenD97 8:bba05e863b68 27 q2_star_des = 1/(L1*(-x*sin(q1)-(y+y01)*cos(q1)))*(-1*(-y+y01+L1*sin(q1))*des_twist[0]+1*(-y+y01))*des_twist[1]);
SvenD97 7:b77f2201b156 28
MaikOvermars 0:4cb1de41d049 29
MaikOvermars 0:4cb1de41d049 30 return 5.5;
MaikOvermars 0:4cb1de41d049 31 }