ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asservissement/Asserv.h@91:c0b436c52ede, 2015-04-24 (annotated)

Committer:
Near32
Date:
Fri Apr 24 10:24:09 2015 +0000
Revision:
91:c0b436c52ede
Parent:
90:294b16267109
Child:
95:33520ead3b94
Child:
100:a827a645d6c2
testing pid.... end.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Near32 36:54f86bc6fd80 1 /*KalmanFilter*/
Near32 36:54f86bc6fd80 2 #include "EKF.h"
Near32 36:54f86bc6fd80 3 #include "Odometry.h"
Near32 89:d05001d85a02 4 //#define debugAsserv
Near32 91:c0b436c52ede 5 #define verboseGOAL
Near32 89:d05001d85a02 6
Near32 43:87bdce65341f 7 /*
Near32 36:54f86bc6fd80 8 template<typename T>
Near32 36:54f86bc6fd80 9 Mat<T> motion_bicycle3( Mat<T> state, Mat<T> command, T dt = (T)0.5);
Near32 36:54f86bc6fd80 10 template<typename T>
Near32 36:54f86bc6fd80 11 Mat<T> sensor_bicycle3( Mat<T> state, Mat<T> command, Mat<T> d_state, T dt = 0.5 );
Near32 36:54f86bc6fd80 12 template<typename T>
Near32 36:54f86bc6fd80 13 Mat<T> jmotion_bicycle3( Mat<T> state, Mat<T> command, T dt = 0.5);
Near32 36:54f86bc6fd80 14 template<typename T>
Near32 36:54f86bc6fd80 15 Mat<T> jmotion_bicycle3_command(Mat<T> state, Mat<T> command, T dt = 0.5);
Near32 36:54f86bc6fd80 16 template<typename T>
Near32 36:54f86bc6fd80 17 Mat<T> jsensor_bicycle3( Mat<T> state, Mat<T> command, Mat<T> d_state, T dt = 0.5);
Near32 36:54f86bc6fd80 18 template<typename T>
Near32 36:54f86bc6fd80 19 bool setPWM(PwmOut *servo,T p);
Near32 43:87bdce65341f 20 */
Near32 36:54f86bc6fd80 21
Near32 36:54f86bc6fd80 22 template<typename T>
Near32 36:54f86bc6fd80 23 bool setPWM(PwmOut *servo,T p)
Near32 36:54f86bc6fd80 24 {
Near32 36:54f86bc6fd80 25 if(p <= (T)1.0 && p >= (T)0.0)
Near32 36:54f86bc6fd80 26 {
Near32 36:54f86bc6fd80 27 servo->write((float)p);
Near32 36:54f86bc6fd80 28 return true;
Near32 36:54f86bc6fd80 29 }
Near32 36:54f86bc6fd80 30
Near32 36:54f86bc6fd80 31 return false;
Near32 36:54f86bc6fd80 32 }
Near32 36:54f86bc6fd80 33
Near32 36:54f86bc6fd80 34 template<typename T>
Near32 36:54f86bc6fd80 35 Mat<T> motion_bicycle3( Mat<T> state, Mat<T> command, T dt)
Near32 43:87bdce65341f 36 {
Near32 36:54f86bc6fd80 37 Mat<T> bicycle(3,1);
Near32 36:54f86bc6fd80 38 bicycle.set((T)36, 1,1); /*radius*/
Near32 36:54f86bc6fd80 39 bicycle.set((T)36, 2,1);
Near32 36:54f86bc6fd80 40 bicycle.set((T)220, 3,1); /*entre-roue*/
Near32 47:4909e97570f6 41 Mat<T> r(state);
Near32 47:4909e97570f6 42
Near32 47:4909e97570f6 43 /*state v w */
Near32 47:4909e97570f6 44 /*
Near32 36:54f86bc6fd80 45 T v = state.get(4,1);
Near32 36:54f86bc6fd80 46 T w = state.get(5,1);
Near32 47:4909e97570f6 47 */
Near32 47:4909e97570f6 48 /*state phi_l phi_r */
Near32 47:4909e97570f6 49 T v = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)+r.get(5,1));
Near32 47:4909e97570f6 50 T w = bicycle.get(1,1)/(2*bicycle.get(3,1))*(r.get(4,1)-r.get(5,1));
Near32 36:54f86bc6fd80 51
Near32 36:54f86bc6fd80 52 r.set( r.get(1,1) + v*cos(r.get(3,1))*dt, 1,1);
Near32 36:54f86bc6fd80 53 r.set( r.get(2,1) + v*sin(r.get(3,1))*dt, 2,1);
Near32 36:54f86bc6fd80 54
Near32 36:54f86bc6fd80 55 T angle = (r.get(3,1) + dt*w);
Near32 89:d05001d85a02 56 if( angle < -(T)PI)
Near32 36:54f86bc6fd80 57 {
Near32 89:d05001d85a02 58 angle = angle - (T)PI*ceil(angle/PI);
Near32 36:54f86bc6fd80 59 }
Near32 89:d05001d85a02 60 else if( angle > (T)PI)
Near32 36:54f86bc6fd80 61 {
Near32 89:d05001d85a02 62 angle = angle - (T)PI*floor(angle/PI);
Near32 36:54f86bc6fd80 63 }
Near32 36:54f86bc6fd80 64
Near32 81:08eacb6179d8 65 r.set( angle, 3,1);
Near32 36:54f86bc6fd80 66
Near32 36:54f86bc6fd80 67
Near32 36:54f86bc6fd80 68 /*----------------------------------------*/
Near32 36:54f86bc6fd80 69 /*Modele du moteur*/
Near32 36:54f86bc6fd80 70 /*----------------------------------------*/
Near32 36:54f86bc6fd80 71 //double r1 = bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)+command.get(2,1));
Near32 47:4909e97570f6 72 //double r2 = bicycle.get(3,1)/bicycle.get(1,1)*(command.get(1,1)/bicycle.get(3,1)-command.get(2,1));
Near32 47:4909e97570f6 73 /*state v w */
Near32 47:4909e97570f6 74 /*
Near32 36:54f86bc6fd80 75 T r1 = bicycle.get(1,1)/2*(command.get(1,1)+command.get(2,1));
Near32 36:54f86bc6fd80 76 T r2 = bicycle.get(1,1)/bicycle.get(3,1)*(command.get(1,1)-command.get(2,1));
Near32 47:4909e97570f6 77 */
Near32 47:4909e97570f6 78 /*state phi_l phi_r*/
Near32 47:4909e97570f6 79 T r1 = command.get(1,1);
Near32 47:4909e97570f6 80 T r2 = command.get(2,1);
Near32 36:54f86bc6fd80 81
Near32 36:54f86bc6fd80 82
Near32 36:54f86bc6fd80 83 r.set( r1, 4,1);
Near32 36:54f86bc6fd80 84 r.set( r2, 5,1);
Near32 36:54f86bc6fd80 85
Near32 36:54f86bc6fd80 86
Near32 36:54f86bc6fd80 87 /*----------------------------------------*/
Near32 36:54f86bc6fd80 88 /*----------------------------------------*/
Near32 36:54f86bc6fd80 89
Near32 36:54f86bc6fd80 90 return r;
Near32 36:54f86bc6fd80 91 }
Near32 36:54f86bc6fd80 92
Near32 36:54f86bc6fd80 93 template<typename T>
Near32 36:54f86bc6fd80 94 Mat<T> sensor_bicycle3( Mat<T> state, Mat<T> command, Mat<T> d_state, T dt)
Near32 36:54f86bc6fd80 95 {
Near32 36:54f86bc6fd80 96 /*
Near32 36:54f86bc6fd80 97 double angle = state.get(3,1);
Near32 36:54f86bc6fd80 98 if( angle < -PI)
Near32 36:54f86bc6fd80 99 {
Near32 36:54f86bc6fd80 100 angle = angle - PI*ceil(angle/PI);
Near32 36:54f86bc6fd80 101 }
Near32 36:54f86bc6fd80 102 else if( angle > PI)
Near32 36:54f86bc6fd80 103 {
Near32 36:54f86bc6fd80 104 angle = angle - PI*floor(angle/PI);
Near32 36:54f86bc6fd80 105 }
Near32 36:54f86bc6fd80 106
Near32 36:54f86bc6fd80 107 state.set( atan21(sin(angle), cos(angle)), 3,1);
Near32 36:54f86bc6fd80 108 */
Near32 36:54f86bc6fd80 109 return state;
Near32 36:54f86bc6fd80 110 }
Near32 36:54f86bc6fd80 111
Near32 36:54f86bc6fd80 112 template<typename T>
Near32 36:54f86bc6fd80 113 Mat<T> jmotion_bicycle3( Mat<T> state, Mat<T> command, T dt)
Near32 43:87bdce65341f 114 {
Near32 43:87bdce65341f 115 T h = pow(numeric_limits<T>::epsilon(),(T)0.5)*norme2(state)+ pow(numeric_limits<T>::epsilon(), (T)0.5);
Near32 36:54f86bc6fd80 116 Mat<T> var( (T)0, state.getLine(), state.getColumn());
Near32 36:54f86bc6fd80 117 var.set( h, 1,1);
Near32 36:54f86bc6fd80 118 Mat<T> G(motion_bicycle3(state+var, command, dt) - motion_bicycle3(state-var, command,dt));
Near32 36:54f86bc6fd80 119
Near32 36:54f86bc6fd80 120 for(int i=2;i<=state.getLine();i++)
Near32 36:54f86bc6fd80 121 {
Near32 36:54f86bc6fd80 122 var.set( (T)0, i-1,1);
Near32 36:54f86bc6fd80 123 var.set( h, i,1);
Near32 36:54f86bc6fd80 124 G = operatorL(G, motion_bicycle3(state+var, command, dt) - motion_bicycle3(state-var, command,dt) );
Near32 36:54f86bc6fd80 125 }
Near32 36:54f86bc6fd80 126
Near32 36:54f86bc6fd80 127
Near32 36:54f86bc6fd80 128 return ((T)1.0/(2*h))*G;
Near32 36:54f86bc6fd80 129 }
Near32 36:54f86bc6fd80 130
Near32 36:54f86bc6fd80 131 template<typename T>
Near32 36:54f86bc6fd80 132 Mat<T> jmotion_bicycle3_command(Mat<T> state, Mat<T> command, T dt)
Near32 36:54f86bc6fd80 133 {
Near32 43:87bdce65341f 134 T h = pow(numeric_limits<T>::epsilon(), (T)0.5)+sqrt(numeric_limits<T>::epsilon())*norme2(state);
Near32 36:54f86bc6fd80 135 Mat<T> var( (T)0, command.getLine(), command.getColumn());
Near32 36:54f86bc6fd80 136 var.set( h, 1,1);
Near32 36:54f86bc6fd80 137 Mat<T> G(motion_bicycle3(state, command+var, dt) - motion_bicycle3(state, command-var,dt));
Near32 36:54f86bc6fd80 138
Near32 36:54f86bc6fd80 139 for(int i=2;i<=command.getLine();i++)
Near32 36:54f86bc6fd80 140 {
Near32 36:54f86bc6fd80 141 var.set( (T)0, i-1,1);
Near32 36:54f86bc6fd80 142 var.set( h, i,1);
Near32 36:54f86bc6fd80 143 G = operatorL(G, motion_bicycle3(state, command+var, dt) - motion_bicycle3(state, command-var,dt) );
Near32 36:54f86bc6fd80 144 }
Near32 36:54f86bc6fd80 145
Near32 36:54f86bc6fd80 146
Near32 36:54f86bc6fd80 147 return (1.0/(2*h))*G;
Near32 36:54f86bc6fd80 148 }
Near32 36:54f86bc6fd80 149
Near32 36:54f86bc6fd80 150
Near32 36:54f86bc6fd80 151 template<typename T>
Near32 36:54f86bc6fd80 152 Mat<T> jsensor_bicycle3( Mat<T> state, Mat<T> command, Mat<T> d_state, T dt)
Near32 36:54f86bc6fd80 153 {
Near32 36:54f86bc6fd80 154 T h = pow(numeric_limits<T>::epsilon(), (T)0.5)+sqrt(numeric_limits<T>::epsilon())*norme2(state);
Near32 36:54f86bc6fd80 155 Mat<T> var((T)0, state.getLine(), state.getColumn());
Near32 36:54f86bc6fd80 156 var.set( h, 1,1);
Near32 36:54f86bc6fd80 157 Mat<T> H(sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt));
Near32 36:54f86bc6fd80 158
Near32 36:54f86bc6fd80 159 for(int i=2;i<=state.getLine();i++)
Near32 36:54f86bc6fd80 160 {
Near32 36:54f86bc6fd80 161 var.set( (T)0, i-1,1);
Near32 36:54f86bc6fd80 162 var.set( h, i,1);
Near32 36:54f86bc6fd80 163 H = operatorL(H, sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt) );
Near32 36:54f86bc6fd80 164 }
Near32 36:54f86bc6fd80 165
Near32 36:54f86bc6fd80 166
Near32 36:54f86bc6fd80 167 return ((T)1.0/(2*h))*H;
Near32 36:54f86bc6fd80 168 /*
Near32 36:54f86bc6fd80 169 double h = sqrt(numeric_limits<double>::epsilon())*10e2+sqrt(numeric_limits<double>::epsilon())*norme2(state);
Near32 36:54f86bc6fd80 170 Mat<double> var((double)0, state.getLine(), state.getColumn());
Near32 36:54f86bc6fd80 171 var.set( h, 1,1);
Near32 36:54f86bc6fd80 172 Mat<double> H(sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt));
Near32 36:54f86bc6fd80 173
Near32 36:54f86bc6fd80 174 for(int i=2;i<=state.getLine();i++)
Near32 36:54f86bc6fd80 175 {
Near32 36:54f86bc6fd80 176 var.set( (double)0, i-1,1);
Near32 36:54f86bc6fd80 177 var.set( h, i,1);
Near32 36:54f86bc6fd80 178 H = operatorL(H, sensor_bicycle3(state+var, command, d_state, dt) - sensor_bicycle3(state-var, command, d_state, dt) );
Near32 36:54f86bc6fd80 179
Near32 36:54f86bc6fd80 180 }
Near32 36:54f86bc6fd80 181
Near32 36:54f86bc6fd80 182
Near32 36:54f86bc6fd80 183 return (1.0/(2*h))*H;
Near32 36:54f86bc6fd80 184 */
Near32 43:87bdce65341f 185 }
Near32 43:87bdce65341f 186 //int reduc = 16;
Near32 43:87bdce65341f 187
Near32 43:87bdce65341f 188 extern Serial logger;
Near32 43:87bdce65341f 189
Near32 43:87bdce65341f 190 /*---------------------------------------------------------------------------------------------------------*/
Near32 43:87bdce65341f 191 /*---------------------------------------------------------------------------------------------------------*/
Near32 43:87bdce65341f 192
Near32 43:87bdce65341f 193 template<typename T>
Near32 43:87bdce65341f 194 class Asserv
Near32 43:87bdce65341f 195 {
Near32 43:87bdce65341f 196 private :
Near32 43:87bdce65341f 197
Near32 43:87bdce65341f 198 Odometry* odometry;
Near32 43:87bdce65341f 199 EKF<T>* instanceEKF;
Near32 43:87bdce65341f 200 int nbrstate;
Near32 43:87bdce65341f 201 int nbrcontrol;
Near32 43:87bdce65341f 202 int nbrobs;
Near32 43:87bdce65341f 203 T dt;
Near32 43:87bdce65341f 204
Near32 43:87bdce65341f 205 T stdnoise;
Near32 43:87bdce65341f 206 Mat<T> X;
Near32 43:87bdce65341f 207 Mat<T> dX;
Near32 50:16c033eea17d 208
Near32 50:16c033eea17d 209 Mat<T> dXalpha;
Near32 50:16c033eea17d 210 Mat<T> dXrho;
Near32 50:16c033eea17d 211 Mat<T> dXbeta;
Near32 50:16c033eea17d 212
Near32 43:87bdce65341f 213 Mat<T> initX;
Near32 43:87bdce65341f 214 Mat<T> z;
Near32 43:87bdce65341f 215 Mat<T> u;
Near32 43:87bdce65341f 216 bool extended;
Near32 43:87bdce65341f 217 bool filterOn;
Near32 43:87bdce65341f 218
Near32 43:87bdce65341f 219 Mat<T> ki;
Near32 43:87bdce65341f 220 Mat<T> kp;
Near32 43:87bdce65341f 221 Mat<T> kd;
Near32 43:87bdce65341f 222
Near32 43:87bdce65341f 223 public :
Near32 43:87bdce65341f 224
Near32 43:87bdce65341f 225 T phi_r;
Near32 43:87bdce65341f 226 T phi_l;
Near32 43:87bdce65341f 227 T phi_max;
Near32 43:87bdce65341f 228 bool execution;
Near32 50:16c033eea17d 229 bool isarrived;
Near32 50:16c033eea17d 230 bool isarrivedalpha;
Near32 50:16c033eea17d 231 bool isarrivedrho;
Near32 89:d05001d85a02 232 int behaviour;
Near32 43:87bdce65341f 233
Near32 43:87bdce65341f 234 Asserv(Odometry* odometry)
Near32 43:87bdce65341f 235 {
Near32 43:87bdce65341f 236 /*Odometry*/
Near32 43:87bdce65341f 237 this->odometry = odometry;
Near32 44:d5f95af61243 238 phi_max = (T)100.0;
Near32 43:87bdce65341f 239
Near32 43:87bdce65341f 240 /*KalmanFilter*/
Near32 43:87bdce65341f 241 //double phi_max = 100;
Near32 43:87bdce65341f 242 /*en millimetres*/
Near32 43:87bdce65341f 243
Near32 43:87bdce65341f 244 nbrstate = 5;
Near32 43:87bdce65341f 245 nbrcontrol = 2;
Near32 43:87bdce65341f 246 nbrobs = 5;
Near32 43:87bdce65341f 247 dt = (T)0.05;
Near32 43:87bdce65341f 248 stdnoise = (T)0.05;
Near32 43:87bdce65341f 249
Near32 43:87bdce65341f 250 initX = Mat<T>((T)0, nbrstate, 1);
Near32 43:87bdce65341f 251 initX.set( (T)0, 3,1);
Near32 43:87bdce65341f 252 X = initX;
Near32 43:87bdce65341f 253
Near32 43:87bdce65341f 254 extended = true;
Near32 43:87bdce65341f 255 filterOn = true;
Near32 43:87bdce65341f 256
Near32 43:87bdce65341f 257 instanceEKF = new EKF<T>(nbrstate, nbrcontrol, nbrobs, dt, stdnoise, /*current state*/ initX, extended, filterOn);
Near32 43:87bdce65341f 258
Near32 43:87bdce65341f 259 instanceEKF->initMotion(motion_bicycle3);
Near32 43:87bdce65341f 260 instanceEKF->initSensor(sensor_bicycle3);
Near32 43:87bdce65341f 261 instanceEKF->initJMotion(jmotion_bicycle3);
Near32 43:87bdce65341f 262 //instanceEKF.initJMotionCommand(jmotion_bicycle3_command);
Near32 43:87bdce65341f 263 instanceEKF->initJSensor(jsensor_bicycle3);
Near32 43:87bdce65341f 264
Near32 43:87bdce65341f 265 /*desired State : (x y theta phiright phileft)*/
Near32 43:87bdce65341f 266 dX = Mat<T>((T)0, nbrstate, 1);
Near32 43:87bdce65341f 267 dX.set( (T)0, 1,1);
Near32 43:87bdce65341f 268 dX.set( (T)0, 2,1);
Near32 43:87bdce65341f 269 dX.set( (T)0, 3,1);
Near32 43:87bdce65341f 270 dX.set( (T)0, 4,1);
Near32 50:16c033eea17d 271 dX.set( (T)0, 5,1);
Near32 50:16c033eea17d 272
Near32 50:16c033eea17d 273 dXalpha = dX;
Near32 50:16c033eea17d 274 dXrho = dX;
Near32 43:87bdce65341f 275
Near32 43:87bdce65341f 276 ki = Mat<T>((T)0, nbrcontrol, nbrstate);
Near32 43:87bdce65341f 277 kp = Mat<T>((T)0, nbrcontrol, nbrstate);
Near32 43:87bdce65341f 278 kd = Mat<T>((T)0, nbrcontrol, nbrstate);
Near32 43:87bdce65341f 279 //Mat<double> kdd((double)0.0015, nbrcontrol, nbrstate);
Near32 43:87bdce65341f 280
Near32 43:87bdce65341f 281 for(int i=1;i<=nbrstate;i++)
Near32 43:87bdce65341f 282 {
Near32 43:87bdce65341f 283 kp.set( (T)0.01, i, i);
Near32 43:87bdce65341f 284 kd.set( (T)0.0001, i, i);
Near32 43:87bdce65341f 285 ki.set( (T)0.0001, i, i);
Near32 43:87bdce65341f 286 }
Near32 43:87bdce65341f 287
Near32 43:87bdce65341f 288 instanceEKF->setKi(ki);
Near32 43:87bdce65341f 289 instanceEKF->setKp(kp);
Near32 43:87bdce65341f 290 instanceEKF->setKd(kd);
Near32 43:87bdce65341f 291 //instance.setKdd(kdd);
Near32 43:87bdce65341f 292
Near32 43:87bdce65341f 293 u = Mat<T>(transpose( instanceEKF->getCommand()) );
Near32 43:87bdce65341f 294
Near32 43:87bdce65341f 295 /*Observations*/
Near32 43:87bdce65341f 296 /*il nous faut 5 observation : mais on n'en met à jour que 3...*/
Near32 43:87bdce65341f 297 z = Mat<T>((T)0,5,1);
Near32 43:87bdce65341f 298 this->measurementCallback(&z);
Near32 43:87bdce65341f 299
Near32 43:87bdce65341f 300 /*----------------------------------------------------------------------------------------------*/
Near32 43:87bdce65341f 301 isarrived = true;
Near32 50:16c033eea17d 302 isarrivedalpha = true;
Near32 50:16c033eea17d 303 isarrivedrho = true;
Near32 43:87bdce65341f 304 execution = false;
Near32 89:d05001d85a02 305 behaviour = 0; //alpha : 1 : rho : 2 : dX (beta)
Near32 43:87bdce65341f 306
Near32 43:87bdce65341f 307 }
Near32 43:87bdce65341f 308
Near32 43:87bdce65341f 309 ~Asserv()
Near32 43:87bdce65341f 310 {
Near32 43:87bdce65341f 311 delete instanceEKF;
Near32 43:87bdce65341f 312 }
Near32 43:87bdce65341f 313
Near32 43:87bdce65341f 314 void setGoal(T x, T y, T theta)
Near32 43:87bdce65341f 315 {
Near32 43:87bdce65341f 316 dX.set(x,1,1);
Near32 43:87bdce65341f 317 dX.set(y,2,1);
Near32 43:87bdce65341f 318 dX.set(theta,3,1);
Near32 43:87bdce65341f 319 dX.set((T)0,4,1);
Near32 43:87bdce65341f 320 dX.set((T)0,5,1);
Near32 43:87bdce65341f 321 execution = true;
Near32 43:87bdce65341f 322 isarrived = false;
Near32 50:16c033eea17d 323 isarrivedalpha = false;
Near32 50:16c033eea17d 324 isarrivedrho = false;
Near32 50:16c033eea17d 325
Near32 90:294b16267109 326 T alpha = atan2((dX.get(2,1)-X.get(2,1)),( dX.get(1,1) - X.get(1,1)));
Near32 90:294b16267109 327 alpha = alpha - atan2(sin(X.get(3,1)),cos(X.get(3,1)));
Near32 90:294b16267109 328
Near32 90:294b16267109 329 while(alpha > (T)PI)
Near32 90:294b16267109 330 {
Near32 90:294b16267109 331 alpha -= 2.0f*(T)PI;
Near32 90:294b16267109 332 }
Near32 90:294b16267109 333 while(alpha <= -(T)PI)
Near32 90:294b16267109 334 {
Near32 90:294b16267109 335 alpha += 2.0f*(T)PI;
Near32 90:294b16267109 336 }
Near32 90:294b16267109 337
Near32 91:c0b436c52ede 338 dXalpha.set( X.get(1,1), 1,1);
Near32 91:c0b436c52ede 339 dXalpha.set( X.get(2,1), 2,1);
Near32 51:960ed3e8b648 340 dXalpha.set( alpha, 3,1);
Near32 91:c0b436c52ede 341 dXalpha.set((T)0, 4,1);
Near32 91:c0b436c52ede 342 dXalpha.set((T)0, 5,1);
Near32 50:16c033eea17d 343
Near32 50:16c033eea17d 344 dXrho = dX;
Near32 51:960ed3e8b648 345 dXrho.set( alpha, 3,1);
Near32 43:87bdce65341f 346
Near32 43:87bdce65341f 347 }
Near32 43:87bdce65341f 348
Near32 75:23222b28dc90 349 bool isArrived() { return isarrived;}
Near32 75:23222b28dc90 350 bool isArrivedRho() { return isarrivedrho;}
Near32 43:87bdce65341f 351
Near32 43:87bdce65341f 352 void stop()
Near32 43:87bdce65341f 353 {
Near32 43:87bdce65341f 354 execution = false;
Near32 43:87bdce65341f 355 }
Near32 43:87bdce65341f 356
Near32 43:87bdce65341f 357 void update(T deltat)
Near32 43:87bdce65341f 358 {
Near32 43:87bdce65341f 359 if(execution)
Near32 43:87bdce65341f 360 {
Near32 47:4909e97570f6 361 dt = deltat;
Near32 47:4909e97570f6 362
Near32 43:87bdce65341f 363 /*Asservissement*/
Near32 47:4909e97570f6 364 this->measurementCallback(&z);
Near32 73:d8e1b543fbe3 365 //transpose(z).afficherMblue();
Near32 50:16c033eea17d 366
Near32 50:16c033eea17d 367
Near32 50:16c033eea17d 368 /*Gestion des comportements : alpha --> rho --> beta --> isarrived = true;*/
Near32 50:16c033eea17d 369 /*------------------------------------------------------------------------*/
Near32 89:d05001d85a02 370
Near32 89:d05001d85a02 371 Mat<T> dXbehaviour(dX);
Near32 90:294b16267109 372 if(isarrivedalpha && phi_l+phi_r <= phi_max/20 && behaviour == 0)
Near32 50:16c033eea17d 373 {
Near32 50:16c033eea17d 374 behaviour=1;
Near32 50:16c033eea17d 375 }
Near32 90:294b16267109 376 if(isarrivedalpha && isarrivedrho && phi_l+phi_r <= phi_max/20 && behaviour == 1)
Near32 50:16c033eea17d 377 {
Near32 50:16c033eea17d 378 behaviour = 2;
Near32 50:16c033eea17d 379 }
Near32 50:16c033eea17d 380
Near32 50:16c033eea17d 381 switch(behaviour)
Near32 50:16c033eea17d 382 {
Near32 50:16c033eea17d 383 case 0:
Near32 90:294b16267109 384 if(fabs_(dXalpha.get(3,1)-X.get(3,1) ) <= (T)0.02)
Near32 50:16c033eea17d 385 {
Near32 50:16c033eea17d 386 behaviour = 1;
Near32 50:16c033eea17d 387 dXbehaviour = dXrho;
Near32 51:960ed3e8b648 388 isarrivedalpha = true;
Near32 50:16c033eea17d 389 }
Near32 50:16c033eea17d 390 else
Near32 50:16c033eea17d 391 {
Near32 50:16c033eea17d 392 dXbehaviour = dXalpha;
Near32 50:16c033eea17d 393 isarrivedalpha = false;
Near32 50:16c033eea17d 394 }
Near32 50:16c033eea17d 395
Near32 50:16c033eea17d 396 break;
Near32 50:16c033eea17d 397
Near32 50:16c033eea17d 398 case 1:
Near32 91:c0b436c52ede 399 if(norme2( extract(dXrho-X,1,1, 2,1) ) <= (T)10)
Near32 50:16c033eea17d 400 {
Near32 50:16c033eea17d 401 behaviour = 2;
Near32 50:16c033eea17d 402 dXbehaviour = dX;
Near32 50:16c033eea17d 403 isarrivedrho = true;
Near32 50:16c033eea17d 404 }
Near32 50:16c033eea17d 405 else
Near32 50:16c033eea17d 406 {
Near32 50:16c033eea17d 407 dXbehaviour = dXrho;
Near32 50:16c033eea17d 408 isarrivedrho = false;
Near32 50:16c033eea17d 409 }
Near32 50:16c033eea17d 410 break;
Near32 50:16c033eea17d 411
Near32 50:16c033eea17d 412 default:
Near32 90:294b16267109 413 if(norme2(dX-X) <=5)
Near32 50:16c033eea17d 414 {
Near32 50:16c033eea17d 415 isarrived = true;
Near32 50:16c033eea17d 416 }
Near32 50:16c033eea17d 417 else
Near32 50:16c033eea17d 418 {
Near32 51:960ed3e8b648 419 isarrived = false;
Near32 50:16c033eea17d 420 }
Near32 51:960ed3e8b648 421 dXbehaviour = dX;
Near32 50:16c033eea17d 422 break;
Near32 50:16c033eea17d 423 }
Near32 89:d05001d85a02 424
Near32 91:c0b436c52ede 425 #ifdef verboseGOAL
Near32 91:c0b436c52ede 426 logger.printf("GOAL : ");
Near32 91:c0b436c52ede 427 transpose(dXbehaviour).afficherMblue();
Near32 91:c0b436c52ede 428 logger.printf("\r\nPOSE : ");
Near32 91:c0b436c52ede 429 transpose(X).afficherMblue();
Near32 91:c0b436c52ede 430 #endif
Near32 50:16c033eea17d 431 /*------------------------------------------------*/
Near32 89:d05001d85a02 432 //if( norme2( extract( dX-X,1,1,2,1)) <= 10 )
Near32 89:d05001d85a02 433 // isarrivedrho = true;
Near32 75:23222b28dc90 434
Near32 81:08eacb6179d8 435 #ifdef debugAsserv
Near32 87:e8b64b4174b8 436 logger.printf("BEHAVIOUR ASSERV : %d\r\n", behaviour);
Near32 81:08eacb6179d8 437 #endif
Near32 89:d05001d85a02 438 instanceEKF->measurement_Callback( z, dXbehaviour, true );
Near32 89:d05001d85a02 439
Near32 89:d05001d85a02 440 #ifdef debugAsserv
Near32 89:d05001d85a02 441 logger.printf("MEASUREMENT CALLBACK : DONE.\r\n");
Near32 89:d05001d85a02 442 #endif
Near32 90:294b16267109 443 //instanceEKF->state_Callback();
Near32 90:294b16267109 444 instanceEKF->setX(z);
Near32 89:d05001d85a02 445 X = instanceEKF->getX();
Near32 89:d05001d85a02 446
Near32 89:d05001d85a02 447 #ifdef debugAsserv
Near32 89:d05001d85a02 448 logger.printf("STATE CALLBACK : DONE. \r\n");
Near32 89:d05001d85a02 449 transpose(X).afficherMblue();
Near32 89:d05001d85a02 450 transpose(z).afficherMblue();
Near32 89:d05001d85a02 451 #endif
Near32 43:87bdce65341f 452
Near32 50:16c033eea17d 453 instanceEKF->computeCommand(dXbehaviour, (T)dt, -1);
Near32 89:d05001d85a02 454
Near32 89:d05001d85a02 455 #ifdef debugAsserv
Near32 89:d05001d85a02 456 logger.printf("COMMAND CALLBACK : DONE. \r\n");
Near32 89:d05001d85a02 457 #endif
Near32 47:4909e97570f6 458 //instanceEKF->computeCommand(dX, (T)dt, -2);
Near32 44:d5f95af61243 459 phi_l = instanceEKF->getCommand().get(1,1);
Near32 44:d5f95af61243 460 phi_r = instanceEKF->getCommand().get(2,1);
Near32 43:87bdce65341f 461 }
Near32 43:87bdce65341f 462 else
Near32 43:87bdce65341f 463 {
Near32 43:87bdce65341f 464 phi_r = (T)0;
Near32 43:87bdce65341f 465 phi_l = (T)0;
Near32 43:87bdce65341f 466 }
Near32 43:87bdce65341f 467 }
Near32 43:87bdce65341f 468
Near32 47:4909e97570f6 469 void measurementCallbackVW( Mat<T>* z)
Near32 43:87bdce65341f 470 {
Near32 43:87bdce65341f 471 z->set( (T)/*conversionUnitée mm */this->odometry->getX(), 1,1);
Near32 43:87bdce65341f 472 z->set( (T)/*conversionUnitée mm*/this->odometry->getY(), 2,1);
Near32 89:d05001d85a02 473
Near32 43:87bdce65341f 474 T theta = (T)this->odometry->getTheta();
Near32 43:87bdce65341f 475 theta = atan21(sin(theta),cos(theta));
Near32 89:d05001d85a02 476
Near32 43:87bdce65341f 477 z->set( (double)/*conversionUnitée rad*/theta, 3,1);//odometry->getTheta(), 3,1);
Near32 89:d05001d85a02 478
Near32 43:87bdce65341f 479 T vx = (T)this->odometry->getVx();
Near32 43:87bdce65341f 480 T vy = (T)this->odometry->getVy();
Near32 43:87bdce65341f 481 z->set( sqrt(vx*vx+vy*vy),4,1);
Near32 43:87bdce65341f 482 z->set( (T)odometry->getW(),5,1);
Near32 43:87bdce65341f 483
Near32 44:d5f95af61243 484 //transpose(*z).afficherMblue();
Near32 47:4909e97570f6 485 }
Near32 47:4909e97570f6 486
Near32 47:4909e97570f6 487 void measurementCallback( Mat<T>* z)
Near32 47:4909e97570f6 488 {
Near32 47:4909e97570f6 489 z->set( (T)/*conversionUnitée mm */this->odometry->getX(), 1,1);
Near32 47:4909e97570f6 490 z->set( (T)/*conversionUnitée mm*/this->odometry->getY(), 2,1);
Near32 47:4909e97570f6 491 T theta = (T)this->odometry->getTheta();
Jagang 78:7c7cefbe1772 492 //theta = atan2(sin(theta),cos(theta));
Near32 47:4909e97570f6 493 z->set( (double)/*conversionUnitée rad*/theta, 3,1);//odometry->getTheta(), 3,1);
Near32 47:4909e97570f6 494
Near32 47:4909e97570f6 495 //State : phi_l phi_r
Near32 47:4909e97570f6 496 z->set( (T)this->odometry->getPhileft(),4,1);
Near32 47:4909e97570f6 497 z->set( (T)this->odometry->getPhiright(),5,1);
Near32 47:4909e97570f6 498
Near32 47:4909e97570f6 499 //transpose(*z).afficherMblue();
Near32 43:87bdce65341f 500 }
Near32 43:87bdce65341f 501
Near32 43:87bdce65341f 502 Mat<T> getX()
Near32 43:87bdce65341f 503 {
Near32 43:87bdce65341f 504 return X;
Near32 43:87bdce65341f 505 }
Near32 43:87bdce65341f 506
Near32 43:87bdce65341f 507 T getPhiR()
Near32 43:87bdce65341f 508 {
Near32 43:87bdce65341f 509 return phi_r;
Near32 43:87bdce65341f 510 }
Near32 43:87bdce65341f 511
Near32 43:87bdce65341f 512 T getPhiL()
Near32 43:87bdce65341f 513 {
Near32 43:87bdce65341f 514 return phi_l;
Near32 43:87bdce65341f 515 }
Near32 43:87bdce65341f 516
Near32 43:87bdce65341f 517 T getPhiMax()
Near32 43:87bdce65341f 518 {
Near32 43:87bdce65341f 519 return phi_max;
Near32 43:87bdce65341f 520 }
Near32 43:87bdce65341f 521
Near32 43:87bdce65341f 522
Near32 43:87bdce65341f 523 };