Diff: Asserv_Plan_B/planB.cpp

Revision:

79:d97090bb6470

Parent:

72:b2a128486332

Child:

83:6bcc38b1c5b5

Child:

84:24d727006218

--- a/Asserv_Plan_B/planB.cpp	Sat Apr 11 10:36:56 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Tue Apr 14 18:27:53 2015 +0000
@@ -14,12 +14,20 @@
     erreur_d = 0;
     cmd_g = 0;
     cmd_d = 0;
-    somme_erreur_g = 0;
+    somme_erreur = 0;
     somme_erreur_d = 0;
-    Kp = 0.02;
-    
-    
-    state = 0; // Etat ou on fait rien
+    delta_erreur = 0;
+    erreur_precedente = 0;
+    Kp = 0.30;
+    Ki = 0.01;
+    Kd = 0.5;
+    //etat_angle = 0;
+    cmd = 0;
+    N = 0;
+    moyenne = 0;
+    limite = 0;
+    done = false;
+    state = 0; // Etat ou l'on ne fait rien
 }
 
 void aserv_planB::setGoal(float x, float y, float theta)
@@ -27,7 +35,7 @@
     m_goalX = x;
     m_goalY = y;
     m_goalTheta = theta;
-    
+
     state = 1; // Etat de rotation 1
 }
 
@@ -35,12 +43,12 @@
 {
     vitesse_d = m_odometry.getVitRight();
     vitesse_g = m_odometry.getVitLeft();
-    
+
     erreur_g = consigne_g - vitesse_g;
     cmd_g = erreur_g*Kp;
     erreur_d = consigne_d - vitesse_d;
     cmd_d = erreur_d*Kp;
-    
+
     m_motorL.setSpeed(cmd_g);
     m_motorR.setSpeed(cmd_d);
 }
@@ -50,33 +58,46 @@
     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 %.2f %.2f\r\n",m_odometry.getTheta()*180/PI,thetaGoal*180/PI,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;
+        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;
-        
-        control_speed();
-    }
-}
 
-void aserv_planB::control_position()
-{
-    //position_g = m_odometry.getPosRight();
-    //position_d = m_odometry.getPosLeft();
-    
-    erreur_position_g = consigne_position_g - position_g;
-    cmd_g = erreur_position_g*Kp;
-    
-    erreur_position_d = consigne_position_d - position_d;
-    cmd_d = erreur_position_d*Kp;
-    
-    m_motorL.setSpeed(cmd_g);
-    m_motorR.setSpeed(cmd_d);
+        /*if(erreur_theta <= abs(0.7))
+        {
+            done = true;
+            logger.printf("Posey\r\n");
+            state = 2;
+        }*/
+    }
+    /*switch(etat_angle)
+    {
+        case 0:
+
+        break;
+
+        case 1:
+
+        break;*/
 }