Diff: Asserv_Plan_B/planB.cpp

Revision:

105:6da275b01395

Parent:

93:4d5664e9188a

--- a/Asserv_Plan_B/planB.cpp	Sun May 03 16:02:17 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Mon May 04 06:18:27 2015 +0000
@@ -15,7 +15,7 @@
     state = 0; // Etat ou l'on ne fait rien
     distance_g = 0;
     distance_d = 0;
-    Kp_distance = 0.0075;
+    Kp_distance = 1.0;
     Ki_distance = 0;
     Kd_distance = 0;
 }
@@ -31,20 +31,6 @@
     state = 2; // Etat de rotation 1
 }
 
-/*void aserv_planB::control_speed()
-{
-    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);
-}*/
-
 void aserv_planB::update(float dt)
 {
     // Etat 1 : Angle theta pour viser dans la direction du point M(x,y)
@@ -64,7 +50,7 @@
         m_motorR.setSpeed(cmd);
 
         N++;
-        if(N==100) // && (abs(erreur_theta)<=2.0)
+        if(N==100) && (abs(erreur_theta)<=2.0))
         {   
             state = 2;
             logger.printf("%.2f %.2f\r\n", erreur_theta*180/PI, thetaGoal*180/PI);
@@ -86,10 +72,10 @@
         m_motorL.setSpeed(0);
         m_motorR.setSpeed(0);
         
-        logger.printf("%.2f %.2f\r\n", m_odometry.getDistLeft(), m_odometry.getDistRight());
+        logger.printf("%.2f %.2f\r\n", erreur_distance_g, erreur_distance_d);
         
         //N++;
-        if(N==100) state = 2;
+        //if(N==100) state = 2;
     }
     
     // Etat 3 : Placement au bon angle Phi souhaité au point M(x,y)
@@ -99,3 +85,17 @@
         m_motorR.setSpeed(0);
     }
 }
+
+/*void aserv_planB::control_speed()
+{
+    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);
+}*/
\ No newline at end of file