Revision 0:1cfd66c3a181, committed 2019-05-22

Comitter:

xav_jann1

Date:

Wed May 22 16:54:27 2019 +0000

Commit message:

Premiere version

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actionneurs/Moteur.cpp	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,57 @@
+#include "Moteur.h"
+
+Moteur::Moteur(PinName pin_en, PinName pin_in1, PinName pin_in2):
+  m_pwm(pin_en), m_in1(pin_in1, 0), m_in2(pin_in2, 0) {
+
+    m_pwm.period_us(100.0f);  // Frequency = 10 kHz
+    
+    // PWM maximale qui peut être envoyée avec la commande turn():
+    m_pwm_max = 0.5;
+    
+    m_offset = 0.0f;
+}
+
+// Setter:
+void Moteur::setPWM(float pwm) { m_pwm = pwm + m_offset; }
+void Moteur::setPWM_max(float pwm) { m_pwm_max = pwm + m_offset; }
+void Moteur::setOffset(float offset) { m_offset = offset; }
+
+/*** Déplacement ***/
+
+// Avant:
+void Moteur::forward() {
+    m_in1 = 1;
+    m_in2 = 0;
+}
+
+// Arrière:
+void Moteur::backward() {
+    m_in1 = 0;
+    m_in2 = 1;
+}
+
+// Opposé:
+void Moteur::opposite() {
+    m_in1 = !m_in1.read();
+    m_in2 = !m_in2.read();
+}
+
+// Arrêt:
+void Moteur::stop() {
+    m_pwm.write(0.0f);  // Duty cycle
+}
+
+// Avance ou recule en fonction du pwm:
+void Moteur::turn(float pwm) {
+    if (pwm < 0) {
+        pwm -= m_offset;
+        if (pwm < -m_pwm_max) pwm = -m_pwm_max;
+        setPWM(-pwm);
+        backward();
+    } else {
+        pwm += m_offset;
+        if (pwm > m_pwm_max) pwm = m_pwm_max;
+        setPWM(pwm);
+        forward();
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actionneurs/Moteur.h	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,33 @@
+#ifndef MOTEUR_H
+#define MOTEUR_H
+
+#include "mbed.h"
+
+class Moteur {
+ public:
+  // Constructeur:
+  Moteur(PinName pin_en, PinName pin_in1, PinName pin_in2);
+  
+  // Déplacement:
+  void forward();
+  void backward();
+  void opposite();
+  void stop();
+  void turn(float pwm);
+  
+  // Setter:
+  void setPWM(float pwm); // entre 0 et 1
+  void setPWM_max(float pwm); // entre 0 et 1
+  void setOffset(float offset); // entre 0 et 1
+  
+ private:
+  PwmOut m_pwm;
+  DigitalOut m_in1;
+  DigitalOut m_in2;
+  
+  float m_pwm_max;
+  float m_offset;
+  
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actionneurs/Servo.cpp	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,33 @@
+#include "Servo.h"
+#include "mbed.h"
+
+// Constructeur:
+Servo::Servo(PinName pin) : m_pin(pin) {}
+
+// Active le servomoteur:
+void Servo::enable(int startPos, float period) {
+    m_position = startPos;
+    m_pulse.attach_us(callback(this, &Servo::startPulse), period);
+}
+
+// Désactive le servomoteur:
+void Servo::disable() {
+    m_pulse.detach();
+}
+
+// Déplace le servomoteur à la position angle (en °):
+void Servo::pos(int angle) {
+    // Conversion degrée en durée d'impulsion:
+    m_position = -11.111f * angle + 2500;
+}
+
+// Début d'une impulsion:
+void Servo::startPulse() {
+    m_pin = 1;
+    m_pulseStop.attach_us(callback(this, &Servo::endPulse), m_position);
+}
+
+// Fin de l'impulsion:
+void Servo::endPulse() {
+    m_pin = 0;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Actionneurs/Servo.h	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,28 @@
+#ifndef SERVO_H
+#define SERVO_H
+
+#include "mbed.h"
+
+class Servo {
+
+public:
+    // Constructeur:
+    Servo(PinName Pin);
+    
+    // Déplacement:
+    void pos(int pos);
+    void enable(int startPos, float period);
+    void disable();
+
+private:
+    // Gestion de l'impulsion:
+    void startPulse();
+    void endPulse();
+
+    int m_position;
+    DigitalOut m_pin;
+    Ticker m_pulse;
+    Timeout m_pulseStop;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Base/Asservissement.cpp	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,57 @@
+#include "Asservissement.h"
+
+// Constructeur:
+Asservissement::Asservissement(float P, float I, float D) {
+  // PID:
+  m_P = P;
+  m_I = I;
+  m_D = D;
+  
+  m_erreur_integral = 0;
+  
+  printf("Asservissement:\r\n");
+  printf("  - P: %p\r\n", &m_P);
+  printf("  - I: %p\r\n", &m_I);
+  printf("  - D: %p\r\n", &m_D);
+}
+
+// Setters:
+void Asservissement::setP(float P) { m_P = P; }
+void Asservissement::setI(float I) { m_I = I; }
+void Asservissement::setD(float D) { m_D = D; }
+void Asservissement::setPID(float P, float I, float D) {
+  m_P = P;
+  m_I = I;
+  m_D = D;
+}
+
+// Pointeurs:
+float* Asservissement::getP_ptr() { return &m_P; }
+float* Asservissement::getI_ptr() { return &m_I; }
+float* Asservissement::getD_ptr() { return &m_D; }
+
+// Asservissement:
+void Asservissement::toValue(float value) {
+  m_desired_value = value;
+  m_erreur_integral = 0;
+}
+
+// Calcul de l'erreur:
+float Asservissement::computeError(float value) {
+  // Erreur de valeur:
+  float erreur = m_desired_value - value;
+
+  // Dérivée:
+  float delta_erreur = m_erreur_prev - erreur;
+
+  // Intégrale:
+  m_erreur_integral += erreur;
+
+  // Erreur totale:
+  float total_erreur = m_P * erreur + m_I * m_erreur_integral + m_D * delta_erreur;
+
+  // Enregistrement:
+  m_erreur_prev = erreur;
+
+  return total_erreur;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Base/Asservissement.h	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,39 @@
+#ifndef ASSERVISSEMENT_H
+#define ASSERVISSEMENT_H
+
+#include "mbed.h"
+
+class Asservissement {
+ public:
+  // Constructeur:
+  Asservissement(float P, float I, float D);
+  
+  // Setters:
+  void setP(float P);
+  void setI(float I);
+  void setD(float D);
+  void setPID(float P, float I, float D);
+  
+  // Pointeurs:
+  float* getP_ptr();
+  float* getI_ptr();
+  float* getD_ptr();
+  
+  // Asservissement:
+  void toValue(float value);
+  
+  // Calcul d'asservissement:
+  float computeError(float value);
+
+ private:  
+  // PID:
+  float m_P, m_I, m_D;
+  
+  // Value:
+  float m_desired_value;
+  
+  float m_erreur_prev, m_erreur_integral;
+  
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Base/Base.cpp	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,142 @@
+#include "Base.h"
+
+// Constructeur:
+Base::Base(Moteur* moteurG, Moteur* moteurD, Odometrie* odometrie, int update_delay_us): 
+    m_asservissement_distance(1, 1, 1), m_asservissement_orientation(1, 1, 1) { 
+    
+    m_moteurG = moteurG;
+    m_moteurD = moteurD;
+    m_moteurs_isEnabled = false;
+
+    // Odométrie:
+    m_odometrie = odometrie;
+    
+    // Asservissement:
+    m_pos_distance = 0;
+    m_pos_orientation = 0;
+    m_moteurG->setPWM_max(0.4);
+    m_moteurD->setPWM_max(0.4);
+    m_moteurG->setOffset(0.1);
+    m_moteurD->setOffset(0.1);
+    m_pwm_dynamic = 0.2f;  // max = 0.6 - 0.1
+    
+    // Erreur maximale:
+    m_max_erreur_distance = 300;
+    m_max_erreur_orientation = 3;
+  
+    // Répartition PWM entre distance et orientation:
+    m_distance_orientation_ratio = 0.5f; // 50% -> distance, 50% -> orientation 
+
+    // Mesure du déplacement toutes les <update_delay_us> µs:
+    m_ticker.attach_us(callback(this, &Base::update), update_delay_us); // 2 kHz pour 500 us
+}
+
+// Setters:
+void Base::setDistanceTo(float d) { m_asservissement_distance.toValue(d); }
+void Base::setOrientationTo(float o) { m_asservissement_orientation.toValue(M_PI * o / 180.0f ); }
+
+void  Base::setPID_distance(float P, float I, float D) {
+    m_asservissement_distance.setPID(P, I, D);
+}
+void  Base::setPID_orientation(float P, float I, float D) {
+    m_asservissement_orientation.setPID(P, I, D);
+}
+
+// Getters:
+float* Base::getErreur_distance_ptr() { return &m_erreur_distance; }
+float* Base::getErreur_orientation_ptr() { return &m_erreur_orientation; }
+float* Base::getPID_PWM_distance_ptr() { return &m_pwm_distance; }
+float* Base::getPID_PWM_orientation_ptr() { return &m_pwm_orientation; }
+float* Base::getPID_PWM_gauche_ptr() { return &m_pwm_gauche; }
+float* Base::getPID_PWM_droite_ptr() { return &m_pwm_droite; }
+
+/*** Déplacement ***/
+
+// Start:
+void Base::start() {
+    m_moteurs_isEnabled = true;
+}
+
+void Base::stop() {
+  m_moteurs_isEnabled = false;
+  
+  // Arrêt des moteurs:
+  m_moteurG->stop();
+  m_moteurD->stop();
+}
+
+
+// Stop:
+
+// Avant: (en mm)
+void Base::forward(float d) {
+    m_pos_distance += d;
+    setDistanceTo(m_pos_distance);
+}
+
+// Arrière: (en mm)
+void Base::backward(float d) {
+    m_pos_distance -= d;
+    setDistanceTo(m_pos_distance);
+}
+
+// Tourner: (en °)
+void Base::turn(float o) {
+    m_pos_orientation += o;
+    setOrientationTo(m_pos_orientation);
+}
+
+// Renvoie le signe de la valeur:
+template <typename T> int sign(T val) {
+    return (T(0) < val) - (val < T(0));
+}
+
+
+// Mise à jour de l'odometrie et de l'asservissement:
+void Base::update() {
+    // Mise à jour de la position:
+    m_odometrie->update();
+    
+    // Calcul de l'erreur de distance:
+    float distance = m_odometrie->getRealDistance();
+    float erreur_distance = m_asservissement_distance.computeError(distance);
+    
+    // Calcul de l'erreur d'orientation:
+    float theta = m_odometrie->getTheta();
+    float erreur_theta = m_asservissement_orientation.computeError(theta);
+    
+    // Correction de l'erreur:
+    if(abs(erreur_distance) > m_max_erreur_distance) erreur_distance = m_max_erreur_distance * sign(erreur_distance);    
+    if(abs(erreur_theta) > m_max_erreur_orientation) erreur_theta = m_max_erreur_orientation * sign(erreur_theta);
+    
+    // PWM correspondant à la distance et à l'orientation:
+    float pwm_distance = (erreur_distance / m_max_erreur_distance) * m_distance_orientation_ratio;
+    float pwm_theta = (erreur_theta / m_max_erreur_orientation) * (1.0f - m_distance_orientation_ratio);
+    
+    // PWM à envoyer aux moteurs:
+    float pwm_droite = (pwm_distance + pwm_theta) * m_pwm_dynamic;
+    float pwm_gauche = (pwm_distance - pwm_theta) * m_pwm_dynamic;
+    
+    // Commande des moteurs:
+    if (m_moteurs_isEnabled == true) {       
+        m_moteurD->turn(pwm_droite);
+        m_moteurG->turn(pwm_gauche);
+    }
+    
+    // Sauvegarde / Debug:
+    m_pwm_distance = pwm_distance;
+    m_pwm_orientation = pwm_theta;
+    m_erreur_distance = erreur_distance;
+    m_erreur_orientation = erreur_theta;
+    m_pwm_droite = pwm_droite;
+    m_pwm_gauche = pwm_gauche;
+}
+
+// Getters:
+float Base::getErreurDistance() { return m_erreur_distance; }
+float Base::getErreurOrientation() { return m_erreur_orientation; }
+
+//float Base::moveTo(float X, float Y);
+//float Base::setAngle();
+
+//float Base::turn(float angle);
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Base/Base.h	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,83 @@
+#ifndef BASE_H
+#define BASE_H
+
+#include "mbed.h"
+#include "Moteur.h"
+#include "Odometrie.h"
+#include "Asservissement.h"
+
+#ifndef M_PI
+#define M_PI   3.14159265358979323846f
+#endif
+
+class Base {
+ public:
+  // Constructeur:
+  Base(Moteur*, Moteur*, Odometrie*, int);
+  
+  // Déplacement:
+  //float moveTo(float X, float Y);
+  //float setAngle();
+  //float forward(float d);
+  void start();
+  void stop();
+  void forward(float);
+  void backward(float);
+  void turn(float); // °
+  
+  //float turn(float angle);
+  
+  // PID:
+  void setPID_distance(float P, float I, float D);
+  void setPID_orientation(float P, float I, float D);
+  
+  // Pointeurs:
+  float* getErreur_distance_ptr();
+  float* getErreur_orientation_ptr();
+  float* getPID_PWM_distance_ptr();
+  float* getPID_PWM_orientation_ptr();
+  float* getPID_PWM_gauche_ptr();
+  float* getPID_PWM_droite_ptr();
+  
+  // Getters:
+  float getErreurDistance();
+  float getErreurOrientation();
+
+  
+ private:
+  // Moteurs:
+  Moteur* m_moteurG;
+  Moteur* m_moteurD;
+  bool m_moteurs_isEnabled;
+  
+  // Odometrie:
+  Odometrie* m_odometrie;
+ 
+  // Asservissement distance et orientation:
+  Asservissement m_asservissement_distance;
+  Asservissement m_asservissement_orientation;
+  float m_pos_distance;
+  float m_pos_orientation;
+  void setDistanceTo(float);
+  void setOrientationTo(float o);
+  //Asservissement m_asservissement_vitesse;
+  float m_max_erreur_distance, m_max_erreur_orientation; 
+  float m_distance_orientation_ratio;
+  
+  // PWM maximale envoyée au moteurs:
+  float m_pwm_max;
+  float m_pwm_dynamic;
+  
+  // Debug:
+  float m_erreur_distance, m_erreur_orientation;
+  float m_pwm_gauche, m_pwm_droite;
+  float m_pwm_distance, m_pwm_orientation;
+  
+  // Mise à jour de la position:
+  void update();
+
+  // Ticker:
+  Ticker m_ticker;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Base/Odometrie.cpp	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,78 @@
+#include "Odometrie.h"
+
+Odometrie::Odometrie(Encodeur* encodeurG, Encodeur* encodeurD, float entraxe) {
+    m_encodeurG = encodeurG;
+    m_encodeurD = encodeurD;
+    
+    m_L = 0.0f;
+    m_X = 0.0f;
+    m_Y = 0.0f;
+    m_Theta = 0; //3.14159265359; // PI
+    m_vitesse = 0.0f;
+    m_distance = 0.0f;
+    
+    m_prev_encodeurG_count = m_encodeurG->getTotalCount();
+    m_prev_encodeurD_count = m_encodeurD->getTotalCount();
+    
+    // Nombre d'impulsions par mm parcourus:
+    m_ticks_par_mm = m_encodeurD->getTicks_par_mm();
+    
+    // Entraxe des 2 encodeurs en ticks:
+    m_entraxe_ticks = entraxe * m_ticks_par_mm;
+    
+    printf("Ticks par mm : %f\n\r", m_ticks_par_mm);
+    printf("Entraxe en ticks : %f\n\r", m_entraxe_ticks);
+}
+
+// Getters:
+float Odometrie::getX() { return m_X; }
+float Odometrie::getY() { return m_Y; }
+float Odometrie::getTheta() { return m_Theta; }
+float Odometrie::getDistance() { return m_distance; }
+float Odometrie::getRealDistance() { return m_distance / m_ticks_par_mm; }
+float Odometrie::getVitesse() { return m_vitesse; }
+
+// Pointeurs:
+float* Odometrie::getX_ptr() { return &m_X; }
+float* Odometrie::getY_ptr() { return &m_Y; }
+float* Odometrie::getTheta_ptr() { return &m_Theta; }
+float* Odometrie::getDistance_ptr() { return &m_distance; }
+float* Odometrie::getVitesse_ptr() { return &m_vitesse; }
+
+// Selon le ClubElek:
+void Odometrie::update() {
+    
+    // Récupère les compteurs des encodeurs:
+    int encodeurG_count = m_encodeurG->getTotalCount();
+    int encodeurD_count = m_encodeurD->getTotalCount();
+    
+    // Différences de ticks depuis le dernier update():
+    int dEncodeurG = encodeurG_count - m_prev_encodeurG_count;
+    int dEncodeurD = encodeurD_count - m_prev_encodeurD_count;
+    
+    // Différences de distance et d'angle:
+    float dL = (dEncodeurD + dEncodeurG) / 2.0f;
+    float dTheta = (dEncodeurD - dEncodeurG) / 2.0f;
+    
+    // Nouvel Angle:
+    m_Theta += dTheta / m_entraxe_ticks;
+    
+    // Nouvelles Positions X et Y:
+    float dX = dL * cos(m_Theta);
+    float dY = dL * sin(m_Theta);
+    m_X += dX / m_ticks_par_mm;
+    m_Y += dY / m_ticks_par_mm; 
+    
+    // Vitesse:
+    m_vitesse = dL;
+    
+    // Distance :
+    m_distance += dL;
+    
+    // Sauvegarde:
+    m_prev_encodeurG_count = encodeurG_count;
+    m_prev_encodeurD_count = encodeurD_count;
+}
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Base/Odometrie.h	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,42 @@
+#ifndef ODOMETRIE_H
+#define ODOMETRIE_H
+
+#include "mbed.h"
+#include "Encodeur.h"
+
+class Odometrie {
+ public:
+  // Constructeur:
+  Odometrie(Encodeur* encodeurG, Encodeur* encodeurD, float entraxe);
+  
+  void update();
+  
+  // Getters:
+  float getX();
+  float getY();
+  float getTheta();
+  float getDistance();
+  float getRealDistance();
+  float getVitesse();
+  
+  // Pointeurs:
+  float* getX_ptr();
+  float* getY_ptr();
+  float* getTheta_ptr();
+  float* getDistance_ptr();
+  float* getVitesse_ptr();
+
+ private:
+  // Position et orientation:
+  float  m_L, m_X, m_Y, m_Theta, m_distance, m_vitesse;
+  
+  // Encodeurs:
+  Encodeur* m_encodeurG;
+  Encodeur* m_encodeurD;
+  float m_ticks_par_mm, m_entraxe_ticks;
+  
+  int m_prev_encodeurG_count, m_prev_encodeurD_count;
+  
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Capteurs/Encodeur.cpp	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,121 @@
+#include "Encodeur.h"
+
+// Timers & Encoders:
+TIM_HandleTypeDef timer3, timer4;
+TIM_Encoder_InitTypeDef encoder3, encoder4;
+
+// Constructeur:
+Encodeur::Encodeur(char e, int resolution, int diametre) {
+  TIM_Encoder_InitTypeDef* encodeur;
+  TIM_TypeDef* TIMx;
+  
+  if (e == 'G') {
+    encodeur = &encoder3;
+    TIMx = TIM3;
+    m_timer = &timer3;
+  } else if (e == 'D') {
+    encodeur = &encoder4;
+    TIMx = TIM4;
+    m_timer = &timer4;
+  }
+  m_resolution = resolution * 4 - 1; // *4 car l'encodeur détecte tous les fronts des 2 signaux
+  m_diametre = diametre;
+  m_ticks_par_mm = (float)m_resolution / (float)(M_PI * m_diametre);
+  
+  m_tours = -1;  // -1 -> 0 au démarrage (car une interruption est déclenchée)
+  
+  EncodeurInit(encodeur, m_timer, TIMx, m_resolution);
+}
+
+// Getters & Setters:
+uint16_t Encodeur::getCount() { return __HAL_TIM_GET_COUNTER(m_timer); }
+int Encodeur::getTours() { return m_tours; }
+int* Encodeur::getTours_ptr() { return &m_tours; }
+void Encodeur::updateTour(int dtour) { m_tours += dtour; }
+
+int Encodeur::getDist() { return M_PI * m_diametre * (m_tours + (float)getCount() / (float)m_resolution); }
+int Encodeur::getTotalCount() {
+    int encodeur_tour = m_tours;
+    
+    // Détecte si le compteur s'est réinitialisé sans ajouter le tour :
+    // (c'est-à-dire TIMx_IRQHandler ne s'est pas encore déclenché)
+    if (__HAL_TIM_GET_FLAG(m_timer, TIM_FLAG_UPDATE) != RESET) {
+        // Récupère la direction de rotation de l'encodeur:
+        bool direction = __HAL_TIM_IS_TIM_COUNTING_DOWN(m_timer);
+
+        // Ajoute ou enlève un tour:
+        encodeur_tour += direction ? -1 : 1;
+    }
+
+    return encodeur_tour * m_resolution + getCount();
+}
+
+int Encodeur::getDiametre() { return m_diametre; }
+int Encodeur::getResolution() { return m_resolution; }
+float Encodeur::getTicks_par_mm() { return m_ticks_par_mm; }
+
+// Initialisation du Timer pour l'Encodeur:
+void EncodeurInit(TIM_Encoder_InitTypeDef* encodeur, TIM_HandleTypeDef* timer, TIM_TypeDef* TIMx, uint32_t resolution) {
+    // Timer:
+    timer->Instance = TIMx;
+    timer->Init.Prescaler = 0;
+    timer->Init.Period = resolution;
+    timer->Init.CounterMode = TIM_COUNTERMODE_UP;
+    timer->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+    
+    // Encodeur:
+    encodeur->EncoderMode = TIM_ENCODERMODE_TI12;
+    
+    // Channel 1:
+    encodeur->IC1Filter = 0;
+    encodeur->IC1Polarity = TIM_INPUTCHANNELPOLARITY_FALLING;
+    encodeur->IC1Prescaler = TIM_ICPSC_DIV1;
+    encodeur->IC1Selection = TIM_ICSELECTION_DIRECTTI;
+    
+    // Channel 2:
+    encodeur->IC2Filter = 0;
+    encodeur->IC2Polarity = TIM_INPUTCHANNELPOLARITY_FALLING;
+    encodeur->IC2Prescaler = TIM_ICPSC_DIV1;
+    encodeur->IC2Selection = TIM_ICSELECTION_DIRECTTI;
+
+    if (HAL_TIM_Encoder_Init(timer, encodeur) != HAL_OK) {
+        printf("Couldn't Init Encoder\r\n");
+        while (1) {}
+    }
+
+    if (HAL_TIM_Encoder_Start(timer, TIM_CHANNEL_1)!= HAL_OK) {
+        printf("Couldn't Start Encoder\r\n");
+        while (1) {}
+    }
+    
+    // Active l'interruption du timer:
+    HAL_TIM_Base_Start_IT(timer);
+}
+
+
+// Fonction d'interruptions pour compter le nombre de tours:
+extern Encodeur encodeurG, encodeurD;
+extern "C"
+{
+    // Gestion de l'interruption du timer 3:
+    void TIM3_IRQHandler(void) {
+        HAL_TIM_IRQHandler(&timer3);
+
+        // Récupère la direction de rotation de l'encodeur:
+        bool direction = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer3);
+
+        // Ajoute ou enlève un tour:
+        encodeurG.updateTour(direction ? -1 : 1);
+    }
+    
+    // Gestion de l'interruption du timer 4:
+    void TIM4_IRQHandler(void) {
+        HAL_TIM_IRQHandler(&timer4);
+
+        // Récupère la direction de rotation de l'encodeur:
+        bool direction = __HAL_TIM_IS_TIM_COUNTING_DOWN(&timer4);
+
+        // Ajoute ou enlève un tour:
+        encodeurD.updateTour(direction ? -1 : 1);
+    }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Capteurs/Encodeur.h	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,39 @@
+#ifndef ENCODEUR_H
+#define ENCODEUR_H
+
+#include "mbed.h"
+
+#ifndef M_PI
+//#define M_PI   3.14159265358979323846
+#define M_PI   3.14159265358979323846f
+#endif
+
+// Initialisation du Timer pour l'Encodeur:
+void EncodeurInit(TIM_Encoder_InitTypeDef* encodeur, TIM_HandleTypeDef* timer, TIM_TypeDef* TIMx, uint32_t resolution);
+
+class Encodeur {
+ public:
+  // Constructeur:
+  Encodeur(char e, int resolution, int diametre);
+  
+  // Getters & Setter:
+  uint16_t getCount();
+  int getTotalCount();
+  int getTours();
+  int* getTours_ptr();
+  void updateTour(int);
+  int getDist();     // en mm
+  
+  int getDiametre(); // en mm
+  int getResolution(); 
+  float getTicks_par_mm();
+  
+ private:
+  int m_tours;
+  int m_resolution;
+  int m_diametre;  // en mm
+  float m_ticks_par_mm;
+  TIM_HandleTypeDef* m_timer;
+};
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Capteurs/Infra.cpp	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,33 @@
+#include "Infra.h"
+#include <math.h>
+
+// Constructeur:
+Infra::Infra(PinName pin)
+    : m_analog(pin),
+      m_isOn(true),
+      m_distdetect(30.0f),
+      m_detect(false) {}
+
+// Mesure:
+bool Infra::mesure() {
+  // Mesure:
+  m_voltage = m_analog.read();
+  m_voltage = (float)m_voltage * (float)3.3;
+  m_distance = (float)29.988 * powf(m_voltage, -1.173) / 2.0f;
+  
+  // Détection:
+  if (m_isOn) m_detect = m_distance < m_distdetect;
+  else m_detect = false;
+  
+  return m_detect;
+}
+
+// Setters:
+void Infra::setDistDetect(float detect) { m_distdetect = detect; }
+void Infra::on() { m_isOn = true; }
+void Infra::off() { m_isOn = false; }
+
+// Getters:
+float Infra::getDistance() { return m_distance; }
+float Infra::getVoltage() { return m_voltage; }
+float Infra::getDistdetect() { return m_distdetect; }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Capteurs/Infra.h	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,33 @@
+#ifndef INFRA_H
+#define INFRA_H
+
+#include "mbed.h"
+#include <vector>
+
+class Infra {
+ public:
+  // Constructeur:
+  Infra(PinName);
+
+  // Mesure:
+  bool mesure();
+  
+  // Setters:
+  void setDistDetect(float);
+  void on();
+  void off();
+
+  // Getters:
+  float getDistance();
+  float getDistdetect();
+  float getVoltage();
+
+ private:
+  AnalogIn m_analog;
+  bool m_isOn;
+  float m_voltage;
+  float m_distance;
+  float m_distdetect;
+  bool m_detect;
+};
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Capteurs/Nucleo/EncoderMspInitF4.cpp	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,57 @@
+#include "mbed.h"
+/*
+ * HAL_TIM_Encoder_MspInit()
+ * Overrides the __weak function stub in stm32f4xx_hal_tim.h
+ *
+ * Edit the below for your preferred pin wiring & pullup/down
+ * I have encoder common at 3V3, using GPIO_PULLDOWN on inputs.
+ * Encoder A&B outputs connected directly to GPIOs.
+ *
+ * www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/DM00102166.pdf
+ * www.st.com/st-web-ui/static/active/en/resource/technical/document/datasheet/DM00141306.pdf
+ *
+ * TIM3_CH1: AF2 @ PA_6, PB_4, PC_6
+ * TIM3_CH2: AF2 @ PA_7, PB_5, PC_7
+ *
+ * TIM4_CH1: AF2 @ PB_6, PD_12
+ * TIM4_CH2: AF2 @ PB_7, PD_13
+ *
+ */
+
+#ifdef TARGET_STM32F4
+void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef *htim)
+{
+    GPIO_InitTypeDef GPIO_InitStruct;
+    
+    // Timer 3:
+    if (htim->Instance == TIM3) { // PB4 PB5 = Nucleo D5 D4
+        __TIM3_CLK_ENABLE();
+        __GPIOB_CLK_ENABLE();
+        GPIO_InitStruct.Pin = GPIO_PIN_4 | GPIO_PIN_5;
+        GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+        GPIO_InitStruct.Pull = GPIO_PULLUP;
+        GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
+        GPIO_InitStruct.Alternate = GPIO_AF2_TIM3;
+        HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+        
+        // Interruption:
+        HAL_NVIC_SetPriority(TIM3_IRQn,0,0);
+        HAL_NVIC_EnableIRQ(TIM3_IRQn);
+    }
+    // Timer 4:
+    else if (htim->Instance == TIM4) { // PB6 PB7 = Nucleo D10 MORPHO_PB7
+        __TIM4_CLK_ENABLE();
+        __GPIOB_CLK_ENABLE();
+        GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7;
+        GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+        GPIO_InitStruct.Pull = GPIO_PULLUP;
+        GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
+        GPIO_InitStruct.Alternate = GPIO_AF2_TIM4;
+        HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+        // Interruption:
+        HAL_NVIC_SetPriority(TIM4_IRQn,0,0);
+        HAL_NVIC_EnableIRQ(TIM4_IRQn);
+    }
+}
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PURPLE_steps.cpp	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,114 @@
+// Eléments:
+#include "Base.h"
+#include "Servo.h"
+#include "Infra.h"
+
+extern Base base;
+extern Servo servo;
+extern Infra infraFrontG;
+extern Infra infraFrontD;
+extern Infra infraBack;
+extern DigitalOut pompe;
+
+// Déplacement pour côté Jaune:
+int PURPLE_steps(int state) {
+    int timeOut = 3000;
+    
+    // 0: Avant:
+    if (state == 0) {
+        base.forward(-1600);
+        timeOut = 6000;
+    } 
+    
+    // 1: Tourne:
+    else if (state == 1) {
+         base.turn(-90.0f);
+         infraFrontG.off();
+         infraFrontD.off();
+         infraBack.on();
+    }
+    
+    // 2: Avance vers palet:
+    else if (state == 2) {
+        servo.pos(75);
+        base.forward(-450);
+        timeOut = 3000;
+    }
+    
+    // 3: Servo:
+    else if (state == 3) {
+        //base.stop();
+        servo.pos(35);
+        timeOut = 3000;   
+    }
+    
+    // 4: Recule du palet:
+    else if (state == 4) {
+        base.start();
+        base.backward(-400);
+        timeOut = 3000;
+    }
+    
+    // 5: Tourne
+    else if (state == 5) {
+        //base.start();
+        base.turn(-90.0f);
+    }
+    
+    // 6: Avance
+    else if (state == 6) {
+        base.backward(-550);
+        infraFrontG.on();
+        infraFrontD.on();
+        
+    }
+    
+    // 7: Tourne
+    else if (state == 7) {
+        base.turn(90.0f);
+        servo.pos(130.0f);
+        infraFrontG.off();
+        infraFrontD.off();
+    }
+    
+    // 8: Avance
+    else if (state == 8) base.forward(-400.0f);
+    
+    // 9: Pompe à air
+    else if (state == 9) {
+        pompe = 1;
+        timeOut = 2000;
+    }
+    
+    // 10: Recule du Goldenium
+    else if (state == 10) {
+        base.backward(-500.0f);
+        timeOut = 1000;
+    }
+    
+    // 11: Désactive la pompe
+    else if (state == 11) {
+        pompe = 0;
+        timeOut = 1000;    
+    }
+    
+    // 12: Tourne pour revenir
+    else if (state == 12) {
+        base.turn(-90.0f);
+    }
+    
+    // 13: Avance pour revenir
+    else if (state == 13) {
+        base.forward(-1700);
+        infraFrontG.on();
+        infraFrontD.on();
+        timeOut = 6000;
+    }
+    
+    // 14: Fin
+    else if (state == 14) {
+        base.stop();
+    }
+    
+    return timeOut;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/YELLOW_steps.cpp	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,113 @@
+// Eléments:
+#include "Base.h"
+#include "Servo.h"
+#include "Infra.h"
+
+extern Base base;
+extern Servo servo;
+extern Infra infraFrontG;
+extern Infra infraFrontD;
+extern Infra infraBack;
+extern DigitalOut pompe;
+
+// Déplacement pour côté Jaune:
+int YELLOW_steps(int state) {
+    int timeOut = 3000;
+    
+    // 0: Avant:
+    if (state == 0) {
+        base.forward(-1700);
+        timeOut = 6000;
+    } 
+    
+    // 1: Tourne:
+    else if (state == 1) {
+        base.turn(90.0f);
+        infraFrontG.off();
+        infraFrontD.off();
+        infraBack.on();
+    }
+    
+    // 2: Avance vers palet:
+    else if (state == 2) {
+        servo.pos(10);
+        base.forward(-470);
+        timeOut = 3000;
+    }
+    
+    // 3: Servo:
+    else if (state == 3) {
+        //base.stop();
+        servo.pos(45);
+        timeOut = 3000;   
+    }
+    
+    // 4: Recule du palet:
+    else if (state == 4) {
+        base.start();
+        base.backward(-200);
+        timeOut = 3000;
+    }
+    
+    // 5: Tourne
+    else if (state == 5) {
+        base.start();
+        base.turn(90.0f);
+    }
+    
+    // 6: Avance
+    else if (state == 6) {
+        base.backward(-520);
+        infraFrontG.on();
+        infraFrontD.on();
+    }
+    
+    // 7: Tourne
+    else if (state == 7) {
+        base.turn(-90.0f);
+        servo.pos(130.0f);
+        infraFrontG.off();
+        infraFrontD.off();
+    }
+    
+    // 8: Avance
+    else if (state == 8) base.forward(-400.0f);
+    
+    // 9: Pompe à air
+    else if (state == 9) {
+        pompe = 1;
+        timeOut = 2000;
+    }
+    
+    // 10: Recule du Goldenium
+    else if (state == 10) {
+        base.backward(-500.0f);
+        timeOut = 1000;
+    }
+    
+    // 11: Désactive la pompe
+    else if (state == 11) {
+        pompe = 0;
+        timeOut = 1000;    
+    }
+    
+    // 12: Tourne pour revenir
+    else if (state == 12) {
+        base.turn(90.0f);
+    }
+    
+    // 13: Avance pour revenir
+    else if (state == 13) {
+        base.forward(-1700);
+        infraFrontG.on();
+        infraFrontD.on();
+        timeOut = 6000;
+    }
+    
+    // 14: Fin
+    else if (state == 14) {
+        base.stop();
+    }
+    
+    return timeOut;
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,295 @@
+#include "mbed.h"
+
+// Eléments:
+#include "Base.h"
+#include "Moteur.h"
+#include "Encodeur.h"
+#include "Odometrie.h"
+#include "Servo.h"
+#include "Infra.h"
+#include <math.h>
+
+/*** Paramètres ***/
+
+// Pin tirette:
+#define pin_tirette  A4
+
+// Pin interrupteur:
+#define pin_switch_color  A5
+
+// Pin Servo:
+#define pin_servo  A0
+
+// Pins des moteurs:
+#define pins_moteurG_enA_in1_in2  D8, D11, D12
+#define pins_moteurD_enB_in1_in2  D7, D14, D15
+
+// Pin pompe à air:
+#define pin_pompe  D6
+
+// Pins Infrarouge:
+#define pin_infra1  A1
+#define pin_infra2  A2
+#define pin_infra3  A3
+
+// Paramètres des encodeurs:
+#define encodeur_res       1024
+#define encodeur_diametre  40        // en mm
+#define encodeurs_entraxe  97/2.0f  // en mm
+
+// Paramètre de l'odometrie et asservissement:
+#define update_delay_us    500       // en µs
+
+/*** Création des objets ***/
+
+// Moteurs:
+Moteur moteurG(pins_moteurG_enA_in1_in2);
+Moteur moteurD(pins_moteurD_enB_in1_in2);
+
+// Encodeurs:
+Encodeur encodeurG('G', encodeur_res, encodeur_diametre);
+Encodeur encodeurD('D', encodeur_res, encodeur_diametre);
+
+// Odometrie:
+Odometrie odometrie(&encodeurG, &encodeurD, encodeurs_entraxe);
+
+// Base:
+Base base(&moteurG, &moteurD, &odometrie, update_delay_us);
+#define base_max_erreur_distance  10
+#define base_max_erreur_orientation  0.1f
+
+// Tirette:
+DigitalIn tirette(pin_tirette);
+
+// Interrupteur:
+DigitalIn switch_color(pin_switch_color);
+
+// Pompe:
+DigitalOut pompe(pin_pompe);
+
+// Servo:
+Servo servo(pin_servo);
+
+// Infrarouge:
+Infra infraFrontD(pin_infra1);
+Infra infraFrontG(pin_infra2);
+Infra infraBack(pin_infra3);
+int detection();
+bool isStopped = false;
+
+// Buzzer:
+void buzzer_on();
+void buzzer_off();
+void buzzer_note();
+
+// Fin de l'épreuve:
+#define epreuve_duration  100 // s
+Timeout epreuve_timeout;
+void finish() { base.stop(); }
+
+// Prototypes - Machines d'états:
+extern int PURPLE_steps(int state);
+extern int YELLOW_steps(int state);
+
+int main() {
+    
+  printf("STM32 Base\r\n");
+  printf("Adresses de variables: (pour STM-Studio)\r\n");
+  printf(" - encodeurG.tours   = %p\r\n", encodeurG.getTours_ptr());
+  printf(" - encodeurD.tours   = %p\r\n", encodeurD.getTours_ptr());
+  printf(" - odometrie.X       = %p\r\n", odometrie.getX_ptr());
+  printf(" - odometrie.Y       = %p\r\n", odometrie.getY_ptr());
+  printf(" - odometrie.Theta   = %p\r\n", odometrie.getTheta_ptr());
+  printf(" - base.erreur_distance    = %p\r\n", base.getErreur_distance_ptr());
+  printf(" - base.erreur_orientation = %p\r\n", base.getErreur_orientation_ptr());
+  printf(" - base.pwm_distance       = %p\r\n", base.getPID_PWM_distance_ptr());
+  printf(" - base.pwm_orientation    = %p\r\n", base.getPID_PWM_orientation_ptr());
+  printf(" - base.pwm_gauche   = %p\r\n", base.getPID_PWM_gauche_ptr());
+  printf(" - base.pwm_droite   = %p\r\n", base.getPID_PWM_droite_ptr());
+  
+  // PID:
+  base.setPID_distance(1, 0, 1);
+  base.setPID_orientation(10, 0, 1);
+  
+  base.start();
+  
+  // Devant:
+  //base.forward(-1400);
+  base.forward(1000);
+  while(1) wait(1);
+  //base.turn(360.0f*10);
+  wait(6);
+  base.turn(90.0f);
+  wait(3);
+  base.forward(-600);
+  wait(3);
+  
+  while(1) wait(1);
+  
+  /*while(1) {
+    buzzer_on();
+    wait(4);
+    buzzer_off();
+    wait(2);    
+  }*/
+  
+  /*while(1) {
+    infraFrontG.mesure();
+    infraFrontD.mesure();
+    infraBack.mesure();
+    float distanceG = infraFrontG.getDistance();
+    float distanceD = infraFrontD.getDistance();
+    float distanceB = infraBack.getDistance();
+    
+    printf("f_G: %f \t f_D: %f \t f_B: %f\r\n", distanceG, distanceD, distanceB);
+    wait(0.1);
+  }*/
+  
+  // Démarre le servo:
+  servo.enable(1500, 20000);
+  servo.pos(90);
+  
+  // Tirette:
+  char count = 0;
+  /*while (count < 5) {
+    if(!tirette) count++;
+    else count = 0;
+    wait(0.1);
+  }*/
+  
+  //wait(1);
+  
+  // Fin de l'épreuve:
+  epreuve_timeout.attach(&finish, epreuve_duration);
+  
+  // Couleur du robot:
+  bool color_side = 1;//switch_color;
+  
+  // Démarre les moteurs:
+  base.start();
+  
+  // Eteint la pompe:
+  pompe = 0;
+  
+  // Désactive capteur arrière:
+  infraBack.off();
+  
+  // Déplacement:
+  int time = 0, timeOut = 500, state = 0;
+  while (1) {
+    // Gestion du temps:
+    wait_ms(1);
+    
+    // Détection:
+    //if (time % 50 == 0 || isStopped)
+    //    time += detection();
+    //else
+        time++;
+    
+    // Etape suivante:
+    
+    // Récupère les erreurs:
+    //float erreur_distance = base.getErreurDistance();
+    //float erreur_orientation = base.getErreurOrientation();
+    //if (time % 100 == 0) printf("d: %f, o: %f\r\n", erreur_distance, erreur_orientation);
+    
+    // Si la position est respectée:
+    //if (state <= 11 && (time > timeOut || abs(erreur_distance) < base_max_erreur_distance && abs(erreur_orientation) < base_max_erreur_orientation)){
+    if (state <= 14 && time > timeOut){
+      if (color_side == 0) timeOut = PURPLE_steps(state);
+      else timeOut = YELLOW_steps(state);
+      
+      state++;
+      time = 0; 
+    }
+    
+  }
+
+}
+
+// Détection de l'environnement;
+int detection() {
+    bool detectG = infraFrontG.mesure();
+    bool detectD = infraFrontD.mesure();
+    bool detectB = infraBack.mesure();
+    
+    //if (detectG || detectD || detectB) {
+    if (detectG || detectD) {
+        base.stop();
+        buzzer_on();
+        
+        // Si première détection:
+        if (isStopped == false) {
+            isStopped = true;
+            moteurG.setPWM(0.1);
+            moteurD.setPWM(0.1);
+            moteurG.forward();
+            moteurD.forward();
+            wait(0.5);
+            moteurD.stop();
+            moteurG.stop();
+        } else {
+            wait(1);
+        }
+        
+    } else if(detectB) {
+        base.stop();
+        buzzer_on();
+        
+        // Si première détection:
+        if (isStopped == false) {
+            isStopped = true;
+            moteurG.setPWM(0.1);
+            moteurD.setPWM(0.1);
+            moteurG.backward();
+            moteurD.backward();
+            wait(0.5);
+            moteurD.stop();
+            moteurG.stop();
+        } else {
+            wait(1);
+        }
+    }
+    else { 
+        if (isStopped == true) base.start();
+        buzzer_off();
+        isStopped = false;
+        return 1;
+    }
+    
+    return 0;
+}
+
+
+/*PwmOut buzzer(PB_1);
+Ticker buzzer_ticker;
+Timeout buzzer_timeout;
+DigitalOut buzzer_led(D2, 0);
+bool buzzer_state = false;
+*/
+void buzzer_note() {
+    /*float period;
+    if (buzzer_state) period =  1000000.0f / 440.0f;
+    else period = 1000000.0f / 349.0f;
+    buzzer_state = !buzzer_state;
+    
+    buzzer_led  = !buzzer_led;
+    
+    buzzer.period_us(period);
+    buzzer.write(0.50f); // 50% duty cycle
+    */
+}
+
+void buzzer_turn_off() {
+    /*buzzer_ticker.detach();
+    buzzer.write(0);
+    buzzer_led = 0;*/
+}
+
+void buzzer_on() {
+    //buzzer_ticker.attach(&buzzer_note, 0.4);
+}
+
+void buzzer_off() {
+   //buzzer_timeout.attach(&buzzer_turn_off, 1.0f);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed May 22 16:54:27 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file