Diff: Asserv_Plan_B/planB.cpp

Revision:

121:0cc17ba879cb

Parent:

119:c45efcd706d9

Child:

123:55e5e9acc541

--- a/Asserv_Plan_B/planB.cpp	Wed May 06 15:17:16 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Thu May 07 14:13:58 2015 +0000
@@ -5,7 +5,9 @@
 
 aserv_planB::aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
 {
-    limite = 0.5;
+    limite = 0.55;
+    lim_max = 0.30;
+    lim_min = 0.1995;
     cmd = 0;
     cmd_g = 0;
     cmd_d = 0;
@@ -22,11 +24,11 @@
     
     Kp_angle = 3.5; //Fixed à 3.0 pour 180 deg
     Ki_angle = 0.0;
-    Kd_angle = 0.1;
+    Kd_angle = 0.2;
     
-    Kp_distance = 0.0040;
+    Kp_distance = 0.0041;
     Ki_distance = 0.00003;//0.000001
-    Kd_distance = 0.0;//0.05
+    Kd_distance = 0.01;//0.05
     
     N = 0;
     arrived = false;
@@ -36,35 +38,31 @@
 
 void aserv_planB::setGoal(float x, float y, float phi)
 {
+    arrived = false;
     m_goalX = x;
     m_goalY = y;
     m_goalPhi = phi;
     distanceGoal = sqrt(carre(m_goalX-m_odometry.getX())+carre(m_goalY-m_odometry.getY()));
     state = 1; // Etat de rotation 1
+    Kd_distance = 0.01;
     N = 0;
-    arrived = false;
 }
 
 void aserv_planB::stop(void)
 {
     m_motorL.setSpeed(0);
     m_motorR.setSpeed(0);
-    /*m_goalX = m_odometry.getX();
-    m_goalY = m_odometry.getY();
-    setGoal(m_goalX, m_goalY);*/
 }
 
 void aserv_planB::setGoal(float x, float y)
 {
     squip = true;
     setGoal(x, y, 0);
-    squip = false;
 }
 
 void aserv_planB::update(float dt)
 {
-    if(state == 1) thetaGoal = atan2(m_goalY-m_odometry.getY(),m_goalX-m_odometry.getX());
-    else if(state == 3) thetaGoal = m_goalPhi;
+    thetaGoal = atan2(m_goalY-m_odometry.getY(),m_goalX-m_odometry.getX());
     float erreur_theta = thetaGoal-m_odometry.getTheta();
     
     float erreur_distance = sqrt(carre(m_goalX-m_odometry.getX())+carre(m_goalY-m_odometry.getY()));
@@ -79,6 +77,12 @@
     
     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)
@@ -94,26 +98,25 @@
         
         if(abs(erreur_theta) < 0.05f) N++;
         else N = 0;
-        if(N > 10)
+        if(N > 5)
         {
             m_motorL.setSpeed(0);
             m_motorR.setSpeed(0);
             state = 2;
             logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
             somme_erreur_theta = 0;
-            N = 0;
-            Kp_angle = 3.5;
         }
     }
 
     // Etat 2 : Parcours du robot jusqu'au point M(x,y)
     if(state == 2) 
     {
-        if(delta_erreur_distance > 0) // On a dépassé le point
+        //Source d'erreurs et de ralentissements
+        /*if(delta_erreur_distance > 0) // On a dépassé le point
         {
             state = 1;
             return;
-        }
+        }*/
         
         if(abs(erreur_distance) > 55.0f) somme_erreur_distance = 0;
         
@@ -131,38 +134,51 @@
         }
         else
         {
-            if(cmd_g > 0.2f) cmd_g = 0.2f;
-            else if(cmd_g < -0.2f) cmd_g = -0.2f;
+            Kd_distance = 0.01;
+            if(cmd_g > lim_max) cmd_g = lim_max;
+            else if(cmd_g < -lim_max) cmd_g = -lim_max;
             
-            if(cmd_d > 0.2f) cmd_d = 0.2f;
-            else if(cmd_d < -0.2f) cmd_d = -0.2f;
+            if(cmd_d > lim_max) cmd_d = lim_max;
+            else if(cmd_d < -lim_max) cmd_d = -lim_max;
         }
         
-        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_g > 0.01f && cmd_g < lim_min) cmd_g = lim_min;
+        if(cmd_g < -0.01f && cmd_g > -lim_min) cmd_g = -lim_min;
         
-        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;
+        if(cmd_d > 0.01f && cmd_d < lim_min) cmd_d = lim_min;
+        if(cmd_d < -0.01f && cmd_d > -lim_min) cmd_d = -lim_min;
             
         m_motorL.setSpeed(cmd_g);
         m_motorR.setSpeed(cmd_d);
         
-        if(abs(erreur_distance) < 10.0f) N++;
+        if(abs(erreur_distance) < 5.0f) N++;
         else N = 0;
         if(N > 10)
         {
-            logger.printf("Erreur distance : %.2f\r\n", erreur_distance);
-            somme_erreur_distance = 0;
-            state = 3;
-            N = 0;
-            Kp_angle = 3.5;
+            delta_erreur_theta = 0;
+            erreur_precedente_theta = 0;
+            somme_erreur_theta = 0;
+            erreur_theta = 0;
+            if(squip == true) 
+            {
+                arrived = true;
+                squip = false;
+                state = 0;
+            }
+            else state = 3;
+            logger.printf("Erreur distance : %.2f, Arrived : %d, Etat = %d\r\n", erreur_distance, arrived, (int)state);
         }
     }
 
     // Etat 3 : Placement au bon angle Phi souhaité au point M(x,y)
-    if(state == 3 && squip == false)
+    if(state == 3)
     {
-        /*cmd = erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle;
+        erreur_theta = m_goalPhi-m_odometry.getTheta();
+        
+        if(erreur_theta <= PI) erreur_theta += 2.0f*PI;
+        if(erreur_theta >= PI) erreur_theta -= 2.0f*PI;
+        
+        cmd = erreur_theta*Kp_angle;
         
         if(cmd > limite) cmd = limite;
         else if(cmd < -limite) cmd = -limite;
@@ -174,17 +190,11 @@
         else N = 0;
         if(N > 10)
         {
-            m_motorL.setSpeed(0);
-            m_motorR.setSpeed(0);
             logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
             somme_erreur_theta = 0;
-            N = 0;
-            Kp_angle = 3.0;
             arrived = true;
-        }*/
-        m_motorL.setSpeed(0);
-        m_motorR.setSpeed(0);
-        arrived = true;
-        state = 0;
+            squip = false;
+            state = 0;
+        }
     }
 }