Robot's source code
Dependencies: mbed
Diff: Asservissement/Asserv.h
- Revision:
- 81:08eacb6179d8
- Parent:
- 78:7c7cefbe1772
- Child:
- 87:e8b64b4174b8
- Child:
- 94:5c37bcf73d14
diff -r 7c7cefbe1772 -r 08eacb6179d8 Asservissement/Asserv.h --- a/Asservissement/Asserv.h Tue Apr 14 15:49:22 2015 +0000 +++ b/Asservissement/Asserv.h Tue Apr 14 18:31:41 2015 +0000 @@ -1,6 +1,7 @@ /*KalmanFilter*/ #include "EKF.h" #include "Odometry.h" +#define debugAsserv /* template<typename T> Mat<T> motion_bicycle3( Mat<T> state, Mat<T> command, T dt = (T)0.5); @@ -59,7 +60,7 @@ angle = angle - PI*floor(angle/PI); } - r.set( atan21(sin(angle), cos(angle)), 3,1); + r.set( angle, 3,1); /*----------------------------------------*/ @@ -320,7 +321,7 @@ T alpha = atan21((dX.get(2,1)-X.get(2,1)),( dX.get(1,1) - X.get(1,1))); alpha = alpha - atan21(sin(X.get(3,1)),cos(X.get(3,1))); - alpha = atan21( sin(alpha), cos(alpha)); + //alpha = atan21( sin(alpha), cos(alpha)); dXalpha.set( alpha, 3,1); dXrho = dX; @@ -352,11 +353,11 @@ int behaviour = 0; //alpha : 1 : rho : 2 : dX (beta) Mat<T> dXbehaviour(dX); - if(isarrivedalpha) + if(isarrivedalpha && && phi_l+phi_r <= phi_max/10) { behaviour=1; } - if(isarrivedalpha && isarrivedrho) + if(isarrivedalpha && isarrivedrho && && phi_l+phi_r <= phi_max/10) { behaviour = 2; } @@ -364,7 +365,7 @@ switch(behaviour) { case 0: - if(fabs_(dXalpha.get(3,1)-X.get(3,1) ) <= (T)0.01) + if(fabs_(dXalpha.get(3,1)-X.get(3,1) ) <= (T)0.02) { behaviour = 1; dXbehaviour = dXrho; @@ -408,7 +409,9 @@ if( norme2( extract( dX-X,1,1,2,1)) <= 10 ) isarrivedrho = true; - +#ifdef debugAsserv + logger.printf("BEHAVIOUR ASSERV : %d\r\n", behabiour); +#endif instanceEKF->measurement_Callback( z, dXbehaviour, true ); //instanceEKF->state_Callback(); instanceEKF->setX(z);