Diff: Asserv_Plan_B/planB.cpp

Revision:

84:24d727006218

Parent:

79:d97090bb6470

Child:

85:8e95432d99d3

--- a/Asserv_Plan_B/planB.cpp	Tue Apr 14 18:33:18 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Thu Apr 16 12:23:02 2015 +0000
@@ -1,27 +1,14 @@
 #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 = 0.436; //Fixed à 0.436
+    Ki = 0.0;  //Limite a 0.03
+    Kd = 0.0;
     cmd = 0;
     N = 0;
     moyenne = 0;
@@ -55,49 +42,28 @@
 
 void aserv_planB::update(float dt)
 {
-    if(state == 1)
+    // 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();
         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);
         
-        /*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 + somme_erreur*Ki - delta_erreur*Kd;
-        somme_erreur += erreur_theta;
-        delta_erreur = erreur_theta - erreur_precedente;
+        cmd = erreur_theta*Kp + (erreur_theta-erreur_precedente)*Kd;
         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");
-            state = 2;
-        }*/
+        N++;
+        if((N==100) && (abs(erreur_theta)<=1.0)) logger.printf("%.2f\r\n",abs(erreur_theta)*180/PI);
     }
-    /*switch(etat_angle)
+    if(state == 2)
     {
-        case 0:
-
-        break;
-
-        case 1:
-
-        break;*/
+        
+    }
 }