Diff: Asserv_Plan_B/planB.cpp

Revision:

71:95d76c181b22

Parent:

70:56086a37f31f

Child:

72:b2a128486332

--- a/Asserv_Plan_B/planB.cpp	Fri Apr 10 15:23:05 2015 +0000
+++ b/Asserv_Plan_B/planB.cpp	Fri Apr 10 18:01:54 2015 +0000
@@ -1,6 +1,10 @@
 #include "planB.h"
+#include "defines.h"
+
 
-aserv_planB::aserv_planB(Odometry &odometry,Motor &motorL,Motor &motorR) : m_odometry(odometry), m_motorL(motorL), m_motorR(motorR)
+extern Serial logger;
+
+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;
@@ -13,6 +17,18 @@
     somme_erreur_g = 0;
     somme_erreur_d = 0;
     Kp = 0.02;
+    
+    
+    state = 0; // Etat ou on fait rien
+}
+
+void aserv_planB::setGoal(float x, float y, float theta)
+{
+    m_goalX = x;
+    m_goalY = y;
+    m_goalTheta = theta;
+    
+    state = 1; // Etat de rotation 1
 }
 
 void aserv_planB::control_speed()
@@ -31,13 +47,21 @@
 
 void aserv_planB::update(float dt)
 {
-    control_speed();
+    if(state == 1)
+    {
+        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);
+        
+        control_speed();
+    }
 }
 
 void aserv_planB::control_position()
 {
-    position_g = m_odometry.getPosRight();
-    position_d = m_odometry.getPosLeft();
+    //position_g = m_odometry.getPosRight();
+    //position_d = m_odometry.getPosLeft();
     
     erreur_position_g = consigne_position_g - position_g;
     cmd_g = erreur_position_g*Kp;