ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asservissement/Asserv.h@87:e8b64b4174b8, 2015-04-18 (annotated)

Committer:
Near32
Date:
Sat Apr 18 08:29:42 2015 +0000
Revision:
87:e8b64b4174b8
Parent:
81:08eacb6179d8
Child:
89:d05001d85a02
commit; test ?

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