Diff: Asservissement/Asserv.h

Revision:

81:08eacb6179d8

Parent:

78:7c7cefbe1772

Child:

87:e8b64b4174b8

Child:

94:5c37bcf73d14

--- 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);