ARES / Mbed 2 deprecated TestMyPathFind

Test du path finding

Dependencies:   RoboClaw mbed

Fork of TestMyPathFind by Romain Ame

Odometry/Odometry.h@40:309f38d1e49c, 2016-04-05 (annotated)

Committer:
IceTeam
Date:
Tue Apr 05 15:02:12 2016 +0000
Revision:
40:309f38d1e49c
Parent:
38:da3a2c781672 
Petite sauvegarde de batard;

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"
IceTeam 31:8bcc3a0bfa8a 5 #include "../RoboClaw/RoboClaw.h"
sype 0:ad9600df4a70 6
sype 0:ad9600df4a70 7 #define PI 3.1415926535897932384626433832795
sype 10:ae3178aa94e9 8 #define C 1.0
sype 10:ae3178aa94e9 9
IceTeam 31:8bcc3a0bfa8a 10 /* Vitesse d'acceleration d'angle reduite de 8000->4000 */
IceTeam 36:2d7357a385bc 11 #define accel_angle 8000
IceTeam 40:309f38d1e49c 12 #define vitesse_angle 12000
IceTeam 36:2d7357a385bc 13 #define deccel_angle 8000
sype 10:ae3178aa94e9 14
IceTeam 40:309f38d1e49c 15 #define accel_dista 12000
IceTeam 40:309f38d1e49c 16 #define vitesse_dista 20000
IceTeam 40:309f38d1e49c 17 #define deccel_dista 12000
IceTeam 40:309f38d1e49c 18
IceTeam 40:309f38d1e49c 19 /* Si le robot parcourt un trop grand angle, il diminuer l'entraxe, sinon l'augmenter */
IceTeam 40:309f38d1e49c 20 #define ENTRAXE 243.8
IceTeam 40:309f38d1e49c 21
sype 0:ad9600df4a70 22
sype 0:ad9600df4a70 23 /*
sype 0:ad9600df4a70 24 * Author : Benjamin Bertelone, reworked by Simon Emarre
sype 0:ad9600df4a70 25 */
sype 0:ad9600df4a70 26
sype 38:da3a2c781672 27 extern Serial logger;
sype 10:ae3178aa94e9 28 extern DigitalOut led;
sype 2:abdf8c6823a1 29
IceTeam 9:e39b218ab20d 30 /** Permet la gestion de l'odometrie du robot **/
sype 0:ad9600df4a70 31 class Odometry
sype 0:ad9600df4a70 32 {
IceTeam 8:12d7123a500e 33 public:
sype 10:ae3178aa94e9 34 /** Constructeur standard
IceTeam 8:12d7123a500e 35 * @param diameter_right Definit le diametre de la roue droite
IceTeam 8:12d7123a500e 36 * @param diameter_left Definit le diametre de la roue gauche
IceTeam 8:12d7123a500e 37 * @param v Definit l'entraxe du robot
IceTeam 8:12d7123a500e 38 * @param rc Definit une reference vers l'objet permettant l'asserv des moteurs
sype 10:ae3178aa94e9 39 * @note Cet objet doit etre initialise en amont
IceTeam 8:12d7123a500e 40 */
sype 10:ae3178aa94e9 41 Odometry(double diameter_right, double diameter_left, double v, uint16_t quadrature, RoboClaw &rc);
IceTeam 8:12d7123a500e 42
IceTeam 40:309f38d1e49c 43 /** Demande au robot d'effectuer un certain nombre de tour sur lui même */
IceTeam 35:4e3d9ab1b94b 44 void TestEntraxe(int i);
IceTeam 35:4e3d9ab1b94b 45
IceTeam 40:309f38d1e49c 46 /** Demande au robot d'effectuer un déplacement sur l'avant. Voir si l'on peut enlever la correction PID */
IceTeam 40:309f38d1e49c 47 void Forward(float i);
IceTeam 40:309f38d1e49c 48
IceTeam 8:12d7123a500e 49 /** Les fonctions suivantes permettent de reinitialiser les valeurs de position de l'odometrie.
IceTeam 8:12d7123a500e 50 */
IceTeam 8:12d7123a500e 51 void setPos(double x, double y, double theta);
IceTeam 8:12d7123a500e 52 void setX(double x);
IceTeam 8:12d7123a500e 53 void setY(double y);
IceTeam 8:12d7123a500e 54 void setTheta(double theta);
IceTeam 8:12d7123a500e 55
IceTeam 8:12d7123a500e 56 /** Les fonctions suivantes permettent de deplacer le robot vers une position ou un angle voulu
IceTeam 8:12d7123a500e 57 */
sype 14:d5e21f71c2a9 58 void GotoXY(double x_goal, double y_goal);
IceTeam 8:12d7123a500e 59 void GotoXYT(double x_goal, double y_goal, double theta_goal);
IceTeam 8:12d7123a500e 60 void GotoThet(double theta_);
IceTeam 8:12d7123a500e 61 void GotoDist(double distance);
IceTeam 8:12d7123a500e 62
IceTeam 8:12d7123a500e 63 double getX() {
IceTeam 8:12d7123a500e 64 return x;
IceTeam 8:12d7123a500e 65 }
IceTeam 8:12d7123a500e 66 double getY() {
IceTeam 8:12d7123a500e 67 return y;
IceTeam 8:12d7123a500e 68 }
IceTeam 8:12d7123a500e 69 double getTheta() {
IceTeam 8:12d7123a500e 70 return theta; // ]-PI;PI]
IceTeam 8:12d7123a500e 71 }
IceTeam 8:12d7123a500e 72 double getTheta_(double x, double y);
IceTeam 8:12d7123a500e 73
IceTeam 8:12d7123a500e 74 double abs_d(double x) {
IceTeam 8:12d7123a500e 75 if(x<0) return -x;
IceTeam 8:12d7123a500e 76 else return x;
IceTeam 8:12d7123a500e 77 }
IceTeam 7:961c1acdf753 78
sype 10:ae3178aa94e9 79 /* Les fonction suivantes sont actuellement inutilisables, elles pourraient buguer en cas d'utilisation. */
sype 10:ae3178aa94e9 80
IceTeam 8:12d7123a500e 81 double getVitLeft() {
IceTeam 8:12d7123a500e 82 return m_vitLeft;
IceTeam 8:12d7123a500e 83 }
IceTeam 8:12d7123a500e 84 double getVitRight() {
IceTeam 8:12d7123a500e 85 return m_vitRight;
IceTeam 8:12d7123a500e 86 }
IceTeam 8:12d7123a500e 87
IceTeam 8:12d7123a500e 88 double getDistLeft() {
IceTeam 8:12d7123a500e 89 return m_distLeft;
IceTeam 8:12d7123a500e 90 }
IceTeam 8:12d7123a500e 91 double getDistRight() {
IceTeam 8:12d7123a500e 92 return m_distRight;
IceTeam 8:12d7123a500e 93 }
IceTeam 8:12d7123a500e 94
IceTeam 8:12d7123a500e 95 void setDistLeft(double dist) {
IceTeam 8:12d7123a500e 96 m_distLeft = dist;
IceTeam 8:12d7123a500e 97 }
IceTeam 8:12d7123a500e 98 void setDistRight(double dist) {
IceTeam 8:12d7123a500e 99 m_distRight = dist;
IceTeam 8:12d7123a500e 100 }
IceTeam 8:12d7123a500e 101
IceTeam 8:12d7123a500e 102 double calcul_distance(double x, double y, double theta_goal);
IceTeam 8:12d7123a500e 103
IceTeam 8:12d7123a500e 104 int32_t getPulsesLeft(void) {
IceTeam 8:12d7123a500e 105 return m_pulses_left;
IceTeam 8:12d7123a500e 106 }
IceTeam 8:12d7123a500e 107 int32_t getPulsesRight(void) {
IceTeam 8:12d7123a500e 108 return m_pulses_right;
IceTeam 8:12d7123a500e 109 }
IceTeam 8:12d7123a500e 110 double carre(double a) {
IceTeam 8:12d7123a500e 111 return a*a;
IceTeam 8:12d7123a500e 112 }
sype 14:d5e21f71c2a9 113
sype 14:d5e21f71c2a9 114 void getEnc();
sype 14:d5e21f71c2a9 115
IceTeam 8:12d7123a500e 116 /** La fonction retourne vraie quand le robot atteint l'angle voulu avec une marge d'erreur definie par la fonction
IceTeam 8:12d7123a500e 117 * @param theta_ valeur de l'angle devant etre atteint
IceTeam 8:12d7123a500e 118 */
sype 10:ae3178aa94e9 119 bool isArrived(void) {return arrived;}
IceTeam 8:12d7123a500e 120 /** Permet de mettre à jour les valeurs de l'odometrie
IceTeam 8:12d7123a500e 121 */
IceTeam 8:12d7123a500e 122 void update_odo(void);
IceTeam 8:12d7123a500e 123
IceTeam 8:12d7123a500e 124 private:
IceTeam 8:12d7123a500e 125 RoboClaw &roboclaw;
IceTeam 8:12d7123a500e 126 int32_t m_pulses_left;
IceTeam 8:12d7123a500e 127 int32_t m_pulses_right;
sype 10:ae3178aa94e9 128 uint8_t pos_prog;
IceTeam 8:12d7123a500e 129 double x, y, theta;
IceTeam 8:12d7123a500e 130 double m_vitLeft, m_vitRight;
IceTeam 8:12d7123a500e 131 double m_distLeft, m_distRight;
IceTeam 8:12d7123a500e 132
IceTeam 8:12d7123a500e 133 double m_distPerTick_left, m_distPerTick_right, m_v;
IceTeam 8:12d7123a500e 134
IceTeam 8:12d7123a500e 135 double erreur_ang;
sype 10:ae3178aa94e9 136 bool arrived;
sype 0:ad9600df4a70 137 };
sype 0:ad9600df4a70 138
sype 0:ad9600df4a70 139 #endif