ARES / Mbed 2 deprecated Robot2016_2-0

Robot secondaire

Dependencies:   RoboClaw mbed StepperMotor

Fork of RoboClaw by Simon Emarre

Odometry/Odometry.h@7:961c1acdf753, 2015-11-24 (annotated)

Committer:
IceTeam
Date:
Tue Nov 24 22:54:55 2015 +0000
Revision:
7:961c1acdf753
Parent:
6:f54df7542988
Child:
8:12d7123a500e
(Romain) Toujours de la doc ;)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sype 0:ad9600df4a70 1 #ifndef ODOMETRY_H
sype 0:ad9600df4a70 2 #define ODOMETRY_H
sype 0:ad9600df4a70 3
sype 0:ad9600df4a70 4 #include "mbed.h"
sype 0:ad9600df4a70 5 #include "RoboClaw.h"
sype 0:ad9600df4a70 6
sype 0:ad9600df4a70 7 #define PI 3.1415926535897932384626433832795
sype 2:abdf8c6823a1 8 #define C 1.1538461538461538461538461538462
sype 0:ad9600df4a70 9
sype 0:ad9600df4a70 10 /*
sype 0:ad9600df4a70 11 * Author : Benjamin Bertelone, reworked by Simon Emarre
sype 0:ad9600df4a70 12 */
sype 0:ad9600df4a70 13
sype 2:abdf8c6823a1 14 extern Serial pc;
sype 2:abdf8c6823a1 15
sype 0:ad9600df4a70 16 class Odometry
sype 0:ad9600df4a70 17 {
sype 0:ad9600df4a70 18 public:
IceTeam 6:f54df7542988 19 /** Constructeur standart
IceTeam 6:f54df7542988 20 * @param diameter_right Definit le diametre de la roue droite
IceTeam 6:f54df7542988 21 * @param diameter_left Definit le diametre de la roue gauche
IceTeam 6:f54df7542988 22 * @param v Definit l'entraxe du robot
IceTeam 6:f54df7542988 23 * @param rc Definit une reference vers l'objet permettant l'asserv des moteurs
IceTeam 6:f54df7542988 24 @note Cet objet doit etre initialise en amont
IceTeam 6:f54df7542988 25 */
sype 2:abdf8c6823a1 26 Odometry(double diameter_right, double diameter_left, double v, RoboClaw &rc);
sype 0:ad9600df4a70 27
IceTeam 7:961c1acdf753 28 /** Les fonctions suivantes permettent de reinitialiser les valeurs de position de l'odometrie.
IceTeam 7:961c1acdf753 29 */
sype 0:ad9600df4a70 30 void setPos(double x, double y, double theta);
sype 0:ad9600df4a70 31 void setX(double x);
sype 0:ad9600df4a70 32 void setY(double y);
sype 0:ad9600df4a70 33 void setTheta(double theta);
sype 0:ad9600df4a70 34
IceTeam 7:961c1acdf753 35 /** Les fonctions suivantes permettent de deplacer le robot vers une position ou un angle voulu
IceTeam 7:961c1acdf753 36 */
sype 2:abdf8c6823a1 37 void GotoXYT(double x_goal, double y_goal, double theta_goal);
sype 2:abdf8c6823a1 38 void GotoThet(double theta_);
sype 3:62e9d715de65 39 void GotoDist(double distance);
sype 0:ad9600df4a70 40
sype 0:ad9600df4a70 41 double getX() {return x;}
sype 0:ad9600df4a70 42 double getY() {return y;}
sype 0:ad9600df4a70 43 double getTheta() {return theta;} // ]-PI;PI]
sype 2:abdf8c6823a1 44 double getTheta_(double x, double y);
sype 2:abdf8c6823a1 45
sype 2:abdf8c6823a1 46 double abs_d(double x){
sype 2:abdf8c6823a1 47 if(x<0) return -x;
sype 2:abdf8c6823a1 48 else return x;
sype 2:abdf8c6823a1 49 }
sype 0:ad9600df4a70 50
IceTeam 7:961c1acdf753 51 /** Les fonction suivantes sont actuellement inutilisables, elles pourraient buguer en cas d'utilisation.
IceTeam 7:961c1acdf753 52 * @note pour que les fonctions retournent une valeur correcte, il faut actualiser l'odometrie avec update_odo auparavant
IceTeam 7:961c1acdf753 53 */
sype 0:ad9600df4a70 54 double getVitLeft() {return m_vitLeft;}
sype 0:ad9600df4a70 55 double getVitRight() {return m_vitRight;}
sype 0:ad9600df4a70 56
sype 0:ad9600df4a70 57 double getDistLeft() {return m_distLeft;}
sype 0:ad9600df4a70 58 double getDistRight() {return m_distRight;}
sype 0:ad9600df4a70 59
sype 0:ad9600df4a70 60 void setDistLeft(double dist) {m_distLeft = dist;}
sype 0:ad9600df4a70 61 void setDistRight(double dist) {m_distRight = dist;}
IceTeam 7:961c1acdf753 62
sype 0:ad9600df4a70 63 double calcul_distance(double x, double y, double theta_goal);
sype 0:ad9600df4a70 64
sype 3:62e9d715de65 65 int32_t getPulsesLeft(void) {return m_pulses_left;}
sype 3:62e9d715de65 66 int32_t getPulsesRight(void) {return m_pulses_right;}
sype 2:abdf8c6823a1 67 double carre(double a) {return a*a;}
sype 2:abdf8c6823a1 68
IceTeam 7:961c1acdf753 69 /** La fonction retourne vraie quand le robot atteint l'angle voulu avec une marge d'erreur definie par la fonction
IceTeam 7:961c1acdf753 70 * @param theta_ valeur de l'angle devant etre atteint
IceTeam 7:961c1acdf753 71 */
IceTeam 7:961c1acdf753 72 bool isArrivedRot(double theta_);
IceTeam 7:961c1acdf753 73 /** Permet de mettre à jour les valeurs de l'odometrie
IceTeam 7:961c1acdf753 74 */
IceTeam 7:961c1acdf753 75 void update_odo(void);
sype 0:ad9600df4a70 76
sype 0:ad9600df4a70 77 private:
sype 2:abdf8c6823a1 78 RoboClaw &roboclaw;
sype 3:62e9d715de65 79 int32_t m_pulses_left;
sype 3:62e9d715de65 80 int32_t m_pulses_right;
sype 0:ad9600df4a70 81
sype 0:ad9600df4a70 82 double x, y, theta;
sype 0:ad9600df4a70 83 double m_vitLeft, m_vitRight;
sype 0:ad9600df4a70 84 double m_distLeft, m_distRight;
sype 0:ad9600df4a70 85
sype 0:ad9600df4a70 86 double m_distPerTick_left, m_distPerTick_right, m_v;
sype 2:abdf8c6823a1 87
sype 2:abdf8c6823a1 88 double erreur_ang;
sype 0:ad9600df4a70 89 };
sype 0:ad9600df4a70 90
sype 0:ad9600df4a70 91 #endif