Diff: Asservissement/Asserv.h

Revision:

91:c0b436c52ede

Parent:

90:294b16267109

Child:

95:33520ead3b94

Child:

100:a827a645d6c2

--- a/Asservissement/Asserv.h	Thu Apr 23 18:20:35 2015 +0000
+++ b/Asservissement/Asserv.h	Fri Apr 24 10:24:09 2015 +0000
@@ -2,6 +2,7 @@
 #include "EKF.h"
 #include "Odometry.h"
 //#define debugAsserv
+#define verboseGOAL
 
 /*
 template<typename T>
@@ -334,7 +335,11 @@
             alpha += 2.0f*(T)PI;                
         }
         
+        dXalpha.set( X.get(1,1), 1,1);
+        dXalpha.set( X.get(2,1), 2,1);
         dXalpha.set( alpha, 3,1);
+        dXalpha.set((T)0, 4,1);
+        dXalpha.set((T)0, 5,1);
         
         dXrho = dX;
         dXrho.set( alpha, 3,1);
@@ -391,7 +396,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;
@@ -417,6 +422,12 @@
                 break;
             }
             
+#ifdef verboseGOAL
+            logger.printf("GOAL : ");
+            transpose(dXbehaviour).afficherMblue();
+            logger.printf("\r\nPOSE : ");
+            transpose(X).afficherMblue();
+#endif            
             /*------------------------------------------------*/
             //if( norme2( extract( dX-X,1,1,2,1)) <= 10 )
             //    isarrivedrho = true;