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:
bjonkheer
Date:
2018-10-26
Revision:
19:e1e18746d98d
Parent:
6:8ff9566c91e2
Child:
20:31876566d70f

File content as of revision 19:e1e18746d98d:

#include "mbed.h"

double L_ua = 0.5;          //upper arm
double L_la = 0.7;          //lower arm, attached to the end effector
double x01 = 0.0;       
double y01 = 0.2;           //height base joint

void forwardkinematics_function(double q1, double q2) {
    // input are joint angles, output are x and y position of end effector
    
    double x = x01 + L_ua * cos(q1) - L_la * cos(q2);
    double y = y01 + L_ua * sin(q1) - L_la * sin(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;    
}