ARES / Mbed 2 deprecated Robot2016_2-0

Robot secondaire

Dependencies:   RoboClaw mbed StepperMotor

Fork of RoboClaw by Simon Emarre

Odometry/Odometry.h@48:03da1aead032, 2016-04-28 (annotated)

Committer:
sype
Date:
Thu Apr 28 08:11:25 2016 +0000
Revision:
48:03da1aead032
Parent:
46:5658af4e5149
Child:
51:1056dd73a748
Int?gration de "l'algo" carte boutons, maintenant le bouton bleu (utilisateur) est remplac? par le jumper pr?sent sur la carte (on le retire pour d?marrer) et le changement de camp du robot se fait par le bouton pr?sent sur cette carte

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