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

Committer:
SvenD97
Date:
2018-10-22
Revision:
6:8ff9566c91e2
Parent:
5:0dd66c757f24
Child:
7:b77f2201b156
Child:
19:e1e18746d98d

File content as of revision 6:8ff9566c91e2:

#include "mbed.h"

double L1 = 0.5;
double L2 = 0.7;
double x01 = 0.0;
double y01 = 0.2;

void forwardkinematics_function(double q1, double q2) {
    // input are joint angles, output are x and y position of end effector
    
    x = x01 - L1 * sin(q1) + L2 * cos(q1 + q2);
    y = y01 + L1 * cos(q1) + L2 * sin(q1 + q2);    
}

double inversekinematics_function(double q1, double q2, 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;    
}