Diff: Asserv_Plan_B/planB.cpp

Revision:

64:6489bcfc1173

Parent:

62:454cd844fe1e

Child:

67:0bdf181586b5

--- a/Asserv_Plan_B/planB.cpp	Thu Apr 09 17:05:06 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Thu Apr 09 17:40:53 2015 +0000
@@ -2,7 +2,30 @@
 
 aserv_planB::aserv_planB(Odometry &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
 {
+    consigne_g = 10.0;
+    consigne_d = 10.0;
+    vitesse_g = 0;
+    vitesse_d = 0;
+    erreur_g = 0;
+    erreur_d = 0;
+    cmd_g = 0;
+    cmd_d = 0;
+    Kp = 0.05;
+}
+
+void aserv_planB::control_speed()
+{
+    vitesse_g = m_odometry.getVitLeft();
+    vitesse_d = m_odometry.getVitRight();
     
+    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)