ARES / Mbed 2 deprecated Robot2016_2-0

Robot secondaire

Dependencies:   RoboClaw mbed StepperMotor

Fork of RoboClaw by Simon Emarre

Odometry/Odometry.h@9:e39b218ab20d, 2015-11-24 (annotated)

Committer:
IceTeam
Date:
Tue Nov 24 23:05:12 2015 +0000
Revision:
9:e39b218ab20d
Parent:
8:12d7123a500e
Child:
10:ae3178aa94e9
Test de r?solution de bug pour la g?n?ration de 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
IceTeam 9:e39b218ab20d 16 /** Permet la gestion de l'odometrie du robot **/
sype 0:ad9600df4a70 17 class Odometry
sype 0:ad9600df4a70 18 {
IceTeam 8:12d7123a500e 19 public:
IceTeam 8:12d7123a500e 20 /** Constructeur standart
IceTeam 8:12d7123a500e 21 * @param diameter_right Definit le diametre de la roue droite
IceTeam 8:12d7123a500e 22 * @param diameter_left Definit le diametre de la roue gauche
IceTeam 8:12d7123a500e 23 * @param v Definit l'entraxe du robot
IceTeam 8:12d7123a500e 24 * @param rc Definit une reference vers l'objet permettant l'asserv des moteurs
IceTeam 8:12d7123a500e 25 @note Cet objet doit etre initialise en amont
IceTeam 8:12d7123a500e 26 */
IceTeam 8:12d7123a500e 27 Odometry(double diameter_right, double diameter_left, double v, RoboClaw &rc);
IceTeam 8:12d7123a500e 28
IceTeam 8:12d7123a500e 29 /** Les fonctions suivantes permettent de reinitialiser les valeurs de position de l'odometrie.
IceTeam 8:12d7123a500e 30 */
IceTeam 8:12d7123a500e 31 void setPos(double x, double y, double theta);
IceTeam 8:12d7123a500e 32 void setX(double x);
IceTeam 8:12d7123a500e 33 void setY(double y);
IceTeam 8:12d7123a500e 34 void setTheta(double theta);
IceTeam 8:12d7123a500e 35
IceTeam 8:12d7123a500e 36 /** Les fonctions suivantes permettent de deplacer le robot vers une position ou un angle voulu
IceTeam 8:12d7123a500e 37 */
IceTeam 8:12d7123a500e 38 void GotoXYT(double x_goal, double y_goal, double theta_goal);
IceTeam 8:12d7123a500e 39 void GotoThet(double theta_);
IceTeam 8:12d7123a500e 40 void GotoDist(double distance);
IceTeam 8:12d7123a500e 41
IceTeam 8:12d7123a500e 42 double getX() {
IceTeam 8:12d7123a500e 43 return x;
IceTeam 8:12d7123a500e 44 }
IceTeam 8:12d7123a500e 45 double getY() {
IceTeam 8:12d7123a500e 46 return y;
IceTeam 8:12d7123a500e 47 }
IceTeam 8:12d7123a500e 48 double getTheta() {
IceTeam 8:12d7123a500e 49 return theta; // ]-PI;PI]
IceTeam 8:12d7123a500e 50 }
IceTeam 8:12d7123a500e 51 double getTheta_(double x, double y);
IceTeam 8:12d7123a500e 52
IceTeam 8:12d7123a500e 53 double abs_d(double x) {
IceTeam 8:12d7123a500e 54 if(x<0) return -x;
IceTeam 8:12d7123a500e 55 else return x;
IceTeam 8:12d7123a500e 56 }
IceTeam 7:961c1acdf753 57
IceTeam 8:12d7123a500e 58 /** Les fonction suivantes sont actuellement inutilisables, elles pourraient buguer en cas d'utilisation.
IceTeam 8:12d7123a500e 59 * @note pour que les fonctions retournent une valeur correcte, il faut actualiser l'odometrie avec update_odo auparavant
IceTeam 8:12d7123a500e 60 */
IceTeam 8:12d7123a500e 61 double getVitLeft() {
IceTeam 8:12d7123a500e 62 return m_vitLeft;
IceTeam 8:12d7123a500e 63 }
IceTeam 8:12d7123a500e 64 double getVitRight() {
IceTeam 8:12d7123a500e 65 return m_vitRight;
IceTeam 8:12d7123a500e 66 }
IceTeam 8:12d7123a500e 67
IceTeam 8:12d7123a500e 68 double getDistLeft() {
IceTeam 8:12d7123a500e 69 return m_distLeft;
IceTeam 8:12d7123a500e 70 }
IceTeam 8:12d7123a500e 71 double getDistRight() {
IceTeam 8:12d7123a500e 72 return m_distRight;
IceTeam 8:12d7123a500e 73 }
IceTeam 8:12d7123a500e 74
IceTeam 8:12d7123a500e 75 void setDistLeft(double dist) {
IceTeam 8:12d7123a500e 76 m_distLeft = dist;
IceTeam 8:12d7123a500e 77 }
IceTeam 8:12d7123a500e 78 void setDistRight(double dist) {
IceTeam 8:12d7123a500e 79 m_distRight = dist;
IceTeam 8:12d7123a500e 80 }
IceTeam 8:12d7123a500e 81
IceTeam 8:12d7123a500e 82 double calcul_distance(double x, double y, double theta_goal);
IceTeam 8:12d7123a500e 83
IceTeam 8:12d7123a500e 84 int32_t getPulsesLeft(void) {
IceTeam 8:12d7123a500e 85 return m_pulses_left;
IceTeam 8:12d7123a500e 86 }
IceTeam 8:12d7123a500e 87 int32_t getPulsesRight(void) {
IceTeam 8:12d7123a500e 88 return m_pulses_right;
IceTeam 8:12d7123a500e 89 }
IceTeam 8:12d7123a500e 90 double carre(double a) {
IceTeam 8:12d7123a500e 91 return a*a;
IceTeam 8:12d7123a500e 92 }
IceTeam 8:12d7123a500e 93
IceTeam 8:12d7123a500e 94 /** La fonction retourne vraie quand le robot atteint l'angle voulu avec une marge d'erreur definie par la fonction
IceTeam 8:12d7123a500e 95 * @param theta_ valeur de l'angle devant etre atteint
IceTeam 8:12d7123a500e 96 */
IceTeam 8:12d7123a500e 97 bool isArrivedRot(double theta_);
IceTeam 8:12d7123a500e 98 /** Permet de mettre à jour les valeurs de l'odometrie
IceTeam 8:12d7123a500e 99 */
IceTeam 8:12d7123a500e 100 void update_odo(void);
IceTeam 8:12d7123a500e 101
IceTeam 8:12d7123a500e 102 private:
IceTeam 8:12d7123a500e 103 RoboClaw &roboclaw;
IceTeam 8:12d7123a500e 104 int32_t m_pulses_left;
IceTeam 8:12d7123a500e 105 int32_t m_pulses_right;
IceTeam 8:12d7123a500e 106
IceTeam 8:12d7123a500e 107 double x, y, theta;
IceTeam 8:12d7123a500e 108 double m_vitLeft, m_vitRight;
IceTeam 8:12d7123a500e 109 double m_distLeft, m_distRight;
IceTeam 8:12d7123a500e 110
IceTeam 8:12d7123a500e 111 double m_distPerTick_left, m_distPerTick_right, m_v;
IceTeam 8:12d7123a500e 112
IceTeam 8:12d7123a500e 113 double erreur_ang;
sype 0:ad9600df4a70 114 };
sype 0:ad9600df4a70 115
sype 0:ad9600df4a70 116 #endif