Diff: Asservissement/Asserv.h

Revision:

78:7c7cefbe1772

Parent:

75:23222b28dc90

Child:

81:08eacb6179d8

--- a/Asservissement/Asserv.h	Mon Apr 13 17:45:53 2015 +0000
+++ b/Asservissement/Asserv.h	Tue Apr 14 15:49:22 2015 +0000
@@ -352,7 +352,6 @@
             
             int behaviour = 0; //alpha : 1 : rho : 2 : dX (beta)
             Mat<T> dXbehaviour(dX);
-            /*
             if(isarrivedalpha)
             {
                 behaviour=1;
@@ -405,15 +404,14 @@
                 dXbehaviour = dX;
                 break;
             }
-            */
             /*------------------------------------------------*/
             if( norme2( extract( dX-X,1,1,2,1)) <= 10 )
                 isarrivedrho = true;
                 
              
             instanceEKF->measurement_Callback( z, dXbehaviour, true );            
-            instanceEKF->state_Callback();
-            //instanceEKF->setX(z);
+            //instanceEKF->state_Callback();
+            instanceEKF->setX(z);
             X = instanceEKF->getX();            
             
             instanceEKF->computeCommand(dXbehaviour, (T)dt, -1);                    
@@ -448,7 +446,7 @@
         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));
+        //theta = atan2(sin(theta),cos(theta));
         z->set( (double)/*conversionUnitée rad*/theta, 3,1);//odometry->getTheta(), 3,1);   
         
         //State : phi_l phi_r