ARES / Mbed 2 deprecated RobotMain2015

a

Dependencies:   mbed

main.cpp@0:85567bbcebdb, 2014-12-14 (annotated)

Committer:
Jagang
Date:
Sun Dec 14 17:49:01 2014 +0000
Revision:
0:85567bbcebdb
New

Who changed what in which revision?

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