Romain Ame / Mbed 2 deprecated Timer71pt

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   RoboClaw mbed

Fork of Timer by ARES

Odometry/Odometry.h@78:c28bdbf29b6e, 2016-05-05 (annotated)

Committer:
IceTeam
Date:
Thu May 05 13:17:00 2016 +0000
Revision:
78:c28bdbf29b6e
Parent:
77:f19cc7f81f2a 
Des trucs qui compilent

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