ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@20:2840a749fb55, 2014-12-14 (annotated)

Committer:
Jagang
Date:
Sun Dec 14 17:30:27 2014 +0000
Revision:
20:2840a749fb55
Parent:
17:f360e21d3307
Parent:
15:ae29e83947ff
Child:
21:5443f93819db
Merging

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jagang 0:41149573d577 1 #include "mbed.h"
Jagang 0:41149573d577 2 #include "QEI.h"
Jagang 0:41149573d577 3 #include "Odometry.h"
Near32 17:f360e21d3307 4 #include <iostream>
Jagang 13:ae5718ac8511 5 #include "Map.h"
Jagang 0:41149573d577 6
Jagang 0:41149573d577 7
Near32 3:573a0dc8383f 8 /*---------------------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 9 /*---------------------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 10 /*KalmanFilter*/
Jagang 15:ae29e83947ff 11
Near32 3:573a0dc8383f 12 #include "EKF.h"
Near32 17:f360e21d3307 13 Mat<double> motion_bicycle3( Mat<double> state, Mat<double> command, double dt = 0.5);
Near32 17:f360e21d3307 14 Mat<double> sensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt = 0.5 );
Near32 17:f360e21d3307 15 Mat<double> jmotion_bicycle3( Mat<double> state, Mat<double> command, double dt = 0.5);
Near32 17:f360e21d3307 16 Mat<double> jsensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt = 0.5);
Near32 3:573a0dc8383f 17 void measurementCallback( Mat<double>* z, Odometry* odometry);
Near32 17:f360e21d3307 18 bool setPWM(PwmOut *servo,float p);
Near32 3:573a0dc8383f 19
Near32 3:573a0dc8383f 20 Mat<double> bicycle(3,1);
Near32 17:f360e21d3307 21 int reduc = 16;
Near32 3:573a0dc8383f 22 /*---------------------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 23 /*---------------------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 24
Jagang 20:2840a749fb55 25 <<<<<<< local
Near32 17:f360e21d3307 26 /*----------------------------------------------------------------------------------------------*/
Near32 17:f360e21d3307 27 /*Serial*/
Near32 17:f360e21d3307 28 Serial pcs(USBTX, USBRX); // tx, rx
Near32 17:f360e21d3307 29 /*----------------------------------------------------------------------------------------------*/
Jagang 20:2840a749fb55 30 =======
Jagang 13:ae5718ac8511 31 /* --- Initialisation de la liste des obstable --- */
Jagang 13:ae5718ac8511 32 int Obstacle::lastId = 0;
Jagang 20:2840a749fb55 33 >>>>>>> other
Near32 3:573a0dc8383f 34
Jagang 0:41149573d577 35 int main()
Jagang 0:41149573d577 36 {
Near32 17:f360e21d3307 37
Near32 17:f360e21d3307 38
Near32 17:f360e21d3307 39 PwmOut pw1(p22);
Near32 17:f360e21d3307 40 DigitalOut dir1(p21);
Near32 17:f360e21d3307 41 PwmOut pw2(p24);
Near32 17:f360e21d3307 42 DigitalOut dir2(p23);
Near32 17:f360e21d3307 43
Near32 17:f360e21d3307 44 //mbuino
Near32 17:f360e21d3307 45 /*
Near32 17:f360e21d3307 46 PwmOut pw1(P0_17);
Near32 17:f360e21d3307 47 DigitalOut dir1(P0_18);
Near32 17:f360e21d3307 48 PwmOut pw2(P0_23);
Near32 17:f360e21d3307 49 DigitalOut dir2(P0_19);
Near32 17:f360e21d3307 50 */
Near32 17:f360e21d3307 51 /*
Near32 17:f360e21d3307 52 //nucleo
Near32 17:f360e21d3307 53 PwmOut pw1(PB_8);
Near32 17:f360e21d3307 54 DigitalOut dir1(D12);
Near32 17:f360e21d3307 55 PwmOut pw2(PB_9);
Near32 17:f360e21d3307 56 DigitalOut dir2(D13);
Near32 17:f360e21d3307 57 */
Near32 17:f360e21d3307 58 pw1.period_us(10);
Near32 17:f360e21d3307 59 pw2.period_us(10);
Near32 17:f360e21d3307 60
Near32 17:f360e21d3307 61
Near32 17:f360e21d3307 62 dir1.write(0);
Near32 17:f360e21d3307 63 dir2.write(0);
Near32 17:f360e21d3307 64 pw1.write(1.0);
Near32 17:f360e21d3307 65 pw2.write(0.8);
Near32 17:f360e21d3307 66 //setPWM(&pw1,0.9);
Near32 17:f360e21d3307 67 pcs.printf("mise à jour des pwm.\n");
Near32 17:f360e21d3307 68 //while(1);
Near32 3:573a0dc8383f 69 /*----------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 70 /*Odometry*/
Near32 17:f360e21d3307 71 QEI qei_left(p15,p16,NC,1024*reduc,QEI::X4_ENCODING);
Near32 17:f360e21d3307 72 //QEI qei_left(P0_2,P0_7,NC,1024*reduc,QEI::X4_ENCODING);//mbuino
Near32 17:f360e21d3307 73 //QEI qei_left(PA_3,PA_2,NC,1024*reduc,QEI::X4_ENCODING);//nucleo
Near32 17:f360e21d3307 74
Near32 17:f360e21d3307 75 QEI qei_right(p17,p18,NC,1024*reduc,QEI::X4_ENCODING);
Near32 17:f360e21d3307 76 //QEI qei_right(P0_8,P0_20,NC,1024*reduc,QEI::X4_ENCODING);//mbuino
Near32 17:f360e21d3307 77 //QEI qei_right(PA_10,PB_3,NC,1024*reduc,QEI::X4_ENCODING);//nucleo
Jagang 0:41149573d577 78
Jagang 0:41149573d577 79 Odometry odometry(&qei_left,&qei_right,0.07,0.07,0.26);
Near32 3:573a0dc8383f 80 /*----------------------------------------------------------------------------------------------*/
Jagang 0:41149573d577 81
Near32 3:573a0dc8383f 82
Near32 3:573a0dc8383f 83
Near32 3:573a0dc8383f 84 /*----------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 85 /*KalmanFilter*/
Near32 8:1150a13f6967 86 double phi_max = 100;
Near32 3:573a0dc8383f 87 /*en millimetres*/
Near32 3:573a0dc8383f 88 bicycle.set((double)100, 1,1); /*radius*/
Near32 3:573a0dc8383f 89 bicycle.set((double)100, 2,1);
Near32 3:573a0dc8383f 90 bicycle.set((double)66, 3,1); /*entre-roue*/
Near32 3:573a0dc8383f 91
Near32 3:573a0dc8383f 92 int nbrstate = 5;
Near32 3:573a0dc8383f 93 int nbrcontrol = 2;
Near32 17:f360e21d3307 94 int nbrobs = 5;
Near32 3:573a0dc8383f 95 double dt = (double)0.05;
Near32 8:1150a13f6967 96 double stdnoise = (double)0.05;
Near32 3:573a0dc8383f 97
Near32 3:573a0dc8383f 98 Mat<double> initX((double)0, nbrstate, 1);
Near32 3:573a0dc8383f 99 initX.set( (double)0, 3,1);
Near32 3:573a0dc8383f 100
Near32 3:573a0dc8383f 101 bool extended = true;
Near32 17:f360e21d3307 102 bool filterOn = false;
Near32 17:f360e21d3307 103 EKF<double> instance(&pcs, nbrstate, nbrcontrol, nbrobs, dt, stdnoise, /*current state*/ initX, extended, filterOn);
Near32 3:573a0dc8383f 104
Near32 17:f360e21d3307 105 instance.initMotion(motion_bicycle3);
Near32 17:f360e21d3307 106 instance.initSensor(sensor_bicycle3);
Near32 17:f360e21d3307 107 instance.initJMotion(jmotion_bicycle3);
Near32 17:f360e21d3307 108 instance.initJSensor(jsensor_bicycle3);
Near32 3:573a0dc8383f 109
Near32 3:573a0dc8383f 110 /*desired State : (x y theta phiright phileft)*/
Near32 3:573a0dc8383f 111 Mat<double> dX((double)0, nbrstate, 1);
Near32 17:f360e21d3307 112 dX.set( (double)100, 1,1);
Near32 17:f360e21d3307 113 dX.set( (double)0, 2,1);
Near32 4:4025c071b207 114 dX.set( (double)0, 3,1);
Near32 3:573a0dc8383f 115 dX.set( (double)0, 4,1);
Near32 3:573a0dc8383f 116 dX.set( (double)0, 5,1);
Near32 3:573a0dc8383f 117
Near32 17:f360e21d3307 118 Mat<double> ki((double)0, nbrcontrol, nbrstate);
Near32 17:f360e21d3307 119 Mat<double> kp((double)0, nbrcontrol, nbrstate);
Near32 17:f360e21d3307 120 Mat<double> kd((double)0, nbrcontrol, nbrstate);
Near32 17:f360e21d3307 121 //Mat<double> kdd((double)0.0015, nbrcontrol, nbrstate);
Near32 17:f360e21d3307 122
Near32 17:f360e21d3307 123 for(int i=1;i<=nbrstate;i++)
Near32 17:f360e21d3307 124 {
Near32 17:f360e21d3307 125 kp.set( (double)0.01, i, i);
Near32 17:f360e21d3307 126 kd.set( (double)0.0001, i, i);
Near32 17:f360e21d3307 127 ki.set( (double)0.0001, i, i);
Near32 17:f360e21d3307 128 }
Near32 17:f360e21d3307 129
Near32 17:f360e21d3307 130 instance.setKi(ki);
Near32 17:f360e21d3307 131 instance.setKp(kp);
Near32 17:f360e21d3307 132 instance.setKd(kd);
Near32 17:f360e21d3307 133 //instance.setKdd(kdd);
Near32 17:f360e21d3307 134
Near32 3:573a0dc8383f 135 Mat<double> u(transpose( instance.getCommand()) );
Near32 3:573a0dc8383f 136
Near32 3:573a0dc8383f 137 /*Observations*/
Near32 17:f360e21d3307 138 /*il nous faut 5 observation :*/
Near32 17:f360e21d3307 139 Mat<double> z((double)0,5,1);
Near32 3:573a0dc8383f 140 measurementCallback(&z, &odometry);
Near32 3:573a0dc8383f 141
Near32 3:573a0dc8383f 142 /*----------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 143
Near32 3:573a0dc8383f 144
Jagang 0:41149573d577 145 while(1)
Jagang 0:41149573d577 146 {
Near32 17:f360e21d3307 147 //wait(1);
Near32 17:f360e21d3307 148 pcs.printf("%f : %f : %f\n",odometry.getX()*100,odometry.getY()*100,odometry.getTheta()*180/3.14);
Near32 17:f360e21d3307 149
Near32 3:573a0dc8383f 150
Near32 3:573a0dc8383f 151 /*------------------------------------------------------------------------------------------*/
Near32 17:f360e21d3307 152 /*Asservissement*/
Near32 17:f360e21d3307 153
Near32 17:f360e21d3307 154 //measurementCallback(&z, &odometry);
Near32 17:f360e21d3307 155 instance.measurement_Callback( instance.getX(), dX, true );
Near32 17:f360e21d3307 156
Near32 3:573a0dc8383f 157 instance.state_Callback();
Near32 17:f360e21d3307 158
Near32 10:bca0274a007b 159 double phi_r = instance.getCommand().get(1,1);
Near32 10:bca0274a007b 160 double phi_l = instance.getCommand().get(2,1);
Near32 8:1150a13f6967 161
Near32 17:f360e21d3307 162 double phi_max = 100;
Near32 3:573a0dc8383f 163
Near32 17:f360e21d3307 164 instance.computeCommand(dX, (double)dt, -2);
Near32 17:f360e21d3307 165 pcs.printf("command : \n phi_r = %f \n phi_l = %f \n", phi_r/phi_max*100, phi_l/phi_max*100);
Near32 17:f360e21d3307 166 //instance.getX().afficher();
Near32 17:f360e21d3307 167
Near32 17:f360e21d3307 168
Near32 17:f360e21d3307 169 if(phi_r <= 0)
Near32 17:f360e21d3307 170 dir1.write(0);
Near32 17:f360e21d3307 171 else
Near32 17:f360e21d3307 172 dir1.write(1);
Near32 17:f360e21d3307 173
Near32 17:f360e21d3307 174 if(phi_l <= 0)
Near32 17:f360e21d3307 175 dir2.write(0);
Near32 17:f360e21d3307 176 else
Near32 17:f360e21d3307 177 dir2.write(1);
Near32 17:f360e21d3307 178
Near32 17:f360e21d3307 179 if(abs(phi_r/phi_max) < 1.0)
Near32 17:f360e21d3307 180 setPWM(&pw1, (float)abs(phi_r/phi_max));
Near32 17:f360e21d3307 181 else
Near32 17:f360e21d3307 182 cout << "P1..." << endl;
Near32 17:f360e21d3307 183
Near32 17:f360e21d3307 184 if(abs(phi_l/phi_max) < 1.0)
Near32 17:f360e21d3307 185 setPWM(&pw2,(float)abs(phi_l/phi_max));
Near32 17:f360e21d3307 186 else
Near32 17:f360e21d3307 187 pcs.printf("P2...");
Near32 17:f360e21d3307 188
Near32 17:f360e21d3307 189 pcs.printf("\n\n----------------- Commande mise executee. ------------------ \n\n");
Near32 3:573a0dc8383f 190
Jagang 0:41149573d577 191 }
Jagang 0:41149573d577 192 }
Near32 3:573a0dc8383f 193
Near32 3:573a0dc8383f 194 void measurementCallback( Mat<double>* z, Odometry* odometry)
Near32 3:573a0dc8383f 195 {
Near32 3:573a0dc8383f 196 z->set( (double)/*conversionUnitée mm */odometry->getX(), 1,1);
Near32 3:573a0dc8383f 197 z->set( (double)/*conversionUnitée mm*/odometry->getY(), 2,1);
Near32 3:573a0dc8383f 198 z->set( (double)/*conversionUnitée rad*/odometry->getTheta(), 3,1);
Near32 3:573a0dc8383f 199 }
Near32 3:573a0dc8383f 200
Near32 17:f360e21d3307 201 Mat<double> motion_bicycle3( Mat<double> state, Mat<double> command, double dt)
Near32 3:573a0dc8383f 202 {
Near32 3:573a0dc8383f 203 Mat<double> r(state);
Near32 3:573a0dc8383f 204 double v = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)+r.get(5,1));
Near32 17:f360e21d3307 205 double w = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)-r.get(5,1));
Near32 3:573a0dc8383f 206
Near32 3:573a0dc8383f 207 r.set( r.get(1,1) + v*cos(r.get(3,1))*dt, 1,1);
Near32 3:573a0dc8383f 208 r.set( r.get(2,1) + v*sin(r.get(3,1))*dt, 2,1);
Near32 3:573a0dc8383f 209
Near32 17:f360e21d3307 210 double angle = (r.get(3,1) + dt*w);
Near32 3:573a0dc8383f 211 if( angle < -PI)
Near32 3:573a0dc8383f 212 {
Near32 3:573a0dc8383f 213 angle = angle - PI*ceil(angle/PI);
Near32 3:573a0dc8383f 214 }
Near32 3:573a0dc8383f 215 else if( angle > PI)
Near32 3:573a0dc8383f 216 {
Near32 3:573a0dc8383f 217 angle = angle - PI*floor(angle/PI);
Near32 3:573a0dc8383f 218 }
Near32 3:573a0dc8383f 219
Near32 17:f360e21d3307 220 r.set( atan21(sin(angle), cos(angle)), 3,1);
Near32 3:573a0dc8383f 221
Near32 17:f360e21d3307 222
Near32 17:f360e21d3307 223 /*----------------------------------------*/
Near32 17:f360e21d3307 224 /*Modele du moteur*/
Near32 17:f360e21d3307 225 /*----------------------------------------*/
Near32 17:f360e21d3307 226 double r1 = bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)+command.get(2,1));
Near32 17:f360e21d3307 227 double r2 = bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)-command.get(2,1));
Near32 17:f360e21d3307 228
Near32 17:f360e21d3307 229
Near32 17:f360e21d3307 230 r.set( r1, 4,1);
Near32 17:f360e21d3307 231 r.set( r2, 5,1);
Near32 17:f360e21d3307 232
Near32 17:f360e21d3307 233
Near32 17:f360e21d3307 234 /*----------------------------------------*/
Near32 17:f360e21d3307 235 /*----------------------------------------*/
Near32 3:573a0dc8383f 236
Near32 3:573a0dc8383f 237 return r;
Near32 3:573a0dc8383f 238 }
Near32 3:573a0dc8383f 239
Near32 3:573a0dc8383f 240
Near32 17:f360e21d3307 241 Mat<double> sensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
Near32 17:f360e21d3307 242 {
Near32 17:f360e21d3307 243 return state;
Near32 3:573a0dc8383f 244 }
Near32 3:573a0dc8383f 245
Near32 17:f360e21d3307 246
Near32 17:f360e21d3307 247 Mat<double> jmotion_bicycle3( Mat<double> state, Mat<double> command, double dt)
Near32 3:573a0dc8383f 248 {
Near32 3:573a0dc8383f 249 double h = numeric_limits<double>::epsilon()*10e2;
Near32 3:573a0dc8383f 250 Mat<double> var( (double)0, state.getLine(), state.getColumn());
Near32 3:573a0dc8383f 251 var.set( h, 1,1);
Near32 17:f360e21d3307 252 Mat<double> G(motion_bicycle3(state, command, dt) - motion_bicycle3(state+var, command,dt));
Near32 3:573a0dc8383f 253
Near32 3:573a0dc8383f 254 for(int i=2;i<=state.getLine();i++)
Near32 3:573a0dc8383f 255 {
Near32 3:573a0dc8383f 256 var.set( (double)0, i-1,1);
Near32 3:573a0dc8383f 257 var.set( h, i,1);
Near32 17:f360e21d3307 258 G = operatorL(G, motion_bicycle3(state, command, dt) - motion_bicycle3(state+var, command,dt) );
Near32 3:573a0dc8383f 259 }
Near32 3:573a0dc8383f 260
Near32 3:573a0dc8383f 261
Near32 3:573a0dc8383f 262 return (1.0/h)*G;
Near32 3:573a0dc8383f 263 }
Near32 3:573a0dc8383f 264
Near32 17:f360e21d3307 265 Mat<double> jsensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
Near32 3:573a0dc8383f 266 {
Near32 3:573a0dc8383f 267 double h = numeric_limits<double>::epsilon()*10e2;
Near32 3:573a0dc8383f 268 Mat<double> var((double)0, state.getLine(), state.getColumn());
Near32 3:573a0dc8383f 269 var.set( h, 1,1);
Near32 17:f360e21d3307 270 Mat<double> H(sensor_bicycle3(state, command, d_state, dt) - sensor_bicycle3(state+var, command, d_state, dt));
Near32 3:573a0dc8383f 271
Near32 3:573a0dc8383f 272 for(int i=2;i<=state.getLine();i++)
Near32 17:f360e21d3307 273 {
Near32 3:573a0dc8383f 274 var.set( (double)0, i-1,1);
Near32 3:573a0dc8383f 275 var.set( h, i,1);
Near32 17:f360e21d3307 276 Mat<double> temp(sensor_bicycle3(state, command, d_state, dt) - sensor_bicycle3(state+var, command, d_state, dt));
Near32 17:f360e21d3307 277
Near32 17:f360e21d3307 278 H = operatorL(H, temp );
Near32 17:f360e21d3307 279 pcs.printf("sensor bicycle %d...\n",i);
Near32 3:573a0dc8383f 280 }
Near32 3:573a0dc8383f 281
Near32 3:573a0dc8383f 282
Near32 3:573a0dc8383f 283 return (1.0/h)*H;
Near32 17:f360e21d3307 284 }
Near32 17:f360e21d3307 285
Near32 17:f360e21d3307 286 bool setPWM(PwmOut *servo,float p)
Near32 17:f360e21d3307 287 {
Near32 17:f360e21d3307 288 if(p <= 1.0f && p >= 0.0f)
Near32 17:f360e21d3307 289 {
Near32 17:f360e21d3307 290 servo->write(p);
Near32 17:f360e21d3307 291 return true;
Near32 17:f360e21d3307 292 }
Near32 17:f360e21d3307 293
Near32 17:f360e21d3307 294 return false;
Near32 3:573a0dc8383f 295 }