Eva Krolis / Mbed 2 deprecated Kinematics

Good Jacobian and code Not been tested

Dependencies:   MODSERIAL biquadFilter mbed

Fork of Kinematics by Ramon Waninge

main.cpp@0:779fe292e912, 2018-10-26 (annotated)

Committer:
Ramonwaninge
Date:
Fri Oct 26 12:10:12 2018 +0000
Revision:
0:779fe292e912
Child:
1:f63be2020475
Forward kinematics implemented, tickers attached, xcoordinate code typed in text. NOT TESTED YET.;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ramonwaninge 0:779fe292e912 1 #include "mbed.h"
Ramonwaninge 0:779fe292e912 2 #define PI 3.14159265
Ramonwaninge 0:779fe292e912 3
Ramonwaninge 0:779fe292e912 4
Ramonwaninge 0:779fe292e912 5
Ramonwaninge 0:779fe292e912 6 //Joe dit zijn de inputsignalen
Ramonwaninge 0:779fe292e912 7 double theta1; double theta4; double emg1; double emg2; double emg3;
Ramonwaninge 0:779fe292e912 8 // Joe dit zijn de outputsignalen
Ramonwaninge 0:779fe292e912 9 double x; double y;
Ramonwaninge 0:779fe292e912 10 //Joe dit zijn de constantes
Ramonwaninge 0:779fe292e912 11 double ll; double lu; double lb; double lend;
Ramonwaninge 0:779fe292e912 12 //Joe dit zijn de Tickers
Ramonwaninge 0:779fe292e912 13 Ticker emgcheck
Ramonwaninge 0:779fe292e912 14
Ramonwaninge 0:779fe292e912 15 //forward kinematics
Ramonwaninge 0:779fe292e912 16 double alfax = 1/2 (lb + lix + ll (Cos[theta1] - Cos[theta4]) + (
Ramonwaninge 0:779fe292e912 17 2 Sqrt[-(lix^2/4) + lu^2 +
Ramonwaninge 0:779fe292e912 18 1/2 ll (lix (Cos[theta1] + Cos[theta4]) - ll (1 + Cos[theta1 + theta4]))] (-Sin[
Ramonwaninge 0:779fe292e912 19 theta1] + Sin[theta4]))/
Ramonwaninge 0:779fe292e912 20 Sqrt[(-(lix/ll) + Cos[theta1] + Cos[theta4])^2 + (Sin[theta1] - Sin[theta4)^2]
Ramonwaninge 0:779fe292e912 21 double alfay = -le + ((-(lix/ll) + Cos[theta1] + Cos[theta4]) Sqrt[-(lix^2/4) + lu^2 +
Ramonwaninge 0:779fe292e912 22 1/2 ll (lix (Cos[theta1] + Cos[theta4]) - ll (1 + Cos[theta1 + theta4]))])/Sqrt[(-(
Ramonwaninge 0:779fe292e912 23 lix/ll) + Cos[theta1] + Cos[theta4])^2 + (Sin[theta1] - Sin[theta4])^2] +
Ramonwaninge 0:779fe292e912 24 1/2 ll (Sin[theta1] + Sin[theta4])
Ramonwaninge 0:779fe292e912 25
Ramonwaninge 0:779fe292e912 26 //Joe, hieronder staan de functies die door de tickers aangeroepen worden
Ramonwaninge 0:779fe292e912 27 void xcor(){
Ramonwaninge 0:779fe292e912 28 //als emg1==voorbij treshold,
Ramonwaninge 0:779fe292e912 29 //double theta1-> plus een paar counts (emg*richting)
Ramonwaninge 0:779fe292e912 30 //double theta4-> plus een paar counts (emg*richting)
Ramonwaninge 0:779fe292e912 31 //reken x door
Ramonwaninge 0:779fe292e912 32 //end
Ramonwaninge 0:779fe292e912 33 }
Ramonwaninge 0:779fe292e912 34 void ycor(){}
Ramonwaninge 0:779fe292e912 35 void flip(){}
Ramonwaninge 0:779fe292e912 36
Ramonwaninge 0:779fe292e912 37 int main()
Ramonwaninge 0:779fe292e912 38 {
Ramonwaninge 0:779fe292e912 39 //default = theta1 = theta4 = pi/2
Ramonwaninge 0:779fe292e912 40 emgcheck.attach(xcor, 0.001);
Ramonwaninge 0:779fe292e912 41 emgcheck.attach(ycor, 0.001);
Ramonwaninge 0:779fe292e912 42 emgcheck.attach(flip, 0.001);
Ramonwaninge 0:779fe292e912 43
Ramonwaninge 0:779fe292e912 44
Ramonwaninge 0:779fe292e912 45
Ramonwaninge 0:779fe292e912 46 }