Revision 79:d97090bb6470, committed 2015-04-14

Comitter:

sype

Date:

Tue Apr 14 18:27:53 2015 +0000

Parent:

73:d8e1b543fbe3

Child:

80:5399183aa39b

Commit message:

PID posey

Changed in this revision

--- 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;*/
 }

--- a/Asserv_Plan_B/planB.h	Sat Apr 11 10:36:56 2015 +0000
+++ b/Asserv_Plan_B/planB.h	Tue Apr 14 18:27:53 2015 +0000
@@ -8,7 +8,6 @@
     aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR);
     void update(float dt);
     void control_speed();
-    void control_position();
     void setGoal(float x, float y, float theta);
     float Kp, Ki, Kd;
 
@@ -21,12 +20,19 @@
     float erreur_d, vitesse_d;
     float cmd_g, cmd_d;
     float consigne_g, consigne_d;
-    float somme_erreur_g, somme_erreur_d;
-    
+    float somme_erreur, somme_erreur_d;
+    float delta_erreur;
+    float erreur_precedente;
     float m_goalX, m_goalY, m_goalTheta;
+    bool done;
+    int N;
+    float moyenne;
+    float limite;
     
     char state;
+    //char etat_angle;
     
+    float cmd;
     float erreur_position_g, position_g;
     float erreur_position_d, position_d;
     float consigne_position_g, consigne_position_d;

--- a/Odometry/Odometry2.cpp	Sat Apr 11 10:36:56 2015 +0000
+++ b/Odometry/Odometry2.cpp	Tue Apr 14 18:27:53 2015 +0000
@@ -40,7 +40,7 @@
     this->x = x;
 }
 
-void Odometry2::setY(float Y)
+void Odometry2::setY(float y)
 {
     this->y = y;
 }

--- a/Odometry/Odometry2.h	Sat Apr 11 10:36:56 2015 +0000
+++ b/Odometry/Odometry2.h	Tue Apr 14 18:27:53 2015 +0000
@@ -11,7 +11,7 @@
         
         void setPos(float x, float y, float theta);
         void setX(float x);
-        void setY(float Y);
+        void setY(float y);
         void setTheta(float theta);
         
         float getX() {return x;}

--- a/main.cpp	Sat Apr 11 10:36:56 2015 +0000
+++ b/main.cpp	Tue Apr 14 18:27:53 2015 +0000
@@ -6,7 +6,7 @@
 #include "Map.h"
 #include "AX12.h"
 
-#define PLAN_A
+#define PLAN_B
 
 #ifdef PLAN_A
     #include "Odometry.h"
@@ -19,8 +19,8 @@
 #include "Motor.h"
 /*----------------------------------------------------------------------------------------------*/
 /*Serial*/    
-Serial logger(OUT_TX, OUT_RX); // tx, rx
-//Serial logger(USBTX,USBRX);
+//Serial logger(OUT_TX, OUT_RX); // tx, rx
+Serial logger(USBTX,USBRX);
 //logger.baud((int)115200);
 /*----------------------------------------------------------------------------------------------*/
 
@@ -56,12 +56,12 @@
         QEI qei_right(PB_3,PA_10,NC,1024*4,QEI::X4_ENCODING);
         QEI qei_left(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING);
         
-        Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,280);
+        Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,255);
     #else
         QEI qei_left(ODO_G_A,ODO_G_B,NC,1024*4,QEI::X4_ENCODING);
         QEI qei_right(ODO_D_A,ODO_D_B,NC,1024*4,QEI::X4_ENCODING);
         
-        Odometry2 odometry(&qei_left,&qei_right,63/2.,63/2.,280);
+        Odometry2 odometry(&qei_left,&qei_right,63/2.,63/2.,255);
         
         odometry.setTheta(PI/2);
         odometry.setX(0);
@@ -112,7 +112,7 @@
         }
     #else
         aserv_planB asserv(odometry,motorL,motorR);
-        asserv.setGoal(-5000,-5000,0);
+        asserv.setGoal(1,-5000,0);
         
         t.start();
         t.reset();
@@ -122,9 +122,9 @@
             //Parametrage des coef par bluetooth
             while(logger.readable()) {
                 char c = logger.getc();
-                if(c=='a') asserv.Kp += 0.001;
-                else if(c=='z') asserv.Kp -= 0.001;
-                //logger.printf("%f\n\r",asserv.Kp);
+                if(c=='a') asserv.Kd += 0.001;
+                else if(c=='z') asserv.Kd -= 0.001;
+                logger.printf("%f\n\r",asserv.Kd);
             }
             
             //Asservissement :
@@ -133,7 +133,7 @@
             odometry.update(dt);
             asserv.update(dt);
             
-            wait(0.1);             
+            wait_ms(100);            
         }
     #endif