ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@31:e60cd1c984f4, 2015-02-03 (annotated)

Committer:
Near32
Date:
Tue Feb 03 18:44:33 2015 +0000
Revision:
31:e60cd1c984f4
Parent:
30:33e970ba1fe5
Child:
32:148147c0755e
PID test

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 26:54e1afed58b2 68 pw1.write(0.0);
Near32 26:54e1afed58b2 69 pw2.write(0.0);
Near32 17:f360e21d3307 70 //setPWM(&pw1,0.9);
Near32 24:3c0422e1ebd6 71 pcs.printf("mise a jour des pwm.\n");
Near32 26:54e1afed58b2 72 Timer t;
Near32 24:3c0422e1ebd6 73
Near32 3:573a0dc8383f 74 /*----------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 75 /*Odometry*/
Near32 28:0a659a910771 76 QEI qei_left(D2,D3,NC,1024,QEI::X4_ENCODING);
Near32 28:0a659a910771 77 QEI qei_right(D4,D5,NC,1024,QEI::X4_ENCODING);
Near32 31:e60cd1c984f4 78 Odometry odometry(&qei_left,&qei_right,63,63,280);
Near32 31:e60cd1c984f4 79 bool testOdo = false;
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 24:3c0422e1ebd6 86 //double phi_max = 100;
Near32 3:573a0dc8383f 87 /*en millimetres*/
Near32 26:54e1afed58b2 88 bicycle.set((double)36, 1,1); /*radius*/
Near32 26:54e1afed58b2 89 bicycle.set((double)36, 2,1);
Near32 26:54e1afed58b2 90 bicycle.set((double)220, 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 24:3c0422e1ebd6 102 bool filterOn = true;
Near32 24:3c0422e1ebd6 103 pcs.printf("mise a jour des pwm.\n");
Near32 24:3c0422e1ebd6 104 EKF<double> instance(nbrstate, nbrcontrol, nbrobs, dt, stdnoise, /*current state*/ initX, extended, filterOn);
Near32 3:573a0dc8383f 105
Near32 17:f360e21d3307 106 instance.initMotion(motion_bicycle3);
Near32 17:f360e21d3307 107 instance.initSensor(sensor_bicycle3);
Near32 17:f360e21d3307 108 instance.initJMotion(jmotion_bicycle3);
Near32 24:3c0422e1ebd6 109 //instance.initJMotionCommand(jmotion_bicycle3_command);
Near32 17:f360e21d3307 110 instance.initJSensor(jsensor_bicycle3);
Near32 3:573a0dc8383f 111
Near32 3:573a0dc8383f 112 /*desired State : (x y theta phiright phileft)*/
Near32 3:573a0dc8383f 113 Mat<double> dX((double)0, nbrstate, 1);
Near32 31:e60cd1c984f4 114 dX.set( (double)50, 1,1);
Near32 17:f360e21d3307 115 dX.set( (double)0, 2,1);
Near32 31:e60cd1c984f4 116 dX.set( (double)0, 3,1);
Near32 3:573a0dc8383f 117 dX.set( (double)0, 4,1);
Near32 3:573a0dc8383f 118 dX.set( (double)0, 5,1);
Near32 3:573a0dc8383f 119
Near32 17:f360e21d3307 120 Mat<double> ki((double)0, nbrcontrol, nbrstate);
Near32 17:f360e21d3307 121 Mat<double> kp((double)0, nbrcontrol, nbrstate);
Near32 17:f360e21d3307 122 Mat<double> kd((double)0, nbrcontrol, nbrstate);
Near32 17:f360e21d3307 123 //Mat<double> kdd((double)0.0015, nbrcontrol, nbrstate);
Near32 17:f360e21d3307 124
Near32 17:f360e21d3307 125 for(int i=1;i<=nbrstate;i++)
Near32 17:f360e21d3307 126 {
Near32 17:f360e21d3307 127 kp.set( (double)0.01, i, i);
Near32 17:f360e21d3307 128 kd.set( (double)0.0001, i, i);
Near32 17:f360e21d3307 129 ki.set( (double)0.0001, i, i);
Near32 17:f360e21d3307 130 }
Near32 17:f360e21d3307 131
Near32 17:f360e21d3307 132 instance.setKi(ki);
Near32 17:f360e21d3307 133 instance.setKp(kp);
Near32 17:f360e21d3307 134 instance.setKd(kd);
Near32 17:f360e21d3307 135 //instance.setKdd(kdd);
Near32 17:f360e21d3307 136
Near32 3:573a0dc8383f 137 Mat<double> u(transpose( instance.getCommand()) );
Near32 3:573a0dc8383f 138
Near32 3:573a0dc8383f 139 /*Observations*/
Near32 28:0a659a910771 140 /*il nous faut 5 observation : mais on n'en met à jour que 3...*/
Near32 17:f360e21d3307 141 Mat<double> z((double)0,5,1);
Near32 28:0a659a910771 142 measurementCallback(&z, &odometry);
Near32 3:573a0dc8383f 143
Near32 3:573a0dc8383f 144 /*----------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 145
Near32 3:573a0dc8383f 146
Near32 30:33e970ba1fe5 147 while(abs(instance.getX().get(1,1)-50) > 10)
Near32 26:54e1afed58b2 148 {
Near32 26:54e1afed58b2 149 t.start();
Near32 24:3c0422e1ebd6 150 //pcs.printf("%f : %f : %f\n", (double)odometry.getX()*100,(double)odometry.getY()*100,(double)odometry.getTheta()*180/3.14);
Near32 17:f360e21d3307 151
Near32 3:573a0dc8383f 152
Near32 3:573a0dc8383f 153 /*------------------------------------------------------------------------------------------*/
Near32 17:f360e21d3307 154 /*Asservissement*/
Near32 17:f360e21d3307 155
Near32 28:0a659a910771 156 measurementCallback(&z, &odometry);
Near32 28:0a659a910771 157 instance.measurement_Callback( z, dX, true );
Near32 17:f360e21d3307 158
Near32 3:573a0dc8383f 159 instance.state_Callback();
Near32 30:33e970ba1fe5 160 //instance.setX(z);
Near32 30:33e970ba1fe5 161
Near32 30:33e970ba1fe5 162 //instance.computeCommand(dX, (double)dt, -2);
Near32 30:33e970ba1fe5 163 instance.computeCommand(dX, (double)t.read(), -2);
Near32 10:bca0274a007b 164 double phi_r = instance.getCommand().get(1,1);
Near32 10:bca0274a007b 165 double phi_l = instance.getCommand().get(2,1);
Near32 8:1150a13f6967 166
Near32 26:54e1afed58b2 167 double phi_max = 1.0;
Near32 30:33e970ba1fe5 168
Near32 26:54e1afed58b2 169 //pcs.printf("command : \n phi_r = %f \n phi_l = %f \n", ((double)phi_r/phi_max), ((double)phi_l/phi_max));
Near32 30:33e970ba1fe5 170 //(instance.getCommand()).afficher();
Near32 31:e60cd1c984f4 171 blue.printf("State : \n\r");
Near32 31:e60cd1c984f4 172 (instance.getX()).afficherMblue();
Near32 31:e60cd1c984f4 173 blue.printf("Odometry : \n\r");
Near32 31:e60cd1c984f4 174 z.afficherMblue();
Near32 31:e60cd1c984f4 175 //blue.printf(" x : %f ;\t y : %f ;\t theta : %f \n\r",z.get(1,1),z.get(2,1),z.get(3,1) );
Near32 30:33e970ba1fe5 176 //pcs.printf("Sigma : \n\r");
Near32 30:33e970ba1fe5 177 //(instance.getSigma()).afficher();
Near32 30:33e970ba1fe5 178 pcs.printf("Kalman Gain : \n\r");
Near32 30:33e970ba1fe5 179 (instance.getKGain()).afficherM();
Near32 17:f360e21d3307 180
Near32 17:f360e21d3307 181
Near32 24:3c0422e1ebd6 182 if(phi_r >= 0)
Near32 24:3c0422e1ebd6 183 dir1.write(1);
Near32 24:3c0422e1ebd6 184 else
Near32 17:f360e21d3307 185 dir1.write(0);
Near32 17:f360e21d3307 186
Near32 17:f360e21d3307 187 if(phi_l <= 0)
Near32 17:f360e21d3307 188 dir2.write(0);
Near32 17:f360e21d3307 189 else
Near32 17:f360e21d3307 190 dir2.write(1);
Near32 17:f360e21d3307 191
Near32 31:e60cd1c984f4 192 if(!testOdo)
Near32 31:e60cd1c984f4 193 {
Near32 31:e60cd1c984f4 194 if(abs(phi_r/phi_max) <= 1.0)
Near32 31:e60cd1c984f4 195 {
Near32 31:e60cd1c984f4 196 //pcs.printf( "VALUE PWM 1 : %f \n", (float)abs((double)phi_r/phi_max)) ;
Near32 31:e60cd1c984f4 197 setPWM(&pw1, (float)abs((double)phi_r/phi_max));
Near32 31:e60cd1c984f4 198 }
Near32 31:e60cd1c984f4 199 else
Near32 31:e60cd1c984f4 200 {
Near32 31:e60cd1c984f4 201 pcs.printf("P1...");
Near32 31:e60cd1c984f4 202 setPWM(&pw1, (float)abs((double)phi_max/phi_max));
Near32 31:e60cd1c984f4 203 }
Near32 31:e60cd1c984f4 204
Near32 31:e60cd1c984f4 205 if(abs(phi_l/phi_max) <= 1.0)
Near32 31:e60cd1c984f4 206 {
Near32 31:e60cd1c984f4 207 //pcs.printf( "VALUE PWM 2 : %f \n", (float)abs((double)phi_l/phi_max)) ;
Near32 31:e60cd1c984f4 208 setPWM(&pw2,(float)abs((double)phi_l/phi_max));
Near32 31:e60cd1c984f4 209 }
Near32 31:e60cd1c984f4 210 else
Near32 31:e60cd1c984f4 211 {
Near32 31:e60cd1c984f4 212 pcs.printf("P2...");
Near32 31:e60cd1c984f4 213 setPWM(&pw2, (float)abs((double)phi_max/phi_max));
Near32 31:e60cd1c984f4 214 }
Near32 30:33e970ba1fe5 215 }
Near32 17:f360e21d3307 216
Near32 26:54e1afed58b2 217 //pcs.printf("\n\n----------------- Commande mise executee. ------------------ \n");
Near32 26:54e1afed58b2 218 t.stop();
Near32 26:54e1afed58b2 219 pcs.printf("%f s \n\r", t.read());
Near32 26:54e1afed58b2 220 t.reset();
Near32 30:33e970ba1fe5 221 t.start();
Near32 26:54e1afed58b2 222
Near32 26:54e1afed58b2 223 }
Near32 3:573a0dc8383f 224
Near32 26:54e1afed58b2 225
Near32 26:54e1afed58b2 226 DigitalOut led(LED1);
Near32 26:54e1afed58b2 227 while(1)
Near32 26:54e1afed58b2 228 {
Near32 26:54e1afed58b2 229 //setPWM(&pw1,0.0);
Near32 26:54e1afed58b2 230 //setPWM(&pw2,0.0);
Near32 26:54e1afed58b2 231 pw1.write(0.0);
Near32 26:54e1afed58b2 232 pw2.write(0.0);
Near32 26:54e1afed58b2 233 led = !led;
Near32 26:54e1afed58b2 234 wait(1);
Jagang 0:41149573d577 235 }
Jagang 0:41149573d577 236 }
Near32 3:573a0dc8383f 237
Near32 3:573a0dc8383f 238 void measurementCallback( Mat<double>* z, Odometry* odometry)
Near32 3:573a0dc8383f 239 {
Near32 3:573a0dc8383f 240 z->set( (double)/*conversionUnitée mm */odometry->getX(), 1,1);
Near32 3:573a0dc8383f 241 z->set( (double)/*conversionUnitée mm*/odometry->getY(), 2,1);
Near32 31:e60cd1c984f4 242 double theta = odometry->getTheta();
Near32 31:e60cd1c984f4 243 theta = atan21(sin(theta),cos(theta));
Near32 31:e60cd1c984f4 244 z->set( (double)/*conversionUnitée rad*/theta, 3,1);//odometry->getTheta(), 3,1);
Near32 30:33e970ba1fe5 245 double vx = (double)odometry->getVx();
Near32 30:33e970ba1fe5 246 double vy = (double)odometry->getVy();
Near32 30:33e970ba1fe5 247 z->set( sqrt(vx*vx+vy*vy),4,1);
Near32 30:33e970ba1fe5 248 z->set( (double)odometry->getW(),5,1);
Near32 30:33e970ba1fe5 249
Near32 31:e60cd1c984f4 250 //z->afficherM();
Near32 3:573a0dc8383f 251 }
Near32 3:573a0dc8383f 252
Near32 17:f360e21d3307 253 Mat<double> motion_bicycle3( Mat<double> state, Mat<double> command, double dt)
Near32 3:573a0dc8383f 254 {
Near32 3:573a0dc8383f 255 Mat<double> r(state);
Near32 24:3c0422e1ebd6 256 //double v = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)+r.get(5,1));
Near32 24:3c0422e1ebd6 257 //double w = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)-r.get(5,1));
Near32 24:3c0422e1ebd6 258 double v = state.get(4,1);
Near32 24:3c0422e1ebd6 259 double w = state.get(5,1);
Near32 3:573a0dc8383f 260
Near32 3:573a0dc8383f 261 r.set( r.get(1,1) + v*cos(r.get(3,1))*dt, 1,1);
Near32 3:573a0dc8383f 262 r.set( r.get(2,1) + v*sin(r.get(3,1))*dt, 2,1);
Near32 3:573a0dc8383f 263
Near32 17:f360e21d3307 264 double angle = (r.get(3,1) + dt*w);
Near32 3:573a0dc8383f 265 if( angle < -PI)
Near32 3:573a0dc8383f 266 {
Near32 3:573a0dc8383f 267 angle = angle - PI*ceil(angle/PI);
Near32 3:573a0dc8383f 268 }
Near32 3:573a0dc8383f 269 else if( angle > PI)
Near32 3:573a0dc8383f 270 {
Near32 3:573a0dc8383f 271 angle = angle - PI*floor(angle/PI);
Near32 3:573a0dc8383f 272 }
Near32 3:573a0dc8383f 273
Near32 17:f360e21d3307 274 r.set( atan21(sin(angle), cos(angle)), 3,1);
Near32 3:573a0dc8383f 275
Near32 17:f360e21d3307 276
Near32 17:f360e21d3307 277 /*----------------------------------------*/
Near32 17:f360e21d3307 278 /*Modele du moteur*/
Near32 17:f360e21d3307 279 /*----------------------------------------*/
Near32 24:3c0422e1ebd6 280 //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 281 //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 282 double r1 = bicycle.get(1,1)/2*(command.get(1,1)+command.get(2,1));
Near32 24:3c0422e1ebd6 283 double r2 = bicycle.get(1,1)/bicycle.get(3,1)*(command.get(1,1)-command.get(2,1));
Near32 17:f360e21d3307 284
Near32 17:f360e21d3307 285
Near32 17:f360e21d3307 286 r.set( r1, 4,1);
Near32 17:f360e21d3307 287 r.set( r2, 5,1);
Near32 17:f360e21d3307 288
Near32 17:f360e21d3307 289
Near32 17:f360e21d3307 290 /*----------------------------------------*/
Near32 17:f360e21d3307 291 /*----------------------------------------*/
Near32 3:573a0dc8383f 292
Near32 3:573a0dc8383f 293 return r;
Near32 3:573a0dc8383f 294 }
Near32 3:573a0dc8383f 295
Near32 3:573a0dc8383f 296
Near32 17:f360e21d3307 297 Mat<double> sensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
Near32 17:f360e21d3307 298 {
Near32 30:33e970ba1fe5 299 /*
Near32 28:0a659a910771 300 double angle = state.get(3,1);
Near32 28:0a659a910771 301 if( angle < -PI)
Near32 28:0a659a910771 302 {
Near32 28:0a659a910771 303 angle = angle - PI*ceil(angle/PI);
Near32 28:0a659a910771 304 }
Near32 28:0a659a910771 305 else if( angle > PI)
Near32 28:0a659a910771 306 {
Near32 28:0a659a910771 307 angle = angle - PI*floor(angle/PI);
Near32 28:0a659a910771 308 }
Near32 28:0a659a910771 309
Near32 28:0a659a910771 310 state.set( atan21(sin(angle), cos(angle)), 3,1);
Near32 30:33e970ba1fe5 311 */
Near32 17:f360e21d3307 312 return state;
Near32 3:573a0dc8383f 313 }
Near32 3:573a0dc8383f 314
Near32 17:f360e21d3307 315
Near32 17:f360e21d3307 316 Mat<double> jmotion_bicycle3( Mat<double> state, Mat<double> command, double dt)
Near32 3:573a0dc8383f 317 {
Near32 30:33e970ba1fe5 318 double h = sqrt(numeric_limits<double>::epsilon())*norme2(state);
Near32 3:573a0dc8383f 319 Mat<double> var( (double)0, state.getLine(), state.getColumn());
Near32 3:573a0dc8383f 320 var.set( h, 1,1);
Near32 30:33e970ba1fe5 321 Mat<double> G(motion_bicycle3(state+var, command, dt) - motion_bicycle3(state-var, command,dt));
Near32 3:573a0dc8383f 322
Near32 3:573a0dc8383f 323 for(int i=2;i<=state.getLine();i++)
Near32 3:573a0dc8383f 324 {
Near32 3:573a0dc8383f 325 var.set( (double)0, i-1,1);
Near32 3:573a0dc8383f 326 var.set( h, i,1);
Near32 30:33e970ba1fe5 327 G = operatorL(G, motion_bicycle3(state+var, command, dt) - motion_bicycle3(state-var, command,dt) );
Near32 3:573a0dc8383f 328 }
Near32 3:573a0dc8383f 329
Near32 3:573a0dc8383f 330
Near32 30:33e970ba1fe5 331 return (1.0/(2*h))*G;
Near32 3:573a0dc8383f 332 }
Near32 3:573a0dc8383f 333
Near32 24:3c0422e1ebd6 334 Mat<double> jmotion_bicycle3_command(Mat<double> state, Mat<double> command, double dt)
Near32 24:3c0422e1ebd6 335 {
Near32 30:33e970ba1fe5 336 double h = sqrt(numeric_limits<double>::epsilon())*10e2+sqrt(numeric_limits<double>::epsilon())*norme2(state);
Near32 24:3c0422e1ebd6 337 Mat<double> var( (double)0, command.getLine(), command.getColumn());
Near32 24:3c0422e1ebd6 338 var.set( h, 1,1);
Near32 24:3c0422e1ebd6 339 Mat<double> G(motion_bicycle3(state, command+var, dt) - motion_bicycle3(state, command-var,dt));
Near32 24:3c0422e1ebd6 340
Near32 24:3c0422e1ebd6 341 for(int i=2;i<=command.getLine();i++)
Near32 24:3c0422e1ebd6 342 {
Near32 24:3c0422e1ebd6 343 var.set( (double)0, i-1,1);
Near32 24:3c0422e1ebd6 344 var.set( h, i,1);
Near32 30:33e970ba1fe5 345 G = operatorL(G, motion_bicycle3(state, command+var, dt) - motion_bicycle3(state, command-var,dt) );
Near32 24:3c0422e1ebd6 346 }
Near32 24:3c0422e1ebd6 347
Near32 24:3c0422e1ebd6 348
Near32 24:3c0422e1ebd6 349 return (1.0/(2*h))*G;
Near32 24:3c0422e1ebd6 350 }
Near32 24:3c0422e1ebd6 351
Near32 17:f360e21d3307 352 Mat<double> jsensor_bicycle3( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
Near32 3:573a0dc8383f 353 {
Near32 30:33e970ba1fe5 354 double h = sqrt(numeric_limits<double>::epsilon())*10e2+sqrt(numeric_limits<double>::epsilon())*norme2(state);
Near32 3:573a0dc8383f 355 Mat<double> var((double)0, state.getLine(), state.getColumn());
Near32 3:573a0dc8383f 356 var.set( h, 1,1);
Near32 24:3c0422e1ebd6 357 Mat<double> H(sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt));
Near32 3:573a0dc8383f 358
Near32 3:573a0dc8383f 359 for(int i=2;i<=state.getLine();i++)
Near32 24:3c0422e1ebd6 360 {
Near32 3:573a0dc8383f 361 var.set( (double)0, i-1,1);
Near32 3:573a0dc8383f 362 var.set( h, i,1);
Near32 24:3c0422e1ebd6 363 H = operatorL(H, sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt) );
Near32 3:573a0dc8383f 364 }
Near32 3:573a0dc8383f 365
Near32 3:573a0dc8383f 366
Near32 24:3c0422e1ebd6 367 return (1.0/(2*h))*H;
Near32 30:33e970ba1fe5 368 /*
Near32 30:33e970ba1fe5 369 double h = sqrt(numeric_limits<double>::epsilon())*10e2+sqrt(numeric_limits<double>::epsilon())*norme2(state);
Near32 30:33e970ba1fe5 370 Mat<double> var((double)0, state.getLine(), state.getColumn());
Near32 30:33e970ba1fe5 371 var.set( h, 1,1);
Near32 30:33e970ba1fe5 372 Mat<double> H(sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt));
Near32 30:33e970ba1fe5 373
Near32 30:33e970ba1fe5 374 for(int i=2;i<=state.getLine();i++)
Near32 30:33e970ba1fe5 375 {
Near32 30:33e970ba1fe5 376 var.set( (double)0, i-1,1);
Near32 30:33e970ba1fe5 377 var.set( h, i,1);
Near32 30:33e970ba1fe5 378 H = operatorL(H, sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt) );
Near32 30:33e970ba1fe5 379
Near32 30:33e970ba1fe5 380 }
Near32 30:33e970ba1fe5 381
Near32 30:33e970ba1fe5 382
Near32 30:33e970ba1fe5 383 return (1.0/(2*h))*H;
Near32 30:33e970ba1fe5 384 */
Near32 17:f360e21d3307 385 }
Near32 17:f360e21d3307 386
Near32 17:f360e21d3307 387 bool setPWM(PwmOut *servo,float p)
Near32 17:f360e21d3307 388 {
Near32 17:f360e21d3307 389 if(p <= 1.0f && p >= 0.0f)
Near32 17:f360e21d3307 390 {
Near32 17:f360e21d3307 391 servo->write(p);
Near32 17:f360e21d3307 392 return true;
Near32 17:f360e21d3307 393 }
Near32 17:f360e21d3307 394
Near32 17:f360e21d3307 395 return false;
Near32 3:573a0dc8383f 396 }