Diff: Asserv_Plan_B/planB.cpp

Revision:

101:78dc61bd330d

Parent:

100:a827a645d6c2

--- a/Asserv_Plan_B/planB.cpp	Thu Apr 30 16:16:29 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Thu Apr 30 16:23:15 2015 +0000
@@ -1,33 +1,23 @@
 #include "planB.h"
 #include "defines.h"
 
-
 extern Serial logger;
 
 aserv_planB::aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
 {
-    consigne_g = 0.0;
-    consigne_d = 0.0;
-    vitesse_g = 0;
-    vitesse_d = 0;
-    erreur_g = 0;
-    erreur_d = 0;
-    cmd_g = 0;
-    cmd_d = 0;
-    somme_erreur = 0;
-    somme_erreur_d = 0;
-    delta_erreur = 0;
     erreur_precedente = 0;
-    Kp = 0.30;
-    Ki = 0.01;
-    Kd = 0.5;
-    //etat_angle = 0;
+    Kp_angle = 1.2; //Fixed à 0.436 pour 180 deg
+    Kd_angle = 4.455;
     cmd = 0;
+    cmd_g = 0, cmd_d = 0;
     N = 0;
-    moyenne = 0;
-    limite = 0;
     done = false;
     state = 0; // Etat ou l'on ne fait rien
+    distance_g = 0;
+    distance_d = 0;
+    Kp_distance = 0.0075;
+    Ki_distance = 0;
+    Kd_distance = 0;
 }
 
 void aserv_planB::setGoal(float x, float y, float theta)
@@ -35,11 +25,13 @@
     m_goalX = x;
     m_goalY = y;
     m_goalTheta = theta;
-
-    state = 1; // Etat de rotation 1
+    thetaGoal = atan2(m_goalY-m_odometry.getY(),m_goalX-m_odometry.getX());
+    distance_g = sqrt(carre(m_goalY-m_odometry.getY())+carre(m_goalX-m_odometry.getX()));
+    distance_d = distance_g;
+    state = 2; // Etat de rotation 1
 }
 
-void aserv_planB::control_speed()
+/*void aserv_planB::control_speed()
 {
     vitesse_d = m_odometry.getVitRight();
     vitesse_g = m_odometry.getVitLeft();
@@ -51,52 +43,59 @@
 
     m_motorL.setSpeed(cmd_g);
     m_motorR.setSpeed(cmd_d);
-}
+}*/
 
 void aserv_planB::update(float dt)
 {
-    if(state == 1)
-    {
-        float thetaGoal = atan2(m_goalY-m_odometry.getY(),m_goalX-m_odometry.getX());
-
+    // Etat 1 : Angle theta pour viser dans la direction du point M(x,y)
+    if(state == 1 && N < 100)
+    {        
         float erreur_theta = thetaGoal-m_odometry.getTheta();
         
-        if(erreur_theta <= PI) erreur_theta += 2.0f*PI;
-        if(erreur_theta >= PI) erreur_theta -= 2.0f*PI;
+        if(erreur_theta <= PI) erreur_theta += 2.0*PI;
+        if(erreur_theta >= PI) erreur_theta -= 2.0*PI;
         
-        /*if(erreur_theta < 0) etat_angle = 1;
-        else if(erreur_theta > 0) etat_angle = 2;
-        else etat_angle = 0;*/
-        /*limite = (0.5-Kp*erreur_theta)/Ki;
-        if(somme_erreur >= limite) somme_erreur = limite;
-        if(somme_erreur <= -limite) somme_erreur = -limite;*/
+        //logger.printf("%.2f\r\n", erreur_theta*180/PI);
         
-        cmd = erreur_theta*Kp + somme_erreur*Ki - delta_erreur*Kd;
-        somme_erreur += erreur_theta;
-        delta_erreur = erreur_theta - erreur_precedente;
+        cmd = erreur_theta*Kp_angle + (erreur_theta-erreur_precedente)*Kd_angle;
         erreur_precedente = erreur_theta;
         
         m_motorL.setSpeed(-cmd);
         m_motorR.setSpeed(cmd);
 
-        //! Pas bon coeff, mais c'est l'idée
-        consigne_g = 0;//-erreur_theta*0.0001;
-        consigne_d = 0;//erreur_theta*0.0001;
-
-        /*if(erreur_theta <= abs(0.7))
-        {
-            done = true;
-            logger.printf("Posey\r\n");
+        N++;
+        if(N==100) // && (abs(erreur_theta)<=2.0)
+        {   
             state = 2;
-        }*/
+            logger.printf("%.2f %.2f\r\n", erreur_theta*180/PI, thetaGoal*180/PI);
+            m_odometry.setDistLeft(0);
+            m_odometry.setDistRight(0);
+            memo_g = m_odometry.getDistLeft();
+            memo_d = m_odometry.getDistRight();
+        }
     }
-    /*switch(etat_angle)
+    
+    // Etat 2 : Parcours du robot jusqu'au point M(x,y)
+    if(state == 2)
     {
-        case 0:
-
-        break;
-
-        case 1:
-
-        break;*/
+        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(0);
+        m_motorR.setSpeed(0);
+        
+        logger.printf("%.2f %.2f\r\n", m_odometry.getDistLeft(), m_odometry.getDistRight());
+        
+        //N++;
+        if(N==100) state = 2;
+    }
+    
+    // Etat 3 : Placement au bon angle Phi souhaité au point M(x,y)
+    if(state == 3)
+    {
+        m_motorL.setSpeed(0);
+        m_motorR.setSpeed(0);
+    }
 }