Diff: Odometry/Odometry.h

Revision:

2:abdf8c6823a1

Parent:

0:ad9600df4a70

Child:

3:62e9d715de65

--- a/Odometry/Odometry.h	Mon Nov 16 11:34:08 2015 +0000
+++ b/Odometry/Odometry.h	Tue Nov 24 15:02:01 2015 +0000
@@ -5,27 +5,37 @@
 #include "RoboClaw.h"
 
 #define PI 3.1415926535897932384626433832795
+#define C 1.1538461538461538461538461538462
 
 /*
 *   Author : Benjamin Bertelone, reworked by Simon Emarre
 */
 
+extern Serial pc;
+
 class Odometry
 {
     public:
-        Odometry(double diameter_right, double diameter_left, double v);
+        Odometry(double diameter_right, double diameter_left, double v, RoboClaw &rc);
         
         void setPos(double x, double y, double theta);
         void setX(double x);
         void setY(double y);
         void setTheta(double theta);
         
-        void GotoXYT(double x, double y, double theta_goal);
-        void GotoThet(double theta_goal);
+        void GotoXYT(double x_goal, double y_goal, double theta_goal);
+        void GotoThet(double theta_);
+        void GotoB(double distance);
         
         double getX() {return x;}
         double getY() {return y;}
         double getTheta() {return theta;} // ]-PI;PI]
+        double getTheta_(double x, double y);
+        
+        double abs_d(double x){
+            if(x<0) return -x;
+            else return x;
+        }
         
         double getVitLeft() {return m_vitLeft;}
         double getVitRight() {return m_vitRight;}
@@ -41,9 +51,12 @@
         
         long getPulsesLeft(void) {return m_pulses_left;}
         long getPulsesRight(void) {return m_pulses_right;}
+        double carre(double a) {return a*a;}
+        
+        bool isArrivedRot(double theta_);
     
     private:
-    
+        RoboClaw &roboclaw;
         long m_pulses_left;
         long m_pulses_right;
         
@@ -52,6 +65,8 @@
         double m_distLeft, m_distRight;
         
         double m_distPerTick_left, m_distPerTick_right, m_v;
+        
+        double erreur_ang;
 };
 
 #endif