Diff: Asservissement/Asserv.h

Revision:

89:d05001d85a02

Parent:

87:e8b64b4174b8

Child:

90:294b16267109

--- a/Asservissement/Asserv.h	Sat Apr 18 08:40:24 2015 +0000
+++ b/Asservissement/Asserv.h	Thu Apr 23 16:29:31 2015 +0000
@@ -1,7 +1,8 @@
 /*KalmanFilter*/
 #include "EKF.h"
 #include "Odometry.h"
-#define debugAsserv
+//#define debugAsserv
+
 /*
 template<typename T>
 Mat<T> motion_bicycle3( Mat<T> state, Mat<T> command, T dt = (T)0.5);
@@ -51,13 +52,13 @@
     r.set( r.get(2,1) + v*sin(r.get(3,1))*dt, 2,1);
     
     T angle = (r.get(3,1) + dt*w);
-    if( angle < -PI)
+    if( angle < -(T)PI)
     {
-        angle = angle - PI*ceil(angle/PI);
+        angle = angle - (T)PI*ceil(angle/PI);
     }
-    else if( angle > PI)
+    else if( angle > (T)PI)
     {
-        angle = angle - PI*floor(angle/PI);
+        angle = angle - (T)PI*floor(angle/PI);
     }
     
     r.set( angle, 3,1);
@@ -227,6 +228,7 @@
     bool isarrived;
     bool isarrivedalpha;
     bool isarrivedrho;  
+    int behaviour;
     
     Asserv(Odometry* odometry)
     {        
@@ -299,6 +301,7 @@
         isarrivedalpha = true;
         isarrivedrho = true;
         execution = false;
+        behaviour = 0; //alpha : 1 : rho : 2 : dX (beta)
             
     }
     
@@ -350,14 +353,13 @@
             
             /*Gestion des comportements : alpha --> rho --> beta --> isarrived = true;*/
             /*------------------------------------------------------------------------*/
-            
-            int behaviour = 0; //alpha : 1 : rho : 2 : dX (beta)
-            Mat<T> dXbehaviour(dX);
-            if(isarrivedalpha && phi_l+phi_r <= phi_max/10)
+                        
+            Mat<T> dXbehaviour(dX);            
+            if(isarrivedalpha && phi_l+phi_r <= phi_max/10 && behaviour == 0)
             {
                 behaviour=1;
             }
-            if(isarrivedalpha && isarrivedrho && phi_l+phi_r <= phi_max/10)
+            if(isarrivedalpha && isarrivedrho && phi_l+phi_r <= phi_max/10 && behaviour == 1)
             {
                 behaviour = 2;
             }
@@ -365,7 +367,7 @@
             switch(behaviour)
             {
                 case 0:
-                if(fabs_(dXalpha.get(3,1)-X.get(3,1) ) <= (T)0.02)
+                if(fabs_(dXalpha.get(3,1)-X.get(3,1) ) <= (T)0.05)
                 {
                     behaviour = 1;
                     dXbehaviour = dXrho;
@@ -380,7 +382,7 @@
                 break;
                 
                 case 1:
-                if(norme2( extract(dXrho-X,1,1, 2,1) ) <= (T)10)
+                if(norme2( extract(dXrho-X,1,1, 2,1) ) <= (T)5)
                 {
                     behaviour = 2;
                     dXbehaviour = dX;
@@ -405,19 +407,34 @@
                 dXbehaviour = dX;
                 break;
             }
+            
             /*------------------------------------------------*/
-            if( norme2( extract( dX-X,1,1,2,1)) <= 10 )
-                isarrivedrho = true;
+            //if( norme2( extract( dX-X,1,1,2,1)) <= 10 )
+            //    isarrivedrho = true;
                 
 #ifdef debugAsserv
             logger.printf("BEHAVIOUR ASSERV : %d\r\n", behaviour);
 #endif 
-            instanceEKF->measurement_Callback( z, dXbehaviour, true );            
-            //instanceEKF->state_Callback();
-            instanceEKF->setX(z);
-            X = instanceEKF->getX();            
+            instanceEKF->measurement_Callback( z, dXbehaviour, true );  
+            
+#ifdef debugAsserv            
+            logger.printf("MEASUREMENT CALLBACK : DONE.\r\n");          
+#endif            
+            instanceEKF->state_Callback();
+            //instanceEKF->setX(z);
+            X = instanceEKF->getX();
+            
+#ifdef debugAsserv            
+            logger.printf("STATE CALLBACK : DONE. \r\n");
+            transpose(X).afficherMblue();
+            transpose(z).afficherMblue();
+#endif            
             
             instanceEKF->computeCommand(dXbehaviour, (T)dt, -1);                    
+            
+#ifdef debugAsserv            
+            logger.printf("COMMAND CALLBACK : DONE. \r\n");
+#endif            
             //instanceEKF->computeCommand(dX, (T)dt, -2);                    
             phi_l = instanceEKF->getCommand().get(1,1);
             phi_r = instanceEKF->getCommand().get(2,1);                
@@ -433,9 +450,12 @@
     {
         z->set( (T)/*conversionUnitée mm */this->odometry->getX(), 1,1);
         z->set( (T)/*conversionUnitée mm*/this->odometry->getY(), 2,1);
+        
         T theta = (T)this->odometry->getTheta();
         theta = atan21(sin(theta),cos(theta));
+        
         z->set( (double)/*conversionUnitée rad*/theta, 3,1);//odometry->getTheta(), 3,1);   
+        
         T vx = (T)this->odometry->getVx();
         T vy = (T)this->odometry->getVy();
         z->set( sqrt(vx*vx+vy*vy),4,1);