Diff: Asserv_Plan_B/planB.cpp

Revision:

70:56086a37f31f

Parent:

69:6354497f9f59

Child:

71:95d76c181b22

--- a/Asserv_Plan_B/planB.cpp	Fri Apr 10 05:13:51 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Fri Apr 10 15:23:05 2015 +0000
@@ -10,7 +10,9 @@
     erreur_d = 0;
     cmd_g = 0;
     cmd_d = 0;
-    Kp = 0.01;
+    somme_erreur_g = 0;
+    somme_erreur_d = 0;
+    Kp = 0.02;
 }
 
 void aserv_planB::control_speed()
@@ -30,4 +32,19 @@
 void aserv_planB::update(float dt)
 {
     control_speed();
-}
\ No newline at end of file
+}
+
+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);
+}