Diff: Asserv_Plan_B/planB.cpp

Revision:

106:05096985d1b2

Parent:

93:4d5664e9188a

Child:

108:890094ee202a

--- a/Asserv_Plan_B/planB.cpp	Mon May 04 13:00:28 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Tue May 05 05:05:07 2015 +0000
@@ -6,31 +6,114 @@
 aserv_planB::aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
 {
     erreur_precedente = 0;
-    Kp_angle = 1.2; //Fixed à 0.436 pour 180 deg
-    Kd_angle = 4.455;
+    somme_erreur = 0;
+    Kp_angle = 3.0; //Fixed à 3.0 pour 180 deg
+    Ki_angle = 0.0;
+    Kd_angle = 0.0;
     cmd = 0;
     cmd_g = 0, cmd_d = 0;
     N = 0;
     done = false;
+    squip = false;
     state = 0; // Etat ou l'on ne fait rien
-    distance_g = 0;
-    distance_d = 0;
-    Kp_distance = 0.0075;
+    distanceGoal = 0;
+    distance = 0;
+    Kp_distance = 0.004;
     Ki_distance = 0;
     Kd_distance = 0;
 }
 
-void aserv_planB::setGoal(float x, float y, float theta)
+void aserv_planB::setGoal(float x, float y, float phi)
 {
     m_goalX = x;
     m_goalY = y;
-    m_goalTheta = theta;
-    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
+    m_goalPhi = phi;
+    distanceGoal = sqrt(carre(m_goalY-m_odometry.getY())+carre(m_goalX-m_odometry.getX()));
+    state = 3; // Etat de rotation 1
+    N = 0;
+}
+
+void aserv_planB::setGoal(float x, float y)
+{
+    squip = true;
+    setGoal(x, y, 0);
+    squip = false;
 }
 
+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();
+    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)
+    {   
+        //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;
+        
+        m_motorL.setSpeed(-cmd);
+        m_motorR.setSpeed(cmd);
+
+        N++;
+        
+        if(N==120) // && (abs(erreur_theta)<=2.0)
+        {   
+            state = 2;
+            logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
+            N = 0;
+            /*m_odometry.setDistLeft(0);
+            m_odometry.setDistRight(0);
+            memo_g = m_odometry.getDistLeft();
+            memo_d = m_odometry.getDistRight();*/
+            
+        }
+    }
+    
+    // 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);
+        
+        cmd_g = erreur_distance*Kp_distance - erreur_theta*Kp_angle;
+        cmd_d = erreur_distance*Kp_distance + erreur_theta*Kp_angle;
+        m_motorL.setSpeed(cmd_g);
+        m_motorR.setSpeed(cmd_d);
+        
+        N++;
+        
+        if(abs(erreur_distance) <= 3.5 || N > 500) 
+        {
+            state = 3;
+            logger.printf("Erreur de distance : %.1f\r\n", erreur_distance);
+        }
+    }
+    
+    // Etat 3 : Placement au bon angle Phi souhaité au point M(x,y)
+    if(state == 3 && squip==false)
+    {
+        float erreur_phi = m_goalPhi-m_odometry.getTheta();
+        logger.printf("%.2f\r\n", erreur_phi);
+        m_motorL.setSpeed(0);
+        m_motorR.setSpeed(0);
+        done = 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();
@@ -43,59 +126,4 @@
 
     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 erreur_theta = thetaGoal-m_odometry.getTheta();
-        
-        if(erreur_theta <= PI) erreur_theta += 2.0f*PI;
-        if(erreur_theta >= PI) erreur_theta -= 2.0f*PI;
-        
-        //logger.printf("%.2f\r\n", erreur_theta*180/PI);
-        
-        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)<=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();
-        }
-    }
-    
-    // Etat 2 : Parcours du robot jusqu'au point M(x,y)
-    if(state == 2)
-    {
-        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);
-    }
-}
+}*/
\ No newline at end of file