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@10:2b965defcde5, 2018-10-31 (annotated)

Committer:
Ramonwaninge
Date:
Wed Oct 31 09:09:43 2018 +0000
Revision:
10:2b965defcde5
Parent:
9:930bd825689f
Child:
11:256e84fea925
BIEM! alles werkt, de hoeken zijn virtueel, maar gaan nog wel door na zoveel pi, waardoor x en y nan worden;

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 10:2b965defcde5 8 DigitalIn button1(SW3); // defining testbutton NEEDS TO BE REMOVED
Ramonwaninge 10:2b965defcde5 9 DigitalOut ledr(LED1); // Only for testing
Ramonwaninge 10:2b965defcde5 10 //DigitalOut led2(LED2); // Only for testing
Ramonwaninge 10:2b965defcde5 11 InterruptIn button2(SW2); //Only for testing
Ramonwaninge 8:697aa3c94209 12
Ramonwaninge 10:2b965defcde5 13 // nog te verwijderen/ aan te passen, zijn dubbel gedefinieerd
Ramonwaninge 10:2b965defcde5 14 double theta11;
Ramonwaninge 0:779fe292e912 15
Ramonwaninge 10:2b965defcde5 16 //Joe dit zijn de inputsignalen (en tussenvariabelen)
Ramonwaninge 10:2b965defcde5 17 double theta10; //vorige theta
Ramonwaninge 10:2b965defcde5 18 double theta1; // huidige/nieuwe theta
Ramonwaninge 10:2b965defcde5 19 double theta40;
Ramonwaninge 10:2b965defcde5 20 double theta4;
Ramonwaninge 10:2b965defcde5 21 bool emg1;
Ramonwaninge 10:2b965defcde5 22 bool emg2;
Ramonwaninge 10:2b965defcde5 23 bool emg3;
Ramonwaninge 8:697aa3c94209 24 double thetaflip = 0;
Ramonwaninge 8:697aa3c94209 25 double omega1;
Ramonwaninge 8:697aa3c94209 26 double omega4;
Ramonwaninge 10:2b965defcde5 27 double prefx;
Ramonwaninge 10:2b965defcde5 28 double prefy;
Ramonwaninge 8:697aa3c94209 29 double deltat = 0.01;
Ramonwaninge 0:779fe292e912 30 //Joe dit zijn de constantes
Ramonwaninge 8:697aa3c94209 31 double ll = 200.0;
Ramonwaninge 8:697aa3c94209 32 double lu = 170.0;
Ramonwaninge 8:697aa3c94209 33 double lb = 10.0;
Ramonwaninge 8:697aa3c94209 34 double le = 79.0;
Ramonwaninge 8:697aa3c94209 35 double xbase = 340;
Ramonwaninge 2:0a7a3c0c08d3 36
Ramonwaninge 10:2b965defcde5 37
Ramonwaninge 2:0a7a3c0c08d3 38 //forward kinematics, Check mathematica! Omdat mbed in paniek raakt met meerdere wortels, hebben we de vergelijking opgedeeld in 3 stukken
Ramonwaninge 10:2b965defcde5 39 //check void forward voor de berekeningen
Ramonwaninge 2:0a7a3c0c08d3 40 //First define the position equation of x
Ramonwaninge 5:d78ed3a3e66a 41 double xendsum;
Ramonwaninge 5:d78ed3a3e66a 42 double xendsqrt1;
Ramonwaninge 5:d78ed3a3e66a 43 double xendsqrt2;
Ramonwaninge 5:d78ed3a3e66a 44 double xend;
Ramonwaninge 2:0a7a3c0c08d3 45 //Now define the pos. eq. of y
Ramonwaninge 6:59744dfe8ea7 46 double yendsum;
Ramonwaninge 6:59744dfe8ea7 47 double yendsqrt1;
Ramonwaninge 6:59744dfe8ea7 48 double yendsqrt2;
Ramonwaninge 6:59744dfe8ea7 49 double yend;
Ramonwaninge 2:0a7a3c0c08d3 50
Ramonwaninge 2:0a7a3c0c08d3 51
Ramonwaninge 10:2b965defcde5 52 //Hier definieren we de functies en tickers
Ramonwaninge 1:f63be2020475 53 Ticker emgcheck;
Ramonwaninge 7:b59b762c537e 54 Ticker emgcheck2;
Ramonwaninge 9:930bd825689f 55 Ticker emgcheck3;
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 9:930bd825689f 60 // hieronder moet veranderd worden naar if button1 == 0, x = x+eenbeetje
Ramonwaninge 9:930bd825689f 61 //maar daar moet eerst inverse kinematics voor gebeuren.
Ramonwaninge 10:2b965defcde5 62 /*if (button1 == 0){ //als emg1==voorbij treshold, komt waarschijnlijk in de inverse functie
Ramonwaninge 10:2b965defcde5 63 theta1 = PI*(theta1/PI + 0.1);
Ramonwaninge 10:2b965defcde5 64 hij is geblokt omdat ik de knop nodig heb
Ramonwaninge 10:2b965defcde5 65 //default = als x = xbase/2... break, okee dit moet hier niet, maar weet niet waar wel...
Ramonwaninge 4:49dfbfcd3577 66 }
Ramonwaninge 10:2b965defcde5 67 else {theta1 = theta1;}*/
Ramonwaninge 10:2b965defcde5 68 //hieronder komen de doorrekeningen van de hoeken naar de coordinaten, check de mathematicafile voor de afleiding
Ramonwaninge 4:49dfbfcd3577 69 xendsum = lb + xbase +ll*(cos(theta1) - cos(theta4));
Ramonwaninge 4:49dfbfcd3577 70 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 71 xendsqrt2 = sqrt(pow((-xbase/ll+cos(theta1)+cos(theta4)),2)+ pow(sin(theta1) - sin(theta4),2));
Ramonwaninge 2:0a7a3c0c08d3 72 xend = (xendsum + xendsqrt1/xendsqrt2)/2;
Ramonwaninge 6:59744dfe8ea7 73
Ramonwaninge 6:59744dfe8ea7 74 yendsum = -le + ll/2*(sin(theta1)+sin(theta4));
Ramonwaninge 6:59744dfe8ea7 75 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 76 yendsqrt2 = sqrt(pow((-xbase/ll + cos(theta1)+ cos(theta4)),2)+ pow((sin(theta1)-sin(theta4)),2));
Ramonwaninge 6:59744dfe8ea7 77 yend = (yendsum + yendsqrt1/yendsqrt2);
Ramonwaninge 10:2b965defcde5 78
Ramonwaninge 10:2b965defcde5 79 if (xend !=xend){
Ramonwaninge 10:2b965defcde5 80
Ramonwaninge 10:2b965defcde5 81
Ramonwaninge 10:2b965defcde5 82
Ramonwaninge 10:2b965defcde5 83 }//end van if xend !=xend
Ramonwaninge 3:de8d3ca44a3e 84 }
Ramonwaninge 10:2b965defcde5 85 void demomode(){} //Komt nog...
Ramonwaninge 10:2b965defcde5 86 //Alleen nodig in de
Ramonwaninge 6:59744dfe8ea7 87 //als emg2 == voorbij treshold,
Ramonwaninge 10:2b965defcde5 88 //double theta1 -> plus counts (emg*richting)
Ramonwaninge 6:59744dfe8ea7 89 //double theta4 -> plus counts (emg*richting)
Ramonwaninge 6:59744dfe8ea7 90 //reken y door
Ramonwaninge 6:59744dfe8ea7 91 //default = als y = default... break
Ramonwaninge 6:59744dfe8ea7 92 //end
Ramonwaninge 10:2b965defcde5 93 /*void flip(){
Ramonwaninge 10:2b965defcde5 94 if(button2==0){thetaflip = PI*(thetaflip/PI+0.5);} // button2==0 moet veranderd naar emg3>= treshold
Ramonwaninge 10:2b965defcde5 95 }*/ //suppressed omdat ik button 2 nodig heb...
Ramonwaninge 7:b59b762c537e 96
Ramonwaninge 10:2b965defcde5 97 // de beweging voor de xcoordinaat!
Ramonwaninge 7:b59b762c537e 98 void inverse(){
Ramonwaninge 7:b59b762c537e 99 /*
Ramonwaninge 10:2b965defcde5 100 qn = qn-1 + (jacobian^-1)*dPref/dt *deltaT
Ramonwaninge 10:2b965defcde5 101 ofwel
Ramonwaninge 10:2b965defcde5 102 thetai+1 = thetai +(jacobian)^-1*vector(deltaX, DeltaY)
Ramonwaninge 10:2b965defcde5 103 waar Pref = emg signaal
Ramonwaninge 7:b59b762c537e 104 */
Ramonwaninge 10:2b965defcde5 105 emg1 = false;
Ramonwaninge 10:2b965defcde5 106 if(theta10 == 0){ //als de hoek voor de beweging niet bekend is
Ramonwaninge 10:2b965defcde5 107 theta10 = PI*0.5; //Definieer de hoek als half pi !!DIT KAN OOK ERGENS IN DEFAULT STAAN.
Ramonwaninge 10:2b965defcde5 108 theta1 = theta10;
Ramonwaninge 8:697aa3c94209 109 }
Ramonwaninge 10:2b965defcde5 110 if(button2 == 0){emg1 = true;
Ramonwaninge 10:2b965defcde5 111
Ramonwaninge 10:2b965defcde5 112 if(emg1 == true){
Ramonwaninge 10:2b965defcde5 113 prefx = 1; //derivative of Preference = 1 (needs to be adjusted)
Ramonwaninge 10:2b965defcde5 114 theta1 = theta10 +/*inverse jacobian keer*/prefx*deltat; //theta nieuw = oud +jacob*deltax
Ramonwaninge 10:2b965defcde5 115 theta10 = theta1;
Ramonwaninge 10:2b965defcde5 116 }
Ramonwaninge 10:2b965defcde5 117 }//sluit van if emg1=true
Ramonwaninge 10:2b965defcde5 118
Ramonwaninge 9:930bd825689f 119
Ramonwaninge 10:2b965defcde5 120
Ramonwaninge 10:2b965defcde5 121 }//sluit van void inverse
Ramonwaninge 10:2b965defcde5 122
Ramonwaninge 10:2b965defcde5 123 //dit is voor de ycoordinaat
Ramonwaninge 10:2b965defcde5 124 void inverse2(){
Ramonwaninge 10:2b965defcde5 125 emg2 = false; //emg is het inputsignaal van de biceps
Ramonwaninge 10:2b965defcde5 126 if(theta40 == 0){
Ramonwaninge 10:2b965defcde5 127 theta40 = PI*0.5;
Ramonwaninge 10:2b965defcde5 128 theta4 = theta40;
Ramonwaninge 9:930bd825689f 129 }
Ramonwaninge 10:2b965defcde5 130 if(button1 == 0){
Ramonwaninge 10:2b965defcde5 131 emg2 = true;
Ramonwaninge 10:2b965defcde5 132 if(emg2 == true){
Ramonwaninge 10:2b965defcde5 133 prefy = 1;
Ramonwaninge 10:2b965defcde5 134 theta4 = theta40 + /*inverse jacobian keer*/prefy*deltat; //theta nieuw = oud +jacob*deltax
Ramonwaninge 10:2b965defcde5 135 theta40 = theta4;
Ramonwaninge 10:2b965defcde5 136 } //sluit van if emg2==true
Ramonwaninge 10:2b965defcde5 137 }
Ramonwaninge 10:2b965defcde5 138
Ramonwaninge 7:b59b762c537e 139 }
Ramonwaninge 0:779fe292e912 140
Ramonwaninge 10:2b965defcde5 141
Ramonwaninge 0:779fe292e912 142 int main()
Ramonwaninge 0:779fe292e912 143 {
Ramonwaninge 2:0a7a3c0c08d3 144
Ramonwaninge 2:0a7a3c0c08d3 145 pc.baud(115200);
Ramonwaninge 0:779fe292e912 146 //default = theta1 = theta4 = pi/2
Ramonwaninge 10:2b965defcde5 147 emgcheck3.attach(forward, 0.1);
Ramonwaninge 10:2b965defcde5 148 emgcheck2.attach(inverse2, 0.1);
Ramonwaninge 9:930bd825689f 149 emgcheck.attach(inverse, 0.1);
Ramonwaninge 8:697aa3c94209 150 ledr=1;
Ramonwaninge 9:930bd825689f 151 pc.printf("%f", theta10);
Ramonwaninge 3:de8d3ca44a3e 152 while(true){
Ramonwaninge 8:697aa3c94209 153
Ramonwaninge 10:2b965defcde5 154 //if (button1 == 0){
Ramonwaninge 10:2b965defcde5 155 pc.printf("\n\r %f %f \n\r %f %f", theta4,theta1, xend, yend);
Ramonwaninge 10:2b965defcde5 156 wait(0.5);
Ramonwaninge 10:2b965defcde5 157 //}
Ramonwaninge 8:697aa3c94209 158 }
Ramonwaninge 0:779fe292e912 159 }