ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

main.cpp@15:ae29e83947ff, 2014-12-14 (annotated)

Committer:
Jagang
Date:
Sun Dec 14 17:01:57 2014 +0000
Revision:
15:ae29e83947ff
Parent:
13:ae5718ac8511
Child:
20:2840a749fb55
Migration

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 13:ae5718ac8511 4 #include "Map.h"
Jagang 0:41149573d577 5
Jagang 0:41149573d577 6
Near32 3:573a0dc8383f 7 /*---------------------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 8 /*---------------------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 9 /*KalmanFilter*/
Jagang 15:ae29e83947ff 10
Near32 3:573a0dc8383f 11 #include "EKF.h"
Near32 3:573a0dc8383f 12 Mat<double> motion_bicycle2( Mat<double> state, Mat<double> command, double dt = 0.5);
Near32 3:573a0dc8383f 13 Mat<double> sensor_bicycle2( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt = 0.5 );
Near32 3:573a0dc8383f 14 Mat<double> jmotion_bicycle2( Mat<double> state, Mat<double> command, double dt = 0.5);
Near32 3:573a0dc8383f 15 Mat<double> jsensor_bicycle2( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt = 0.5);
Near32 3:573a0dc8383f 16 void measurementCallback( Mat<double>* z, Odometry* odometry);
Near32 3:573a0dc8383f 17
Near32 3:573a0dc8383f 18 Mat<double> bicycle(3,1);
Near32 3:573a0dc8383f 19 /*---------------------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 20 /*---------------------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 21
Jagang 13:ae5718ac8511 22 /* --- Initialisation de la liste des obstable --- */
Jagang 13:ae5718ac8511 23 int Obstacle::lastId = 0;
Near32 3:573a0dc8383f 24
Jagang 0:41149573d577 25 int main()
Jagang 0:41149573d577 26 {
Near32 3:573a0dc8383f 27 /*----------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 28 /*Odometry*/
Jagang 0:41149573d577 29 QEI qei_left(p15,p16,NC,1024,QEI::X4_ENCODING);
Jagang 0:41149573d577 30 QEI qei_right(p17,p18,NC,1024,QEI::X4_ENCODING);
Jagang 0:41149573d577 31
Jagang 0:41149573d577 32 Odometry odometry(&qei_left,&qei_right,0.07,0.07,0.26);
Near32 3:573a0dc8383f 33 /*----------------------------------------------------------------------------------------------*/
Jagang 0:41149573d577 34
Near32 3:573a0dc8383f 35
Near32 3:573a0dc8383f 36
Near32 3:573a0dc8383f 37 /*----------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 38 /*KalmanFilter*/
Near32 8:1150a13f6967 39 double phi_max = 100;
Near32 3:573a0dc8383f 40 /*en millimetres*/
Near32 3:573a0dc8383f 41 bicycle.set((double)100, 1,1); /*radius*/
Near32 3:573a0dc8383f 42 bicycle.set((double)100, 2,1);
Near32 3:573a0dc8383f 43 bicycle.set((double)66, 3,1); /*entre-roue*/
Near32 3:573a0dc8383f 44
Near32 3:573a0dc8383f 45 int nbrstate = 5;
Near32 3:573a0dc8383f 46 int nbrcontrol = 2;
Near32 3:573a0dc8383f 47 int nbrobs = 3;
Near32 3:573a0dc8383f 48 double dt = (double)0.05;
Near32 8:1150a13f6967 49 double stdnoise = (double)0.05;
Near32 3:573a0dc8383f 50
Near32 3:573a0dc8383f 51 Mat<double> initX((double)0, nbrstate, 1);
Near32 3:573a0dc8383f 52 initX.set( (double)0, 3,1);
Near32 3:573a0dc8383f 53
Near32 3:573a0dc8383f 54 bool extended = true;
Near32 3:573a0dc8383f 55 EKF<double> instance(nbrstate, nbrcontrol, nbrobs, dt, stdnoise, /*current state*/ initX, extended);
Near32 3:573a0dc8383f 56
Near32 3:573a0dc8383f 57 instance.initMotion(motion_bicycle2);
Near32 3:573a0dc8383f 58 instance.initSensor(sensor_bicycle2);
Near32 3:573a0dc8383f 59 instance.initJMotion(jmotion_bicycle2);
Near32 3:573a0dc8383f 60 instance.initJSensor(jsensor_bicycle2);
Near32 3:573a0dc8383f 61
Near32 3:573a0dc8383f 62 /*desired State : (x y theta phiright phileft)*/
Near32 3:573a0dc8383f 63 Mat<double> dX((double)0, nbrstate, 1);
Near32 4:4025c071b207 64 dX.set( (double)0, 1,1);
Near32 4:4025c071b207 65 dX.set( (double)10, 2,1);
Near32 4:4025c071b207 66 dX.set( (double)0, 3,1);
Near32 3:573a0dc8383f 67 dX.set( (double)0, 4,1);
Near32 3:573a0dc8383f 68 dX.set( (double)0, 5,1);
Near32 3:573a0dc8383f 69
Near32 3:573a0dc8383f 70 Mat<double> u(transpose( instance.getCommand()) );
Near32 3:573a0dc8383f 71
Near32 3:573a0dc8383f 72 /*Observations*/
Near32 3:573a0dc8383f 73 Mat<double> z(3,1);
Near32 3:573a0dc8383f 74 measurementCallback(&z, &odometry);
Near32 3:573a0dc8383f 75
Near32 3:573a0dc8383f 76 /*----------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 77
Near32 3:573a0dc8383f 78
Near32 3:573a0dc8383f 79
Near32 3:573a0dc8383f 80
Near32 3:573a0dc8383f 81 /*----------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 82 /*Serial*/
Jagang 0:41149573d577 83 Serial pc(USBTX, USBRX); // tx, rx
Near32 3:573a0dc8383f 84 /*----------------------------------------------------------------------------------------------*/
Jagang 0:41149573d577 85
Jagang 0:41149573d577 86 while(1)
Jagang 0:41149573d577 87 {
Jagang 0:41149573d577 88 wait(1);
Jagang 0:41149573d577 89 pc.printf("%f : %f : %f\n",odometry.getX()*100,odometry.getY()*100,odometry.getTheta()*180/3.14);
Near32 3:573a0dc8383f 90
Near32 3:573a0dc8383f 91
Near32 3:573a0dc8383f 92 /*------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 93 /*Asservissement*/
Near32 3:573a0dc8383f 94 measurementCallback(&z, &odometry);
Near32 3:573a0dc8383f 95 instance.measurement_Callback( z, dX );
Near32 3:573a0dc8383f 96 instance.state_Callback();
Near32 10:bca0274a007b 97 double phi_r = instance.getCommand().get(1,1);
Near32 10:bca0274a007b 98 double phi_l = instance.getCommand().get(2,1);
Near32 8:1150a13f6967 99
Near32 3:573a0dc8383f 100
Near32 3:573a0dc8383f 101 instance.computeCommand(dX, (double)dt, -1);
Near32 11:5513638d3c13 102 pc.printf("command : \n phi_r = %f \n phi_l = %f \n", phi_r/phi_max*100, phi_l/phi_max*100);
Near32 3:573a0dc8383f 103
Near32 3:573a0dc8383f 104 /*------------------------------------------------------------------------------------------*/
Near32 3:573a0dc8383f 105
Jagang 0:41149573d577 106 }
Jagang 0:41149573d577 107 }
Near32 3:573a0dc8383f 108
Near32 3:573a0dc8383f 109 void measurementCallback( Mat<double>* z, Odometry* odometry)
Near32 3:573a0dc8383f 110 {
Near32 3:573a0dc8383f 111 z->set( (double)/*conversionUnitée mm */odometry->getX(), 1,1);
Near32 3:573a0dc8383f 112 z->set( (double)/*conversionUnitée mm*/odometry->getY(), 2,1);
Near32 3:573a0dc8383f 113 z->set( (double)/*conversionUnitée rad*/odometry->getTheta(), 3,1);
Near32 3:573a0dc8383f 114 }
Near32 3:573a0dc8383f 115
Near32 3:573a0dc8383f 116 Mat<double> motion_bicycle2( Mat<double> state, Mat<double> command, double dt)
Near32 3:573a0dc8383f 117 {
Near32 3:573a0dc8383f 118 Mat<double> r(state);
Near32 3:573a0dc8383f 119 double v = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)+r.get(5,1));
Near32 3:573a0dc8383f 120 //double w = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)-r.get(5,1));
Near32 3:573a0dc8383f 121
Near32 3:573a0dc8383f 122 r.set( r.get(1,1) + v*cos(r.get(3,1))*dt, 1,1);
Near32 3:573a0dc8383f 123 r.set( r.get(2,1) + v*sin(r.get(3,1))*dt, 2,1);
Near32 3:573a0dc8383f 124
Near32 3:573a0dc8383f 125 double angle = (r.get(3,1) + dt/bicycle.get(3,1)*(r.get(4,1)-r.get(5,1)));
Near32 3:573a0dc8383f 126 if( angle < -PI)
Near32 3:573a0dc8383f 127 {
Near32 3:573a0dc8383f 128 angle = angle - PI*ceil(angle/PI);
Near32 3:573a0dc8383f 129 }
Near32 3:573a0dc8383f 130 else if( angle > PI)
Near32 3:573a0dc8383f 131 {
Near32 3:573a0dc8383f 132 angle = angle - PI*floor(angle/PI);
Near32 3:573a0dc8383f 133 }
Near32 3:573a0dc8383f 134
Near32 3:573a0dc8383f 135 r.set( angle, 3,1);
Near32 3:573a0dc8383f 136
Near32 3:573a0dc8383f 137 r.set( bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)+command.get(2,1)), 4,1);
Near32 3:573a0dc8383f 138 r.set( bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)-command.get(2,1)), 5,1);
Near32 3:573a0dc8383f 139
Near32 3:573a0dc8383f 140 return r;
Near32 3:573a0dc8383f 141 }
Near32 3:573a0dc8383f 142
Near32 3:573a0dc8383f 143
Near32 3:573a0dc8383f 144 Mat<double> sensor_bicycle2( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
Near32 3:573a0dc8383f 145 {
Near32 3:573a0dc8383f 146 return extract(state-d_state, 1,1, 3,1);
Near32 3:573a0dc8383f 147 }
Near32 3:573a0dc8383f 148
Near32 3:573a0dc8383f 149 Mat<double> jmotion_bicycle2( Mat<double> state, Mat<double> command, double dt)
Near32 3:573a0dc8383f 150 {
Near32 3:573a0dc8383f 151 double h = numeric_limits<double>::epsilon()*10e2;
Near32 3:573a0dc8383f 152 Mat<double> var( (double)0, state.getLine(), state.getColumn());
Near32 3:573a0dc8383f 153 var.set( h, 1,1);
Near32 3:573a0dc8383f 154 Mat<double> G(motion_bicycle2(state, command, dt) - motion_bicycle2(state+var, command,dt));
Near32 3:573a0dc8383f 155
Near32 3:573a0dc8383f 156 for(int i=2;i<=state.getLine();i++)
Near32 3:573a0dc8383f 157 {
Near32 3:573a0dc8383f 158 var.set( (double)0, i-1,1);
Near32 3:573a0dc8383f 159 var.set( h, i,1);
Near32 3:573a0dc8383f 160 G = operatorL(G, motion_bicycle2(state, command, dt) - motion_bicycle2(state+var, command,dt) );
Near32 3:573a0dc8383f 161 }
Near32 3:573a0dc8383f 162
Near32 3:573a0dc8383f 163
Near32 3:573a0dc8383f 164 return (1.0/h)*G;
Near32 3:573a0dc8383f 165 }
Near32 3:573a0dc8383f 166
Near32 3:573a0dc8383f 167 Mat<double> jsensor_bicycle2( Mat<double> state, Mat<double> command, Mat<double> d_state, double dt)
Near32 3:573a0dc8383f 168 {
Near32 3:573a0dc8383f 169 double h = numeric_limits<double>::epsilon()*10e2;
Near32 3:573a0dc8383f 170 Mat<double> var((double)0, state.getLine(), state.getColumn());
Near32 3:573a0dc8383f 171 var.set( h, 1,1);
Near32 3:573a0dc8383f 172 Mat<double> H(sensor_bicycle2(state, command, d_state, dt) - sensor_bicycle2(state+var, command, d_state, dt));
Near32 3:573a0dc8383f 173
Near32 3:573a0dc8383f 174 for(int i=2;i<=state.getLine();i++)
Near32 3:573a0dc8383f 175 {
Near32 3:573a0dc8383f 176 var.set( (double)0, i-1,1);
Near32 3:573a0dc8383f 177 var.set( h, i,1);
Near32 3:573a0dc8383f 178 H = operatorL(H, sensor_bicycle2(state, command, d_state, dt) - sensor_bicycle2(state+var, command, d_state, dt) );
Near32 3:573a0dc8383f 179 }
Near32 3:573a0dc8383f 180
Near32 3:573a0dc8383f 181
Near32 3:573a0dc8383f 182 return (1.0/h)*H;
Near32 3:573a0dc8383f 183 }