ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@24:3c0422e1ebd6, 2015-01-15 (annotated)

Committer:
Near32
Date:
Thu Jan 15 15:58:25 2015 +0000
Revision:
24:3c0422e1ebd6
Parent:
23:228317fe0457
Child:
25:c5773b021bb0
Child:
26:54e1afed58b2
bugs fixed : ; Compilation nucleo; port serie monitoring

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