ARES / Mbed 2 deprecated Robot 2015

Robot's source code

Dependencies:   mbed

Asservissement/Asserv.h@100:a827a645d6c2, 2015-04-30 (annotated)

Committer:
Jagang
Date:
Thu Apr 30 16:16:29 2015 +0000
Revision:
100:a827a645d6c2
Parent:
94:5c37bcf73d14
Parent:
91:c0b436c52ede 
Merge 2;

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