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

Committer:
Ramonwaninge
Date:
Tue Oct 30 11:58:36 2018 +0000
Revision:
8:697aa3c94209
Parent:
7:b59b762c537e
Child:
9:930bd825689f
Inverse kinematics deels toegepast. hij werkt alleen voor theta1 en voor de eerste step response;

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