Diff: Asserv_Plan_B/planB.cpp

Revision:

116:73d7d87e0299

Parent:

112:df5388d9f706

Child:

119:c45efcd706d9

diff -r c88c058ced64 -r 73d7d87e0299 Asserv_Plan_B/planB.cpp
--- a/Asserv_Plan_B/planB.cpp	Wed May 06 06:51:49 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Wed May 06 11:14:02 2015 +0000
@@ -5,21 +5,33 @@
 
 aserv_planB::aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
 {
-    somme_erreur = 0;
-    Kp_angle = 3.0; //Fixed à 3.0 pour 180 deg
-    Ki_angle = 0.001;
-    limite = 0.75;
+    limite = 0.65;
     cmd = 0;
-    cmd_g = 0, cmd_d = 0;
+    cmd_g = 0;
+    cmd_d = 0;
+    
+    somme_erreur_theta = 0;
+    delta_erreur_theta = 0;
+    erreur_precedente_theta = 0;
+    
+    somme_erreur_distance = 0;
+    delta_erreur_distance = 0;
+    erreur_precedente_distance = 0;
+    distanceGoal = 0;
+    distance = 0;
+    
+    Kp_angle = 3.5; //Fixed à 3.0 pour 180 deg
+    Ki_angle = 0.0;
+    Kd_angle = 0.1;
+    
+    Kp_distance = 0.0042;
+    Ki_distance = 0.00000;//0.000001
+    Kd_distance = 0.0;//0.05
+    
     N = 0;
     arrived = false;
     squip = false;
     state = 0; // Etat ou l'on ne fait rien
-    distanceGoal = 0;
-    distance = 0;
-    Kp_distance = 0.004;
-    Ki_distance = 0.0;
-    Kd_distance = 0.0;
 }
 
 void aserv_planB::setGoal(float x, float y, float phi)
@@ -30,7 +42,6 @@
     distanceGoal = sqrt(carre(m_goalX-m_odometry.getX())+carre(m_goalY-m_odometry.getY()));
     state = 1; // Etat de rotation 1
     N = 0;
-    Kp_angle = 3.0;
     arrived = false;
 }
 
@@ -38,9 +49,9 @@
 {
     m_motorL.setSpeed(0);
     m_motorR.setSpeed(0);
-    m_goalX = m_odometry.getX();
+    /*m_goalX = m_odometry.getX();
     m_goalY = m_odometry.getY();
-    setGoal(m_goalX, m_goalY);
+    setGoal(m_goalX, m_goalY);*/
 }
 
 void aserv_planB::setGoal(float x, float y)
@@ -52,10 +63,20 @@
 
 void aserv_planB::update(float dt)
 {
-
-    thetaGoal = atan2(m_goalY-m_odometry.getY(),m_goalX-m_odometry.getX());
+    if(state == 1) thetaGoal = atan2(m_goalY-m_odometry.getY(),m_goalX-m_odometry.getX());
+    else if(state == 3) thetaGoal = m_goalPhi;
     float erreur_theta = thetaGoal-m_odometry.getTheta();
+    
     float erreur_distance = sqrt(carre(m_goalX-m_odometry.getX())+carre(m_goalY-m_odometry.getY()));
+    
+    delta_erreur_theta = erreur_theta - erreur_precedente_theta;
+    erreur_precedente_theta = erreur_theta;
+    somme_erreur_theta += erreur_theta;
+    
+    delta_erreur_distance = erreur_distance - erreur_precedente_distance;
+    erreur_precedente_distance = erreur_distance;
+    somme_erreur_distance += erreur_distance;
+    
     if(erreur_theta <= PI) erreur_theta += 2.0f*PI;
     if(erreur_theta >= PI) erreur_theta -= 2.0f*PI;
 
@@ -63,9 +84,7 @@
     if(state == 1)
     {
         //logger.printf("%.2f\r\n", erreur_theta*180/PI);
-        cmd = erreur_theta*Kp_angle + (erreur_theta-erreur_precedente)*Kd_angle + somme_erreur*Ki_angle;
-        erreur_precedente = erreur_theta;
-        somme_erreur += erreur_theta;
+        cmd = erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle;
         
         if(cmd > limite) cmd = limite;
         else if(cmd < -limite) cmd = -limite;
@@ -81,19 +100,18 @@
             m_motorR.setSpeed(0);
             state = 2;
             logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
-            somme_erreur = 0;
+            somme_erreur_theta = 0;
             N = 0;
-            Kp_angle = 3.0;
+            Kp_angle = 3.5;
         }
     }
 
     // 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*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;
+        //logger.printf("%.2f %.2f %.2f\r\n", erreur_distance, cmd_g, cmd_d); //+ somme_erreur_distance*Ki_distance
+        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;
@@ -106,19 +124,39 @@
         
         if(abs(erreur_distance) < 20.0) N++;
         else N = 0;
-        if(N > 50)
+        if(N > 10)
         {
             logger.printf("Erreur distance : %.2f\r\n", erreur_distance);
+            somme_erreur_distance = 0;
             state = 3;
             N = 0;
+            Kp_angle = 3.5;
         }
     }
 
     // Etat 3 : Placement au bon angle Phi souhaité au point M(x,y)
-    if(state == 3)
+    if(state == 3 && squip == false)
     {
-        //float erreur_phi = m_goalPhi-m_odometry.getTheta();
-        //logger.printf("%.2f\r\n", erreur_phi*180/PI);
+        /*cmd = erreur_theta*Kp_angle + delta_erreur_theta*Kd_angle + somme_erreur_theta*Ki_angle;
+        
+        if(cmd > limite) cmd = limite;
+        else if(cmd < -limite) cmd = -limite;
+        
+        m_motorL.setSpeed(-cmd);
+        m_motorR.setSpeed(cmd);
+        
+        if(abs(erreur_theta)< 0.05) N++;
+        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;