Diff: Asserv_Plan_B/planB.cpp

Revision:

72:b2a128486332

Parent:

71:95d76c181b22

Child:

79:d97090bb6470

--- a/Asserv_Plan_B/planB.cpp	Fri Apr 10 18:01:54 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Fri Apr 10 18:40:58 2015 +0000
@@ -6,8 +6,8 @@
 
 aserv_planB::aserv_planB(Odometry2 &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
 {
-    consigne_g = 10.0;
-    consigne_d = 10.0;
+    consigne_g = 0.0;
+    consigne_d = 0.0;
     vitesse_g = 0;
     vitesse_d = 0;
     erreur_g = 0;
@@ -33,8 +33,8 @@
 
 void aserv_planB::control_speed()
 {
-    vitesse_g = m_odometry.getVitRight();
-    vitesse_d = m_odometry.getVitLeft();
+    vitesse_d = m_odometry.getVitRight();
+    vitesse_g = m_odometry.getVitLeft();
     
     erreur_g = consigne_g - vitesse_g;
     cmd_g = erreur_g*Kp;
@@ -52,7 +52,15 @@
         float thetaGoal = atan2(m_goalY-m_odometry.getY(),m_goalX-m_odometry.getX());
         
         float erreur_theta = thetaGoal-m_odometry.getTheta();
-        logger.printf("%f\r\n",erreur_theta*180/PI);
+        if(erreur_theta <= PI) erreur_theta += 2.0*PI;
+        if(erreur_theta >= PI) erreur_theta -= 2.0*PI;
+        
+        //logger.printf("%.2f %.2f %.2f\r\n",m_odometry.getTheta()*180/PI,thetaGoal*180/PI,erreur_theta*180/PI);
+        
+        
+        //! 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();
     }