Diff: Asserv_Plan_B/planB.cpp

Revision:

108:890094ee202a

Parent:

106:05096985d1b2

Child:

112:df5388d9f706

--- a/Asserv_Plan_B/planB.cpp	Tue May 05 05:05:07 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Tue May 05 16:35:42 2015 +0000
@@ -5,22 +5,20 @@
 
 aserv_planB::aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
 {
-    erreur_precedente = 0;
     somme_erreur = 0;
     Kp_angle = 3.0; //Fixed à 3.0 pour 180 deg
-    Ki_angle = 0.0;
-    Kd_angle = 0.0;
+    Ki_angle = 0.00;
     cmd = 0;
     cmd_g = 0, cmd_d = 0;
     N = 0;
-    done = false;
+    arrived = false;
     squip = false;
     state = 0; // Etat ou l'on ne fait rien
     distanceGoal = 0;
     distance = 0;
     Kp_distance = 0.004;
-    Ki_distance = 0;
-    Kd_distance = 0;
+    Ki_distance = 0.001;
+    Kd_distance = 0.0;
 }
 
 void aserv_planB::setGoal(float x, float y, float phi)
@@ -28,9 +26,20 @@
     m_goalX = x;
     m_goalY = y;
     m_goalPhi = phi;
-    distanceGoal = sqrt(carre(m_goalY-m_odometry.getY())+carre(m_goalX-m_odometry.getX()));
-    state = 3; // Etat de rotation 1
+    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;
+}
+
+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)
@@ -41,89 +50,82 @@
 }
 
 void aserv_planB::update(float dt)
-{      
-    
+{
+
     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()));
     if(erreur_theta <= PI) erreur_theta += 2.0f*PI;
     if(erreur_theta >= PI) erreur_theta -= 2.0f*PI;
-    
+
     // Etat 1 : Angle theta pour viser dans la direction du point M(x,y)
-    if(state == 1 && N < 120)
-    {   
+    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;
         
+        if(cmd > 0.8) cmd = 0.8;
+        else if(cmd < -0.8) cmd = -0.8;
+        
         m_motorL.setSpeed(-cmd);
         m_motorR.setSpeed(cmd);
-
-        N++;
         
-        if(N==120) // && (abs(erreur_theta)<=2.0)
-        {   
+        if(abs(erreur_theta)< 0.05) N++;
+        else N = 0;
+        if(N > 10)
+        {
+            m_motorL.setSpeed(0);
+            m_motorR.setSpeed(0);
             state = 2;
             logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
+            somme_erreur = 0;
             N = 0;
-            /*m_odometry.setDistLeft(0);
-            m_odometry.setDistRight(0);
-            memo_g = m_odometry.getDistLeft();
-            memo_d = m_odometry.getDistRight();*/
-            
+            Kp_angle = 5.0;
         }
     }
-    
+
     // Etat 2 : Parcours du robot jusqu'au point M(x,y)
-    if(state == 2)
-    {    
-        float erreur_distance = sqrt(carre(m_goalX-m_odometry.getX())+carre(m_goalY-m_odometry.getY()));
-        //logger.printf("%.2f\r\n", erreur_distance);
+    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;
         
-        cmd_g = erreur_distance*Kp_distance - erreur_theta*Kp_angle;
-        cmd_d = erreur_distance*Kp_distance + erreur_theta*Kp_angle;
+        if(cmd_g > 0.8) cmd_g = 0.8;
+        else if(cmd_g < -0.8) cmd_g = -0.8;
+        
+        if(cmd_d > 0.8) cmd_d = 0.8;
+        else if(cmd_d < -0.8) cmd_d = -0.8;
+                
         m_motorL.setSpeed(cmd_g);
         m_motorR.setSpeed(cmd_d);
         
-        N++;
-        
-        if(abs(erreur_distance) <= 3.5 || N > 500) 
+        if(abs(erreur_distance) <= 10.0) 
         {
+            arrived = true;
+            logger.printf("Erreur distance : %.2f, arrived = %d\r\n", erreur_distance, (int)arrived);
             state = 3;
-            logger.printf("Erreur de distance : %.1f\r\n", erreur_distance);
+            N = 0;
         }
+        /*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)
-    if(state == 3 && squip==false)
+    if(state == 3)
     {
-        float erreur_phi = m_goalPhi-m_odometry.getTheta();
-        logger.printf("%.2f\r\n", erreur_phi);
+        //float erreur_phi = m_goalPhi-m_odometry.getTheta();
+        //logger.printf("%.2f\r\n", erreur_phi*180/PI);
         m_motorL.setSpeed(0);
         m_motorR.setSpeed(0);
-        done = true;
+        arrived = true;
     }
 }
-
-
-/*float erreur_distance_g = distance_g-(m_odometry.getDistLeft()-memo_g); //- distance parcourue par la roue gauche depuis l'état 2
-float erreur_distance_d = distance_d-(m_odometry.getDistRight()-memo_d);
-cmd_g = erreur_distance_g*Kp_distance;
-cmd_d = erreur_distance_d*Kp_distance;
-m_motorL.setSpeed(cmd_g);
-m_motorR.setSpeed(cmd_d);*/
-        
-/*void aserv_planB::control_speed()
-{
-    vitesse_d = m_odometry.getVitRight();
-    vitesse_g = m_odometry.getVitLeft();
-
-    erreur_g = consigne_g - vitesse_g;
-    cmd_g = erreur_g*Kp;
-    erreur_d = consigne_d - vitesse_d;
-    cmd_d = erreur_d*Kp;
-
-    m_motorL.setSpeed(cmd_g);
-    m_motorR.setSpeed(cmd_d);
-}*/
\ No newline at end of file