Diff: Asserv_Plan_B/planB.cpp

Revision:

123:55e5e9acc541

Parent:

121:0cc17ba879cb

--- a/Asserv_Plan_B/planB.cpp	Thu May 07 14:18:07 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Mon May 11 20:32:11 2015 +0000
@@ -27,7 +27,7 @@
     Kd_angle = 0.2;
     
     Kp_distance = 0.0041;
-    Ki_distance = 0.00003;//0.000001
+    Ki_distance = 0.0001;//0.000001
     Kd_distance = 0.01;//0.05
     
     N = 0;
@@ -52,6 +52,7 @@
 {
     m_motorL.setSpeed(0);
     m_motorR.setSpeed(0);
+    state = 0;
 }
 
 void aserv_planB::setGoal(float x, float y)
@@ -77,12 +78,6 @@
     
     if(erreur_theta <= PI) erreur_theta += 2.0f*PI;
     if(erreur_theta >= PI) erreur_theta -= 2.0f*PI;
-    
-    if(state == 0)
-    {
-        m_motorL.setSpeed(0);
-        m_motorR.setSpeed(0);
-    }
 
     // Etat 1 : Angle theta pour viser dans la direction du point M(x,y)
     if(state == 1)
@@ -103,7 +98,7 @@
             m_motorL.setSpeed(0);
             m_motorR.setSpeed(0);
             state = 2;
-            logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
+            //logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
             somme_erreur_theta = 0;
         }
     }
@@ -159,6 +154,8 @@
             erreur_precedente_theta = 0;
             somme_erreur_theta = 0;
             erreur_theta = 0;
+            m_motorL.setSpeed(0);
+            m_motorR.setSpeed(0);
             if(squip == true) 
             {
                 arrived = true;
@@ -166,7 +163,7 @@
                 state = 0;
             }
             else state = 3;
-            logger.printf("Erreur distance : %.2f, Arrived : %d, Etat = %d\r\n", erreur_distance, arrived, (int)state);
+            //logger.printf("Erreur distance : %.2f, Arrived : %d, Etat = %d\r\n", erreur_distance, arrived, (int)state);
         }
     }
 
@@ -190,11 +187,13 @@
         else N = 0;
         if(N > 10)
         {
-            logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
+            //logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
             somme_erreur_theta = 0;
             arrived = true;
             squip = false;
             state = 0;
+            m_motorL.setSpeed(0);
+            m_motorR.setSpeed(0);
         }
     }
 }