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@7:b59b762c537e, 2018-10-30 (annotated)

Committer:
Ramonwaninge
Date:
Tue Oct 30 10:15:03 2018 +0000
Revision:
7:b59b762c537e
Parent:
6:59744dfe8ea7
Child:
8:697aa3c94209
De flip en x doorrekenen werkt; Nu beginnen met inverse impementeren;

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 7:b59b762c537e 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 7:b59b762c537e 10 DigitalOut ledr(LED1);
Ramonwaninge 7:b59b762c537e 11 DigitalOut led2(LED2);
Ramonwaninge 0:779fe292e912 12
Ramonwaninge 0:779fe292e912 13 //Joe dit zijn de inputsignalen
Ramonwaninge 7:b59b762c537e 14 double theta1 = PI*0.5; double theta4 = PI*0.5; double emg1; double emg2; double emg3; double thetaflip = 0;
Ramonwaninge 7:b59b762c537e 15 double omega1; double omega4;
Ramonwaninge 0:779fe292e912 16 // Joe dit zijn de outputsignalen
Ramonwaninge 1:f63be2020475 17 double x; double y;
Ramonwaninge 1:f63be2020475 18
Ramonwaninge 0:779fe292e912 19 //Joe dit zijn de constantes
Ramonwaninge 2:0a7a3c0c08d3 20 double ll = 200.0; double lu = 170.0; double lb = 10.0; double le = 79.0; double xbase = 340;
Ramonwaninge 0:779fe292e912 21 //Joe dit zijn de Tickers
Ramonwaninge 2:0a7a3c0c08d3 22
Ramonwaninge 2:0a7a3c0c08d3 23 //forward kinematics, Check mathematica! Omdat mbed in paniek raakt met meerdere wortels, hebben we de vergelijking opgedeeld in 3 stukken
Ramonwaninge 2:0a7a3c0c08d3 24 //First define the position equation of x
Ramonwaninge 5:d78ed3a3e66a 25 double xendsum;
Ramonwaninge 5:d78ed3a3e66a 26 double xendsqrt1;
Ramonwaninge 5:d78ed3a3e66a 27 double xendsqrt2;
Ramonwaninge 5:d78ed3a3e66a 28 double xend;
Ramonwaninge 2:0a7a3c0c08d3 29 //Now define the pos. eq. of y
Ramonwaninge 6:59744dfe8ea7 30 double yendsum;
Ramonwaninge 6:59744dfe8ea7 31 double yendsqrt1;
Ramonwaninge 6:59744dfe8ea7 32 double yendsqrt2;
Ramonwaninge 6:59744dfe8ea7 33 double yend;
Ramonwaninge 2:0a7a3c0c08d3 34
Ramonwaninge 2:0a7a3c0c08d3 35
Ramonwaninge 2:0a7a3c0c08d3 36 //Hier definieren we de functies
Ramonwaninge 1:f63be2020475 37 Ticker emgcheck;
Ramonwaninge 7:b59b762c537e 38 Ticker emgcheck2;
Ramonwaninge 1:f63be2020475 39
Ramonwaninge 0:779fe292e912 40
Ramonwaninge 0:779fe292e912 41 //Joe, hieronder staan de functies die door de tickers aangeroepen worden
Ramonwaninge 7:b59b762c537e 42 void forward(){ //dit is de ticker die zegt, als button=0, theta 1 wordt groter. dan worden x en y doorgerekend
Ramonwaninge 7:b59b762c537e 43 // hieronder moet veranderd worden naar if button1 == 0, x = x+eenbeetje
Ramonwaninge 7:b59b762c537e 44 //maar daar moet eerst inverse kinematics voor gebeuren.
Ramonwaninge 4:49dfbfcd3577 45 if (button1 == 0){ //als emg1==voorbij treshold,
Ramonwaninge 7:b59b762c537e 46 theta1 = PI*(theta1/PI + 0.1); //double theta1-> plus een paar counts (emg*richting)
Ramonwaninge 4:49dfbfcd3577 47 //double theta4-> plus een paar counts (emg*richting)
Ramonwaninge 4:49dfbfcd3577 48 //default = als x = xbase/2... break, okee dit moet hier niet
Ramonwaninge 4:49dfbfcd3577 49 }
Ramonwaninge 4:49dfbfcd3577 50 else {theta1 = theta1;}
Ramonwaninge 4:49dfbfcd3577 51 xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
Ramonwaninge 4:49dfbfcd3577 52 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 53 xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
Ramonwaninge 2:0a7a3c0c08d3 54 xend = (xendsum + xendsqrt1/xendsqrt2)/2;
Ramonwaninge 6:59744dfe8ea7 55
Ramonwaninge 6:59744dfe8ea7 56 yendsum = -le + ll/2*(sin(theta1)+sin(theta4));
Ramonwaninge 6:59744dfe8ea7 57 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 6:59744dfe8ea7 58 yendsqrt2 = sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
Ramonwaninge 6:59744dfe8ea7 59 yend = (yendsum + yendsqrt1/yendsqrt2);
Ramonwaninge 3:de8d3ca44a3e 60 }
Ramonwaninge 7:b59b762c537e 61 void demomode(){} //Alleen nodig in de DEMOMODE
Ramonwaninge 6:59744dfe8ea7 62 //als emg2 == voorbij treshold,
Ramonwaninge 6:59744dfe8ea7 63 //double theta1 -> plus counts (emg*richting)
Ramonwaninge 6:59744dfe8ea7 64 //double theta4 -> plus counts (emg*richting)
Ramonwaninge 6:59744dfe8ea7 65 //reken y door
Ramonwaninge 6:59744dfe8ea7 66 //default = als y = default... break
Ramonwaninge 6:59744dfe8ea7 67 //end
Ramonwaninge 7:b59b762c537e 68 void flip(){
Ramonwaninge 7:b59b762c537e 69 if(button2==0){thetaflip = PI*(thetaflip/PI+0.5);}
Ramonwaninge 7:b59b762c537e 70 }
Ramonwaninge 7:b59b762c537e 71
Ramonwaninge 7:b59b762c537e 72 void inverse(){
Ramonwaninge 7:b59b762c537e 73 /*
Ramonwaninge 7:b59b762c537e 74 We willen ergens beginnen
Ramonwaninge 7:b59b762c537e 75 Dan de functie qi+1 = qi + (jacobian *(qi - qi-1)/deltaT)deltaT
Ramonwaninge 7:b59b762c537e 76 if
Ramonwaninge 7:b59b762c537e 77 */
Ramonwaninge 7:b59b762c537e 78 theta1-1 = PI*0.5
Ramonwaninge 7:b59b762c537e 79 theta4-1 = PI*0.5
Ramonwaninge 7:b59b762c537e 80
Ramonwaninge 7:b59b762c537e 81 theta11 =
Ramonwaninge 7:b59b762c537e 82 theta14 =
Ramonwaninge 7:b59b762c537e 83
Ramonwaninge 7:b59b762c537e 84 theta1+1
Ramonwaninge 7:b59b762c537e 85 theta4+1
Ramonwaninge 7:b59b762c537e 86
Ramonwaninge 7:b59b762c537e 87 }
Ramonwaninge 0:779fe292e912 88
Ramonwaninge 0:779fe292e912 89 int main()
Ramonwaninge 0:779fe292e912 90 {
Ramonwaninge 2:0a7a3c0c08d3 91
Ramonwaninge 2:0a7a3c0c08d3 92 pc.baud(115200);
Ramonwaninge 0:779fe292e912 93 //default = theta1 = theta4 = pi/2
Ramonwaninge 7:b59b762c537e 94 emgcheck.attach(forward, 0.1);
Ramonwaninge 7:b59b762c537e 95 emgcheck2.attach(flip, 0.1);
Ramonwaninge 3:de8d3ca44a3e 96 while(true){
Ramonwaninge 7:b59b762c537e 97 if(xend !=xend){ledr = 0;}
Ramonwaninge 7:b59b762c537e 98 else{ledr=1;}
Ramonwaninge 7:b59b762c537e 99
Ramonwaninge 7:b59b762c537e 100 if(button2==0){pc.printf("Draai! Hij is plat!");}
Ramonwaninge 7:b59b762c537e 101
Ramonwaninge 5:d78ed3a3e66a 102 if (button1 == 0){
Ramonwaninge 7:b59b762c537e 103 pc.printf("\n\r %f %f \n\r", xend,yend);
Ramonwaninge 7:b59b762c537e 104 pc.printf(" %f\n\r", theta1);
Ramonwaninge 7:b59b762c537e 105 wait(1.5);
Ramonwaninge 7:b59b762c537e 106 }
Ramonwaninge 3:de8d3ca44a3e 107 }
Ramonwaninge 0:779fe292e912 108
Ramonwaninge 0:779fe292e912 109
Ramonwaninge 2:0a7a3c0c08d3 110
Ramonwaninge 0:779fe292e912 111 }