ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asservissement/Asserv.h@107:a6e498b5706c, 2015-05-05 (annotated)

Committer:
Near32
Date:
Tue May 05 12:08:20 2015 +0000
Revision:
107:a6e498b5706c
Parent:
97:70bb90e8fe90 
coefficient pid position : coefficient pid vit --> attention, il y a un gros probl?me sur le gain du pid en vitesse s'il est trop grand....

Who changed what in which revision?

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