Diff: Asservissement/Asserv.h

Revision:

90:294b16267109

Parent:

89:d05001d85a02

Child:

91:c0b436c52ede

diff -r d05001d85a02 -r 294b16267109 Asservissement/Asserv.h
--- a/Asservissement/Asserv.h	Thu Apr 23 16:29:31 2015 +0000
+++ b/Asservissement/Asserv.h	Thu Apr 23 18:20:35 2015 +0000
@@ -322,9 +322,18 @@
         isarrivedalpha = false;
         isarrivedrho = false;
         
-        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));            
+        T alpha = atan2((dX.get(2,1)-X.get(2,1)),( dX.get(1,1) - X.get(1,1)));                        
+        alpha = alpha - atan2(sin(X.get(3,1)),cos(X.get(3,1)));            
+        
+        while(alpha > (T)PI)
+        {
+             alpha -= 2.0f*(T)PI;                 
+        }
+        while(alpha <= -(T)PI)
+        {
+            alpha += 2.0f*(T)PI;                
+        }
+        
         dXalpha.set( alpha, 3,1);
         
         dXrho = dX;
@@ -355,11 +364,11 @@
             /*------------------------------------------------------------------------*/
                         
             Mat<T> dXbehaviour(dX);            
-            if(isarrivedalpha && phi_l+phi_r <= phi_max/10 && behaviour == 0)
+            if(isarrivedalpha && phi_l+phi_r <= phi_max/20 && behaviour == 0)
             {
                 behaviour=1;
             }
-            if(isarrivedalpha && isarrivedrho && phi_l+phi_r <= phi_max/10 && behaviour == 1)
+            if(isarrivedalpha && isarrivedrho && phi_l+phi_r <= phi_max/20 && behaviour == 1)
             {
                 behaviour = 2;
             }
@@ -367,7 +376,7 @@
             switch(behaviour)
             {
                 case 0:
-                if(fabs_(dXalpha.get(3,1)-X.get(3,1) ) <= (T)0.05)
+                if(fabs_(dXalpha.get(3,1)-X.get(3,1) ) <= (T)0.02)
                 {
                     behaviour = 1;
                     dXbehaviour = dXrho;
@@ -396,7 +405,7 @@
                 break;
                 
                 default:
-                if(norme2(dX-X) <=10)
+                if(norme2(dX-X) <=5)
                 {
                     isarrived = true;
                 }
@@ -420,8 +429,8 @@
 #ifdef debugAsserv            
             logger.printf("MEASUREMENT CALLBACK : DONE.\r\n");          
 #endif            
-            instanceEKF->state_Callback();
-            //instanceEKF->setX(z);
+            //instanceEKF->state_Callback();
+            instanceEKF->setX(z);
             X = instanceEKF->getX();
             
 #ifdef debugAsserv