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

Committer:
Ramonwaninge
Date:
Mon Oct 29 15:46:20 2018 +0000
Revision:
5:d78ed3a3e66a
Parent:
4:49dfbfcd3577
Child:
6:59744dfe8ea7
Alleen xcoordinaat nog geimplementeerd DAT WERKT!

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 5:d78ed3a3e66a 23 double xendsum;
Ramonwaninge 5:d78ed3a3e66a 24 double xendsqrt1;
Ramonwaninge 5:d78ed3a3e66a 25 double xendsqrt2;
Ramonwaninge 5:d78ed3a3e66a 26 double xend;
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 4:49dfbfcd3577 40
Ramonwaninge 4:49dfbfcd3577 41 if (button1 == 0){ //als emg1==voorbij treshold,
Ramonwaninge 4:49dfbfcd3577 42 theta1 = PI*(theta1/PI + 0.1); //double theta1-> plus een paar counts (emg*richting)
Ramonwaninge 4:49dfbfcd3577 43 //double theta4-> plus een paar counts (emg*richting)
Ramonwaninge 4:49dfbfcd3577 44 //default = als x = xbase/2... break, okee dit moet hier niet
Ramonwaninge 4:49dfbfcd3577 45 }
Ramonwaninge 4:49dfbfcd3577 46 else {theta1 = theta1;}
Ramonwaninge 4:49dfbfcd3577 47 xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
Ramonwaninge 4:49dfbfcd3577 48 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 4:49dfbfcd3577 49 xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
Ramonwaninge 2:0a7a3c0c08d3 50 xend = (xendsum + xendsqrt1/xendsqrt2)/2;
Ramonwaninge 3:de8d3ca44a3e 51 }
Ramonwaninge 1:f63be2020475 52 void ycor(){
Ramonwaninge 1:f63be2020475 53 //als emg2 == voorbij treshold,
Ramonwaninge 1:f63be2020475 54 //double theta1 -> plus counts (emg*richting)
Ramonwaninge 1:f63be2020475 55 //double theta4 -> plus counts (emg*richting)
Ramonwaninge 1:f63be2020475 56 //reken y door
Ramonwaninge 1:f63be2020475 57 //default = als y = default... break
Ramonwaninge 1:f63be2020475 58 //end
Ramonwaninge 1:f63be2020475 59 }
Ramonwaninge 0:779fe292e912 60 void flip(){}
Ramonwaninge 0:779fe292e912 61
Ramonwaninge 0:779fe292e912 62 int main()
Ramonwaninge 0:779fe292e912 63 {
Ramonwaninge 2:0a7a3c0c08d3 64
Ramonwaninge 2:0a7a3c0c08d3 65 pc.baud(115200);
Ramonwaninge 0:779fe292e912 66 //default = theta1 = theta4 = pi/2
Ramonwaninge 4:49dfbfcd3577 67 emgcheck.attach(xcor, 0.1);
Ramonwaninge 3:de8d3ca44a3e 68 while(true){
Ramonwaninge 5:d78ed3a3e66a 69
Ramonwaninge 5:d78ed3a3e66a 70 if (button1 == 0){
Ramonwaninge 5:d78ed3a3e66a 71 pc.printf("\n\r %f %f \n\r", xend,yend);
Ramonwaninge 5:d78ed3a3e66a 72 pc.printf(" %f\n\r", theta1);
Ramonwaninge 5:d78ed3a3e66a 73 wait(1.5);
Ramonwaninge 5:d78ed3a3e66a 74 }
Ramonwaninge 3:de8d3ca44a3e 75 }
Ramonwaninge 0:779fe292e912 76
Ramonwaninge 0:779fe292e912 77
Ramonwaninge 2:0a7a3c0c08d3 78
Ramonwaninge 0:779fe292e912 79 }