Diff: Asserv_Plan_B/planB.cpp

Revision:

94:5c37bcf73d14

Parent:

86:2fbe5db2627f

Child:

100:a827a645d6c2

--- a/Asserv_Plan_B/planB.cpp	Fri Apr 24 11:12:44 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Thu Apr 30 16:03:55 2015 +0000
@@ -1,23 +1,33 @@
 #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_angle = 1.2; //Fixed à 0.436 pour 180 deg
-    Kd_angle = 4.455;
+    Kp = 0.30;
+    Ki = 0.01;
+    Kd = 0.5;
+    //etat_angle = 0;
     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)
@@ -25,13 +35,11 @@
     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
+
+    state = 1; // Etat de rotation 1
 }
 
-/*void aserv_planB::control_speed()
+void aserv_planB::control_speed()
 {
     vitesse_d = m_odometry.getVitRight();
     vitesse_g = m_odometry.getVitLeft();
@@ -43,59 +51,53 @@
 
     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)
-    {        
+    if(state == 1)
+    {
+        float 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.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);
+        /*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;*/
         
-        cmd = erreur_theta*Kp_angle + (erreur_theta-erreur_precedente)*Kd_angle;
+        cmd = erreur_theta*Kp + somme_erreur*Ki - delta_erreur*Kd;
+        somme_erreur += erreur_theta;
+        delta_erreur = erreur_theta - erreur_precedente;
         erreur_precedente = erreur_theta;
         
         m_motorL.setSpeed(-cmd);
         m_motorR.setSpeed(cmd);
 
-        N++;
-        if(N==100) // && (abs(erreur_theta)<=2.0)
-        {   
+        //! 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");
             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)
+    /*switch(etat_angle)
     {
-        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);
-    }
+        case 0:
+
+        break;
+
+        case 1:
+
+        break;*/
 }