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
Fork of
Robot_Software
by 
Bram Jonkheer
help_functions/kinematics.h@1:ce487c9929dd, 2018-10-22 (annotated)
- Committer:
- MaikOvermars
- Date:
- Mon Oct 22 11:10:41 2018 +0000
- Revision:
- 1:ce487c9929dd
Updated some code and started implementing kinematics.
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| MaikOvermars | 1:ce487c9929dd | 1 | #include "mbed.h" |
| MaikOvermars | 1:ce487c9929dd | 2 | |
| MaikOvermars | 1:ce487c9929dd | 3 | double L1 = 0.5; |
| MaikOvermars | 1:ce487c9929dd | 4 | double L2 = 0.7; |
| MaikOvermars | 1:ce487c9929dd | 5 | double x01 = 0.0; |
| MaikOvermars | 1:ce487c9929dd | 6 | double y01 = 0.2; |
| MaikOvermars | 1:ce487c9929dd | 7 | double q1,q2,x,y; |
| MaikOvermars | 1:ce487c9929dd | 8 | |
| MaikOvermars | 1:ce487c9929dd | 9 | float forwardkinematics_function(double motor_angle) { |
| MaikOvermars | 1:ce487c9929dd | 10 | // input are joint angles, output are x and y position of end effector |
| MaikOvermars | 1:ce487c9929dd | 11 | q1 = motor_angle; |
| MaikOvermars | 1:ce487c9929dd | 12 | q2 = motor_angle; |
| MaikOvermars | 1:ce487c9929dd | 13 | |
| MaikOvermars | 1:ce487c9929dd | 14 | x = x01 - L1 * sin(q1) + L2 * cos(q1 + q2); |
| MaikOvermars | 1:ce487c9929dd | 15 | y = y01 + L1 * cos(q1) + L2 * sin(q1 + q2); |
| MaikOvermars | 1:ce487c9929dd | 16 | return x; |
| MaikOvermars | 1:ce487c9929dd | 17 | } |
| MaikOvermars | 1:ce487c9929dd | 18 | |
| MaikOvermars | 1:ce487c9929dd | 19 | float inversekinematics_function(double reference) { |
| MaikOvermars | 1:ce487c9929dd | 20 | // pseudo inverse jacobian to get joint speeds |
| MaikOvermars | 1:ce487c9929dd | 21 | // input are desired vx and vy of end effector, output joint angle speeds |
| MaikOvermars | 1:ce487c9929dd | 22 | |
| MaikOvermars | 1:ce487c9929dd | 23 | return 5.5; |
| MaikOvermars | 1:ce487c9929dd | 24 | } |