Diff: Asserv_Plan_B/planB.cpp

Revision:

85:8e95432d99d3

Parent:

84:24d727006218

Child:

86:2fbe5db2627f

--- a/Asserv_Plan_B/planB.cpp	Thu Apr 16 12:23:02 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Sat Apr 18 09:39:15 2015 +0000
@@ -6,15 +6,18 @@
 aserv_planB::aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
 {
     erreur_precedente = 0;
-    Kp = 0.436; //Fixed à 0.436
-    Ki = 0.0;  //Limite a 0.03
-    Kd = 0.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)
@@ -22,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();
@@ -38,32 +43,59 @@
 
     m_motorL.setSpeed(cmd_g);
     m_motorR.setSpeed(cmd_d);
-}
+}*/
 
 void aserv_planB::update(float dt)
 {
     // Etat 1 : Angle theta pour viser dans la direction du point M(x,y)
     if(state == 1 && N < 100)
-    {
-        float thetaGoal = atan2(m_goalY-m_odometry.getY(),m_goalX-m_odometry.getX());
+    {        
+        float erreur_theta = thetaGoal-m_odometry.getTheta();
         
-        float erreur_theta = thetaGoal-m_odometry.getTheta();
         if(erreur_theta <= PI) erreur_theta += 2.0*PI;
         if(erreur_theta >= PI) erreur_theta -= 2.0*PI;
         
-        logger.printf("%.2f\r\n",erreur_theta*180/PI);
+        //logger.printf("%.2f\r\n", erreur_theta*180/PI);
         
-        cmd = erreur_theta*Kp + (erreur_theta-erreur_precedente)*Kd;
+        cmd = erreur_theta*Kp_angle + (erreur_theta-erreur_precedente)*Kd_angle;
         erreur_precedente = erreur_theta;
         
         m_motorL.setSpeed(-cmd);
         m_motorR.setSpeed(cmd);
 
         N++;
-        if((N==100) && (abs(erreur_theta)<=1.0)) logger.printf("%.2f\r\n",abs(erreur_theta)*180/PI);
+        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();
+        }
     }
-    if(state == 2)
+    
+    // Etat 2 : Parcours du robot jusqu'au point M(x,y)
+    if(state == 2 && N < 100)
     {
+        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);
     }
 }