Diff: Asserv_Plan_B/planB.cpp

Revision:

112:df5388d9f706

Parent:

108:890094ee202a

Child:

116:73d7d87e0299

--- a/Asserv_Plan_B/planB.cpp	Tue May 05 16:35:42 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Tue May 05 16:58:50 2015 +0000
@@ -7,7 +7,8 @@
 {
     somme_erreur = 0;
     Kp_angle = 3.0; //Fixed à 3.0 pour 180 deg
-    Ki_angle = 0.00;
+    Ki_angle = 0.001;
+    limite = 0.75;
     cmd = 0;
     cmd_g = 0, cmd_d = 0;
     N = 0;
@@ -17,7 +18,7 @@
     distanceGoal = 0;
     distance = 0;
     Kp_distance = 0.004;
-    Ki_distance = 0.001;
+    Ki_distance = 0.0;
     Kd_distance = 0.0;
 }
 
@@ -66,8 +67,8 @@
         erreur_precedente = erreur_theta;
         somme_erreur += erreur_theta;
         
-        if(cmd > 0.8) cmd = 0.8;
-        else if(cmd < -0.8) cmd = -0.8;
+        if(cmd > limite) cmd = limite;
+        else if(cmd < -limite) cmd = -limite;
         
         m_motorL.setSpeed(-cmd);
         m_motorR.setSpeed(cmd);
@@ -82,41 +83,35 @@
             logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
             somme_erreur = 0;
             N = 0;
-            Kp_angle = 5.0;
+            Kp_angle = 3.0;
         }
     }
 
     // Etat 2 : Parcours du robot jusqu'au point M(x,y)
     if(state == 2) 
     {
-        //logger.printf("%.2f %.2f %.2f\r\n", erreur_distance, cmd_g, cmd_d);
-        cmd_g = erreur_distance*Kp_distance + somme_erreur*Ki_distance - erreur_theta*Kp_angle;
-        cmd_d = erreur_distance*Kp_distance + somme_erreur*Ki_distance + erreur_theta*Kp_angle;
+        //logger.printf("%.2f %.2f %.2f\r\n", erreur_distance, cmd_g, cmd_d); //+ somme_erreur*Ki_distance
+        cmd_g = erreur_distance*Kp_distance - erreur_theta*Kp_angle;
+        cmd_d = erreur_distance*Kp_distance + erreur_theta*Kp_angle;
+        //somme_erreur += erreur_distance;
         
-        if(cmd_g > 0.8) cmd_g = 0.8;
-        else if(cmd_g < -0.8) cmd_g = -0.8;
+        if(cmd_g > limite) cmd_g = limite;
+        else if(cmd_g < -limite) cmd_g = -limite;
         
-        if(cmd_d > 0.8) cmd_d = 0.8;
-        else if(cmd_d < -0.8) cmd_d = -0.8;
+        if(cmd_d > limite) cmd_d = limite;
+        else if(cmd_d < -limite) cmd_d = -limite;
                 
         m_motorL.setSpeed(cmd_g);
         m_motorR.setSpeed(cmd_d);
         
-        if(abs(erreur_distance) <= 10.0) 
-        {
-            arrived = true;
-            logger.printf("Erreur distance : %.2f, arrived = %d\r\n", erreur_distance, (int)arrived);
-            state = 3;
-            N = 0;
-        }
-        /*if(abs(erreur_distance) < 20.0) N++;
+        if(abs(erreur_distance) < 20.0) N++;
         else N = 0;
         if(N > 50)
         {
             logger.printf("Erreur distance : %.2f\r\n", erreur_distance);
             state = 3;
             N = 0;
-        }*/
+        }
     }
 
     // Etat 3 : Placement au bon angle Phi souhaité au point M(x,y)