Diff: Asserv_Plan_B/planB.cpp

Revision:

13:5355aed288b0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Asserv_Plan_B/planB.cpp	Tue Jan 05 17:08:15 2016 +0000
@@ -0,0 +1,199 @@
+#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)
+{
+    limite = 0.55;
+    lim_max = 0.30;
+    lim_min = 0.1995;
+    cmd = 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.2;
+    
+    Kp_distance = 0.0041;
+    Ki_distance = 0.0001;//0.000001
+    Kd_distance = 0.01;//0.05
+    
+    N = 0;
+    arrived = false;
+    squip = false;
+    state = 0; // Etat ou l'on ne fait rien
+}
+
+void aserv_planB::setGoal(float x, float y, float phi)
+{
+    arrived = false;
+    m_goalX = x;
+    m_goalY = y;
+    m_goalPhi = phi;
+    distanceGoal = sqrt(carre(m_goalX-m_odometry.getX())+carre(m_goalY-m_odometry.getY()));
+    state = 1; // Etat de rotation 1
+    Kd_distance = 0.01;
+    N = 0;
+}
+
+void aserv_planB::stop(void)
+{
+    m_motorL.setSpeed(0);
+    m_motorR.setSpeed(0);
+    state = 0;
+}
+
+void aserv_planB::setGoal(float x, float y)
+{
+    squip = true;
+    setGoal(x, y, 0);
+}
+
+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()));
+    
+    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;
+
+    // Etat 1 : Angle theta pour viser dans la direction du point M(x,y)
+    if(state == 1)
+    {
+        //logger.printf("%.2f\r\n", erreur_theta*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.05f) N++;
+        else N = 0;
+        if(N > 5)
+        {
+            m_motorL.setSpeed(0);
+            m_motorR.setSpeed(0);
+            state = 2;
+            //logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
+            somme_erreur_theta = 0;
+        }
+    }
+
+    // Etat 2 : Parcours du robot jusqu'au point M(x,y)
+    if(state == 2) 
+    {
+        //Source d'erreurs et de ralentissements
+        /*if(delta_erreur_distance > 0) // On a dépassé le point
+        {
+            state = 1;
+            return;
+        }*/
+        
+        if(abs(erreur_distance) > 55.0f) somme_erreur_distance = 0;
+        
+        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(abs(erreur_distance) > 55.0f)
+        {
+            if(cmd_g > limite) cmd_g = limite;
+            else if(cmd_g < -limite) cmd_g = -limite;
+            
+            if(cmd_d > limite) cmd_d = limite;
+            else if(cmd_d < -limite) cmd_d = -limite;
+        }
+        else
+        {
+            Kd_distance = 0.01;
+            if(cmd_g > lim_max) cmd_g = lim_max;
+            else if(cmd_g < -lim_max) cmd_g = -lim_max;
+            
+            if(cmd_d > lim_max) cmd_d = lim_max;
+            else if(cmd_d < -lim_max) cmd_d = -lim_max;
+        }
+        
+        if(cmd_g > 0.01f && cmd_g < lim_min) cmd_g = lim_min;
+        if(cmd_g < -0.01f && cmd_g > -lim_min) cmd_g = -lim_min;
+        
+        if(cmd_d > 0.01f && cmd_d < lim_min) cmd_d = lim_min;
+        if(cmd_d < -0.01f && cmd_d > -lim_min) cmd_d = -lim_min;
+            
+        m_motorL.setSpeed(cmd_g);
+        m_motorR.setSpeed(cmd_d);
+        
+        if(abs(erreur_distance) < 5.0f) N++;
+        else N = 0;
+        if(N > 10)
+        {
+            delta_erreur_theta = 0;
+            erreur_precedente_theta = 0;
+            somme_erreur_theta = 0;
+            erreur_theta = 0;
+            m_motorL.setSpeed(0);
+            m_motorR.setSpeed(0);
+            if(squip == true) 
+            {
+                arrived = true;
+                squip = false;
+                state = 0;
+            }
+            else state = 3;
+            //logger.printf("Erreur distance : %.2f, Arrived : %d, Etat = %d\r\n", erreur_distance, arrived, (int)state);
+        }
+    }
+
+    // Etat 3 : Placement au bon angle Phi souhaité au point M(x,y)
+    if(state == 3)
+    {
+        erreur_theta = m_goalPhi-m_odometry.getTheta();
+        
+        if(erreur_theta <= PI) erreur_theta += 2.0f*PI;
+        if(erreur_theta >= PI) erreur_theta -= 2.0f*PI;
+        
+        cmd = erreur_theta*Kp_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)
+        {
+            //logger.printf("Erreur theta : %.2f\r\n", erreur_theta*180/PI);
+            somme_erreur_theta = 0;
+            arrived = true;
+            squip = false;
+            state = 0;
+            m_motorL.setSpeed(0);
+            m_motorR.setSpeed(0);
+        }
+    }
+}