Diff: Asservissement/Asserv.h

Revision:

96:5a60caa2410a

Parent:

91:c0b436c52ede

Child:

97:70bb90e8fe90

--- a/Asservissement/Asserv.h	Thu Apr 30 15:55:09 2015 +0000
+++ b/Asservissement/Asserv.h	Thu Apr 30 17:03:26 2015 +0000
@@ -53,6 +53,17 @@
     r.set( r.get(2,1) + v*sin(r.get(3,1))*dt, 2,1);
     
     T angle = (r.get(3,1) + dt*w);
+    
+    while(angle > (T)PI)
+    {
+         angle -= 2.0f*(T)PI;                 
+    }
+    while(angle <= -(T)PI)
+    {
+        angle += 2.0f*(T)PI;                
+    }
+    
+    /*
     if( angle < -(T)PI)
     {
         angle = angle - (T)PI*ceil(angle/PI);
@@ -61,6 +72,7 @@
     {
         angle = angle - (T)PI*floor(angle/PI);
     }
+    */
     
     r.set( angle, 3,1);
     
@@ -263,12 +275,7 @@
         instanceEKF->initJSensor(jsensor_bicycle3);
         
         /*desired State : (x y theta phiright phileft)*/
-        dX = Mat<T>((T)0, nbrstate, 1);
-        dX.set( (T)0, 1,1);
-        dX.set( (T)0, 2,1);
-        dX.set( (T)0, 3,1);
-        dX.set( (T)0, 4,1);
-        dX.set( (T)0, 5,1);
+        dX = Mat<T>((T)0, nbrstate, 1);        
         
         dXalpha = dX;
         dXrho = dX;
@@ -313,6 +320,7 @@
     
     void setGoal(T x, T y, T theta)
     {
+        instanceEKF->resetPID();
         dX.set(x,1,1);
         dX.set(y,2,1);
         dX.set(theta,3,1);
@@ -322,9 +330,10 @@
         isarrived = false;
         isarrivedalpha = false;
         isarrivedrho = false;
+        behaviour = 0;
         
         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)));            
+        alpha = alpha - X.get(3,1);//atan2(sin(X.get(3,1)),cos(X.get(3,1)));            
         
         while(alpha > (T)PI)
         {
@@ -381,7 +390,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.1)
                 {
                     behaviour = 1;
                     dXbehaviour = dXrho;
@@ -450,8 +459,8 @@
             transpose(z).afficherMblue();
 #endif            
             
-            instanceEKF->computeCommand(dXbehaviour, (T)dt, -1);                    
-            
+            //instanceEKF->computeCommand(dXbehaviour, (T)dt, -1);                    
+            instanceEKF->computeCommand(dXbehaviour, (T)dt, 1);                    
 #ifdef debugAsserv            
             logger.printf("COMMAND CALLBACK : DONE. \r\n");
 #endif            
@@ -520,4 +529,4 @@
     }
     
     
-};    
+};    
\ No newline at end of file