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
- Committer:
- MaikOvermars
- Date:
- 2018-10-22
- Revision:
- 1:ce487c9929dd
File content as of revision 1:ce487c9929dd:
#include "mbed.h"
double L1 = 0.5;
double L2 = 0.7;
double x01 = 0.0;
double y01 = 0.2;
double q1,q2,x,y;
float forwardkinematics_function(double motor_angle) {
// input are joint angles, output are x and y position of end effector
q1 = motor_angle;
q2 = motor_angle;
x = x01 - L1 * sin(q1) + L2 * cos(q1 + q2);
y = y01 + L1 * cos(q1) + L2 * sin(q1 + q2);
return x;
}
float inversekinematics_function(double reference) {
// pseudo inverse jacobian to get joint speeds
// input are desired vx and vy of end effector, output joint angle speeds
return 5.5;
}