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@3:de8d3ca44a3e, 2018-10-29 (annotated)

Committer:
Ramonwaninge
Date:
Mon Oct 29 15:01:52 2018 +0000
Revision:
3:de8d3ca44a3e
Parent:
2:0a7a3c0c08d3
Child:
4:49dfbfcd3577
Hij doet het bij benadering;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ramonwaninge 0:779fe292e912 1 #include "mbed.h"
Ramonwaninge 2:0a7a3c0c08d3 2 #include <math.h>
Ramonwaninge 2:0a7a3c0c08d3 3 #include <cmath>
Ramonwaninge 2:0a7a3c0c08d3 4 #include "MODSERIAL.h"
Ramonwaninge 0:779fe292e912 5 #define PI 3.14159265
Ramonwaninge 0:779fe292e912 6
Ramonwaninge 3:de8d3ca44a3e 7 MODSERIAL pc(USBTX, USBRX); // connecting to pc
Ramonwaninge 3:de8d3ca44a3e 8 DigitalIn button1(SW3);
Ramonwaninge 3:de8d3ca44a3e 9 DigitalIn button2(SW2);
Ramonwaninge 2:0a7a3c0c08d3 10 DigitalOut LED(LED1);
Ramonwaninge 0:779fe292e912 11
Ramonwaninge 0:779fe292e912 12 //Joe dit zijn de inputsignalen
Ramonwaninge 3:de8d3ca44a3e 13 double theta1 = PI*0.5; double theta4 = PI*0.5; double emg1; double emg2; double emg3;
Ramonwaninge 0:779fe292e912 14 // Joe dit zijn de outputsignalen
Ramonwaninge 1:f63be2020475 15 double x; double y;
Ramonwaninge 1:f63be2020475 16
Ramonwaninge 0:779fe292e912 17 //Joe dit zijn de constantes
Ramonwaninge 2:0a7a3c0c08d3 18 double ll = 200.0; double lu = 170.0; double lb = 10.0; double le = 79.0; double xbase = 340;
Ramonwaninge 0:779fe292e912 19 //Joe dit zijn de Tickers
Ramonwaninge 2:0a7a3c0c08d3 20
Ramonwaninge 2:0a7a3c0c08d3 21 //forward kinematics, Check mathematica! Omdat mbed in paniek raakt met meerdere wortels, hebben we de vergelijking opgedeeld in 3 stukken
Ramonwaninge 2:0a7a3c0c08d3 22 //First define the position equation of x
Ramonwaninge 2:0a7a3c0c08d3 23 double xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
Ramonwaninge 3:de8d3ca44a3e 24 double xendsqrt1 = 2*sqrt(-xbase*xbase/4 + lu*lu + ll*(xbase*(cos(theta1)+cos(theta4))/2) -ll*(1+ cos(theta1+theta4)))*(-sin(theta1)+sin(theta4));
Ramonwaninge 2:0a7a3c0c08d3 25 double xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
Ramonwaninge 2:0a7a3c0c08d3 26 double xend = (xendsum + xendsqrt1/xendsqrt2)/2;
Ramonwaninge 2:0a7a3c0c08d3 27 //Now define the pos. eq. of y
Ramonwaninge 3:de8d3ca44a3e 28 double yendsum = -le + ll/2*(sin(theta1)+sin(theta4));
Ramonwaninge 2:0a7a3c0c08d3 29 double yendsqrt1 = (-xbase/ll + cos(theta1)+cos(theta4))*sqrt(-xbase*xbase/4 + lu*lu + ll/2*(xbase*(cos(theta1)+cos(theta4))- ll*(1+cos(theta1+theta4))));
Ramonwaninge 3:de8d3ca44a3e 30 double yendsqrt2 = sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
Ramonwaninge 3:de8d3ca44a3e 31 double yend = (yendsum + yendsqrt1/yendsqrt2);
Ramonwaninge 2:0a7a3c0c08d3 32
Ramonwaninge 2:0a7a3c0c08d3 33
Ramonwaninge 2:0a7a3c0c08d3 34 //Hier definieren we de functies
Ramonwaninge 1:f63be2020475 35 Ticker emgcheck;
Ramonwaninge 1:f63be2020475 36
Ramonwaninge 0:779fe292e912 37
Ramonwaninge 0:779fe292e912 38 //Joe, hieronder staan de functies die door de tickers aangeroepen worden
Ramonwaninge 0:779fe292e912 39 void xcor(){
Ramonwaninge 3:de8d3ca44a3e 40 if (button1 == 0){
Ramonwaninge 2:0a7a3c0c08d3 41 //als emg1==voorbij treshold,
Ramonwaninge 3:de8d3ca44a3e 42 theta1 = PI*(theta1/PI + 0.1); //double theta1-> plus een paar counts (emg*richting)
Ramonwaninge 2:0a7a3c0c08d3 43 theta4 = PI*0.5; //double theta4-> plus een paar counts (emg*richting)
Ramonwaninge 2:0a7a3c0c08d3 44 xend = (xendsum + xendsqrt1/xendsqrt2)/2;
Ramonwaninge 1:f63be2020475 45 //default = als x = xbase/2... break
Ramonwaninge 0:779fe292e912 46 //end
Ramonwaninge 0:779fe292e912 47 }
Ramonwaninge 3:de8d3ca44a3e 48 }
Ramonwaninge 1:f63be2020475 49 void ycor(){
Ramonwaninge 1:f63be2020475 50 //als emg2 == voorbij treshold,
Ramonwaninge 1:f63be2020475 51 //double theta1 -> plus counts (emg*richting)
Ramonwaninge 1:f63be2020475 52 //double theta4 -> plus counts (emg*richting)
Ramonwaninge 1:f63be2020475 53 //reken y door
Ramonwaninge 1:f63be2020475 54 //default = als y = default... break
Ramonwaninge 1:f63be2020475 55 //end
Ramonwaninge 1:f63be2020475 56 }
Ramonwaninge 0:779fe292e912 57 void flip(){}
Ramonwaninge 0:779fe292e912 58
Ramonwaninge 0:779fe292e912 59 int main()
Ramonwaninge 0:779fe292e912 60 {
Ramonwaninge 2:0a7a3c0c08d3 61
Ramonwaninge 2:0a7a3c0c08d3 62 pc.baud(115200);
Ramonwaninge 0:779fe292e912 63 //default = theta1 = theta4 = pi/2
Ramonwaninge 0:779fe292e912 64 emgcheck.attach(xcor, 0.001);
Ramonwaninge 0:779fe292e912 65 emgcheck.attach(ycor, 0.001);
Ramonwaninge 0:779fe292e912 66 emgcheck.attach(flip, 0.001);
Ramonwaninge 3:de8d3ca44a3e 67 while(true){
Ramonwaninge 3:de8d3ca44a3e 68 pc.printf("%f %f \n", xend,yend);
Ramonwaninge 3:de8d3ca44a3e 69 wait(1);
Ramonwaninge 3:de8d3ca44a3e 70 }
Ramonwaninge 0:779fe292e912 71
Ramonwaninge 0:779fe292e912 72
Ramonwaninge 2:0a7a3c0c08d3 73
Ramonwaninge 0:779fe292e912 74 }