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@1:f63be2020475, 2018-10-26 (annotated)

Committer:
Ramonwaninge
Date:
Fri Oct 26 14:17:28 2018 +0000
Revision:
1:f63be2020475
Parent:
0:779fe292e912
Child:
2:0a7a3c0c08d3
Error!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ramonwaninge 0:779fe292e912 1 #include "mbed.h"
Ramonwaninge 1:f63be2020475 2 #include "math.h"
Ramonwaninge 0:779fe292e912 3 #define PI 3.14159265
Ramonwaninge 0:779fe292e912 4
Ramonwaninge 0:779fe292e912 5
Ramonwaninge 0:779fe292e912 6
Ramonwaninge 0:779fe292e912 7 //Joe dit zijn de inputsignalen
Ramonwaninge 0:779fe292e912 8 double theta1; double theta4; double emg1; double emg2; double emg3;
Ramonwaninge 0:779fe292e912 9 // Joe dit zijn de outputsignalen
Ramonwaninge 1:f63be2020475 10 double x; double y;
Ramonwaninge 1:f63be2020475 11
Ramonwaninge 0:779fe292e912 12 //Joe dit zijn de constantes
Ramonwaninge 1:f63be2020475 13 double ll = 200.0; double lu = 170.0; double lb = 10.0; double le = 79.0; double xbase = 350.0-lb;
Ramonwaninge 0:779fe292e912 14 //Joe dit zijn de Tickers
Ramonwaninge 1:f63be2020475 15 Ticker emgcheck;
Ramonwaninge 1:f63be2020475 16
Ramonwaninge 0:779fe292e912 17
Ramonwaninge 1:f63be2020475 18 //forward kinematics, Check mathematica!
Ramonwaninge 1:f63be2020475 19 double alfax = 1.0/2.0 *(lb + xbase + ll*(cos(theta1) - cos(theta4)) + (2.0*pow(-(pow(xbase, 2.0))/4 + lu^2 + 1/2*ll*(xbase*(cos(theta1) + cos(theta4)) - ll*(1 + cos(theta1 + theta4))),0.5)*
Ramonwaninge 1:f63be2020475 20 (-sin(theta1) + sin(theta4)))/sqrt((-(xbase/ll) + cos(theta1) + cos(theta4))^2 + (sin(theta1) - sin(theta4)^2)
Ramonwaninge 1:f63be2020475 21 double alfay = -le + ((-(xbase/ll) + cos(theta1) + cos(theta4))*sqrt(-(xbase^2/4) + lu^2 +
Ramonwaninge 1:f63be2020475 22 1/2 ll (xbase (cos(theta1) + cos(theta4)) - ll (1 + cos(theta1 + theta4)))))/ sqrt((-(
Ramonwaninge 1:f63be2020475 23 xbase/ll) + cos(theta1) + cos(theta4))^2 + (sin(theta1) - sin(theta4))^2) +
Ramonwaninge 1:f63be2020475 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 1:f63be2020475 32 //default = als x = xbase/2... break
Ramonwaninge 0:779fe292e912 33 //end
Ramonwaninge 0:779fe292e912 34 }
Ramonwaninge 1:f63be2020475 35 void ycor(){
Ramonwaninge 1:f63be2020475 36 //als emg2 == voorbij treshold,
Ramonwaninge 1:f63be2020475 37 //double theta1 -> plus counts (emg*richting)
Ramonwaninge 1:f63be2020475 38 //double theta4 -> plus counts (emg*richting)
Ramonwaninge 1:f63be2020475 39 //reken y door
Ramonwaninge 1:f63be2020475 40 //default = als y = default... break
Ramonwaninge 1:f63be2020475 41 //end
Ramonwaninge 1:f63be2020475 42 }
Ramonwaninge 0:779fe292e912 43 void flip(){}
Ramonwaninge 0:779fe292e912 44
Ramonwaninge 0:779fe292e912 45 int main()
Ramonwaninge 0:779fe292e912 46 {
Ramonwaninge 0:779fe292e912 47 //default = theta1 = theta4 = pi/2
Ramonwaninge 0:779fe292e912 48 emgcheck.attach(xcor, 0.001);
Ramonwaninge 0:779fe292e912 49 emgcheck.attach(ycor, 0.001);
Ramonwaninge 0:779fe292e912 50 emgcheck.attach(flip, 0.001);
Ramonwaninge 0:779fe292e912 51
Ramonwaninge 0:779fe292e912 52
Ramonwaninge 0:779fe292e912 53
Ramonwaninge 0:779fe292e912 54 }