Odometry.h

00001 #ifndef ODOMETRY_H
00002 #define ODOMETRY_H
00003 
00004 #include "mbed.h"
00005 #include "../RoboClaw/RoboClaw.h"
00006 
00007 #define PI 3.1415926535897932384626433832795
00008 #define C 1.0
00009 
00010 /* Vitesse d'acceleration d'angle reduite de 8000->4000 */
00011 #define accel_angle 8000
00012 #define vitesse_angle 12000
00013 #define deccel_angle 8000
00014 
00015 #define accel_dista 12000
00016 #define vitesse_dista 20000
00017 #define deccel_dista 12000
00018 
00019 /* Si le robot parcourt un trop grand angle, il diminuer l'entraxe, sinon l'augmenter */
00020 #define ENTRAXE 243.8
00021 
00022 
00023 /*
00024 *   Author : Benjamin Bertelone, reworked by Simon Emarre
00025 */
00026 
00027 extern Serial logger;
00028 extern DigitalOut led;
00029 
00030 /** Permet la gestion de l'odometrie du robot **/
00031 class Odometry
00032 {
00033 public:
00034     /** Constructeur standard
00035     * @param diameter_right Definit le diametre de la roue droite
00036     * @param diameter_left Definit le diametre de la roue gauche
00037     * @param v Definit l'entraxe du robot
00038     * @param rc Definit une reference vers l'objet permettant l'asserv des moteurs
00039     * @note Cet objet doit etre initialise en amont
00040     */
00041     Odometry(double diameter_right, double diameter_left, double v, uint16_t quadrature, RoboClaw &rc);
00042 
00043     /** Demande au robot d'effectuer un certain nombre de tour sur lui même */
00044     void TestEntraxe(int i);
00045 
00046     /** Demande au robot d'effectuer un déplacement sur l'avant. Voir si l'on peut enlever la correction PID */
00047     void Forward(float i);
00048 
00049     /** Les fonctions suivantes permettent de reinitialiser les valeurs de position de l'odometrie.
00050     */
00051     void setPos(double x, double y, double theta);
00052     void setX(double x);
00053     void setY(double y);
00054     void setTheta(double theta);
00055 
00056     /** Les fonctions suivantes permettent de deplacer le robot vers une position ou un angle voulu
00057     */
00058     void GotoXY(double x_goal, double y_goal);
00059     void GotoXYT(double x_goal, double y_goal, double theta_goal);
00060     void GotoThet(double theta_);
00061     void GotoDist(double distance);
00062 
00063     double getX() {
00064         return x;
00065     }
00066     double getY() {
00067         return y;
00068     }
00069     double getTheta() {
00070         return theta;   // ]-PI;PI]
00071     }
00072     double getTheta_(double x, double y);
00073 
00074     double abs_d(double x) {
00075         if(x<0) return -x;
00076         else return x;
00077     }
00078 
00079     /* Les fonction suivantes sont actuellement inutilisables, elles pourraient buguer en cas d'utilisation. */
00080 
00081     double getVitLeft() {
00082         return m_vitLeft;
00083     }
00084     double getVitRight() {
00085         return m_vitRight;
00086     }
00087 
00088     double getDistLeft() {
00089         return m_distLeft;
00090     }
00091     double getDistRight() {
00092         return m_distRight;
00093     }
00094 
00095     void setDistLeft(double dist) {
00096         m_distLeft = dist;
00097     }
00098     void setDistRight(double dist) {
00099         m_distRight = dist;
00100     }
00101 
00102     double calcul_distance(double x, double y, double theta_goal);
00103 
00104     int32_t getPulsesLeft(void) {
00105         return m_pulses_left;
00106     }
00107     int32_t getPulsesRight(void) {
00108         return m_pulses_right;
00109     }
00110     double carre(double a) {
00111         return a*a;
00112     }
00113     
00114     void getEnc();
00115     
00116     /** La fonction retourne vraie quand le robot atteint l'angle voulu avec une marge d'erreur definie par la fonction
00117     * @param theta_ valeur de l'angle devant etre atteint
00118     */
00119     bool isArrived(void) {return arrived;}
00120     /** Permet de mettre à jour les valeurs de l'odometrie
00121     */
00122     void update_odo(void);
00123 
00124 private:
00125     RoboClaw &roboclaw;
00126     int32_t m_pulses_left;
00127     int32_t m_pulses_right;
00128     uint8_t pos_prog;
00129     double x, y, theta;
00130     double m_vitLeft, m_vitRight;
00131     double m_distLeft, m_distRight;
00132 
00133     double m_distPerTick_left, m_distPerTick_right, m_v;
00134 
00135     double erreur_ang;
00136     bool arrived;
00137 };
00138 
00139 #endif