Diff: Asserv_Plan_B/planB.cpp

Revision:

119:c45efcd706d9

Parent:

116:73d7d87e0299

Child:

121:0cc17ba879cb

--- a/Asserv_Plan_B/planB.cpp	Wed May 06 11:23:08 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Wed May 06 15:17:16 2015 +0000
@@ -5,7 +5,7 @@
 
 aserv_planB::aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
 {
-    limite = 0.65;
+    limite = 0.5;
     cmd = 0;
     cmd_g = 0;
     cmd_d = 0;
@@ -24,8 +24,8 @@
     Ki_angle = 0.0;
     Kd_angle = 0.1;
     
-    Kp_distance = 0.0042;
-    Ki_distance = 0.00000;//0.000001
+    Kp_distance = 0.0040;
+    Ki_distance = 0.00003;//0.000001
     Kd_distance = 0.0;//0.05
     
     N = 0;
@@ -92,7 +92,7 @@
         m_motorL.setSpeed(-cmd);
         m_motorR.setSpeed(cmd);
         
-        if(abs(erreur_theta)< 0.05) N++;
+        if(abs(erreur_theta) < 0.05f) N++;
         else N = 0;
         if(N > 10)
         {
@@ -109,20 +109,45 @@
     // 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); //+ somme_erreur_distance*Ki_distance
+        if(delta_erreur_distance > 0) // On a dépassé le point
+        {
+            state = 1;
+            return;
+        }
+        
+        if(abs(erreur_distance) > 55.0f) somme_erreur_distance = 0;
+        
         cmd_g = erreur_distance*Kp_distance + somme_erreur_distance*Ki_distance + delta_erreur_distance*Kd_distance - (erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle);
         cmd_d = erreur_distance*Kp_distance + somme_erreur_distance*Ki_distance + delta_erreur_distance*Kd_distance + erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle;
         
-        if(cmd_g > limite) cmd_g = limite;
-        else if(cmd_g < -limite) cmd_g = -limite;
         
-        if(cmd_d > limite) cmd_d = limite;
-        else if(cmd_d < -limite) cmd_d = -limite;
-                
+        if(abs(erreur_distance) > 55.0f)
+        {
+            if(cmd_g > limite) cmd_g = limite;
+            else if(cmd_g < -limite) cmd_g = -limite;
+            
+            if(cmd_d > limite) cmd_d = limite;
+            else if(cmd_d < -limite) cmd_d = -limite;
+        }
+        else
+        {
+            if(cmd_g > 0.2f) cmd_g = 0.2f;
+            else if(cmd_g < -0.2f) cmd_g = -0.2f;
+            
+            if(cmd_d > 0.2f) cmd_d = 0.2f;
+            else if(cmd_d < -0.2f) cmd_d = -0.2f;
+        }
+        
+        if(cmd_g > 0.01f && cmd_g < 0.14f) cmd_g = 0.14f;
+        if(cmd_g < -0.01f && cmd_g > -0.14f) cmd_g = -0.14f;
+        
+        if(cmd_d > 0.01f && cmd_d < 0.14f) cmd_d = 0.14f;
+        if(cmd_d < -0.01f && cmd_d > -0.14f) cmd_d = -0.14f;
+            
         m_motorL.setSpeed(cmd_g);
         m_motorR.setSpeed(cmd_d);
         
-        if(abs(erreur_distance) < 20.0) N++;
+        if(abs(erreur_distance) < 10.0f) N++;
         else N = 0;
         if(N > 10)
         {
@@ -160,5 +185,6 @@
         m_motorL.setSpeed(0);
         m_motorR.setSpeed(0);
         arrived = true;
+        state = 0;
     }
 }