ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@25:c5773b021bb0, 2015-01-20 (annotated)

Committer:
Jagang
Date:
Tue Jan 20 14:34:23 2015 +0000
Revision:
25:c5773b021bb0
Parent:
24:3c0422e1ebd6 
Adding odometry

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