Diff: Asservissement/Asserv.h

Revision:

51:960ed3e8b648

Parent:

50:16c033eea17d

Child:

73:d8e1b543fbe3

--- a/Asservissement/Asserv.h	Fri Apr 03 07:46:17 2015 +0000
+++ b/Asservissement/Asserv.h	Sat Apr 04 10:50:37 2015 +0000
@@ -318,11 +318,13 @@
         isarrivedalpha = false;
         isarrivedrho = false;
         
-        T dalpha = atan21( dX.get(2,1)-X.get(2,1), dX.get(1,1)-X.get(1,1) );
-        dXalpha.set( atan21(sin(dalpha),cos(dalpha)), 3,1);
+        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));            
+        dXalpha.set( alpha, 3,1);
         
         dXrho = dX;
-        dXrho.set( dalpha, 3,1);
+        dXrho.set( alpha, 3,1);
         
     }
     
@@ -348,7 +350,8 @@
             /*------------------------------------------------------------------------*/
             
             int behaviour = 0; //alpha : 1 : rho : 2 : dX (beta)
-            Mat<T> dXbehaviour(1,1);
+            Mat<T> dXbehaviour(dX);
+            /*
             if(isarrivedalpha)
             {
                 behaviour=1;
@@ -361,11 +364,11 @@
             switch(behaviour)
             {
                 case 0:
-                if(fabs_(dXbehaviour.get(3,1)-X.get(3,1) ) <= (T)0.02)
+                if(fabs_(dXalpha.get(3,1)-X.get(3,1) ) <= (T)0.01)
                 {
                     behaviour = 1;
                     dXbehaviour = dXrho;
-                    isarrivedalpha = true;
+                    isarrivedalpha = true;                    
                 }
                 else
                 {
@@ -376,7 +379,7 @@
                 break;
                 
                 case 1:
-                if(norme2( extract(dXrho-X,1,1, 2,1) ) <= (T)5)
+                if(norme2( extract(dXrho-X,1,1, 2,1) ) <= (T)10)
                 {
                     behaviour = 2;
                     dXbehaviour = dX;
@@ -396,12 +399,12 @@
                 }
                 else
                 {
-                    isarrived = false;
-                    dXbehaviour = dX;
+                    isarrived = false;                
                 }
+                dXbehaviour = dX;
                 break;
             }
-            
+            */
             /*------------------------------------------------*/
                       
             instanceEKF->measurement_Callback( z, dXbehaviour, true );