Xavier Jannin / Mbed 2 deprecated PETIT_robot

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:   mbed

Base/Base.cpp@0:1cfd66c3a181, 2019-05-22 (annotated)

Committer:
xav_jann1
Date:
Wed May 22 16:54:27 2019 +0000
Revision:
0:1cfd66c3a181
Premiere version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xav_jann1 0:1cfd66c3a181 1 #include "Base.h"
xav_jann1 0:1cfd66c3a181 2
xav_jann1 0:1cfd66c3a181 3 // Constructeur:
xav_jann1 0:1cfd66c3a181 4 Base::Base(Moteur* moteurG, Moteur* moteurD, Odometrie* odometrie, int update_delay_us):
xav_jann1 0:1cfd66c3a181 5 m_asservissement_distance(1, 1, 1), m_asservissement_orientation(1, 1, 1) {
xav_jann1 0:1cfd66c3a181 6
xav_jann1 0:1cfd66c3a181 7 m_moteurG = moteurG;
xav_jann1 0:1cfd66c3a181 8 m_moteurD = moteurD;
xav_jann1 0:1cfd66c3a181 9 m_moteurs_isEnabled = false;
xav_jann1 0:1cfd66c3a181 10
xav_jann1 0:1cfd66c3a181 11 // Odométrie:
xav_jann1 0:1cfd66c3a181 12 m_odometrie = odometrie;
xav_jann1 0:1cfd66c3a181 13
xav_jann1 0:1cfd66c3a181 14 // Asservissement:
xav_jann1 0:1cfd66c3a181 15 m_pos_distance = 0;
xav_jann1 0:1cfd66c3a181 16 m_pos_orientation = 0;
xav_jann1 0:1cfd66c3a181 17 m_moteurG->setPWM_max(0.4);
xav_jann1 0:1cfd66c3a181 18 m_moteurD->setPWM_max(0.4);
xav_jann1 0:1cfd66c3a181 19 m_moteurG->setOffset(0.1);
xav_jann1 0:1cfd66c3a181 20 m_moteurD->setOffset(0.1);
xav_jann1 0:1cfd66c3a181 21 m_pwm_dynamic = 0.2f; // max = 0.6 - 0.1
xav_jann1 0:1cfd66c3a181 22
xav_jann1 0:1cfd66c3a181 23 // Erreur maximale:
xav_jann1 0:1cfd66c3a181 24 m_max_erreur_distance = 300;
xav_jann1 0:1cfd66c3a181 25 m_max_erreur_orientation = 3;
xav_jann1 0:1cfd66c3a181 26
xav_jann1 0:1cfd66c3a181 27 // Répartition PWM entre distance et orientation:
xav_jann1 0:1cfd66c3a181 28 m_distance_orientation_ratio = 0.5f; // 50% -> distance, 50% -> orientation
xav_jann1 0:1cfd66c3a181 29
xav_jann1 0:1cfd66c3a181 30 // Mesure du déplacement toutes les <update_delay_us> µs:
xav_jann1 0:1cfd66c3a181 31 m_ticker.attach_us(callback(this, &Base::update), update_delay_us); // 2 kHz pour 500 us
xav_jann1 0:1cfd66c3a181 32 }
xav_jann1 0:1cfd66c3a181 33
xav_jann1 0:1cfd66c3a181 34 // Setters:
xav_jann1 0:1cfd66c3a181 35 void Base::setDistanceTo(float d) { m_asservissement_distance.toValue(d); }
xav_jann1 0:1cfd66c3a181 36 void Base::setOrientationTo(float o) { m_asservissement_orientation.toValue(M_PI * o / 180.0f ); }
xav_jann1 0:1cfd66c3a181 37
xav_jann1 0:1cfd66c3a181 38 void Base::setPID_distance(float P, float I, float D) {
xav_jann1 0:1cfd66c3a181 39 m_asservissement_distance.setPID(P, I, D);
xav_jann1 0:1cfd66c3a181 40 }
xav_jann1 0:1cfd66c3a181 41 void Base::setPID_orientation(float P, float I, float D) {
xav_jann1 0:1cfd66c3a181 42 m_asservissement_orientation.setPID(P, I, D);
xav_jann1 0:1cfd66c3a181 43 }
xav_jann1 0:1cfd66c3a181 44
xav_jann1 0:1cfd66c3a181 45 // Getters:
xav_jann1 0:1cfd66c3a181 46 float* Base::getErreur_distance_ptr() { return &m_erreur_distance; }
xav_jann1 0:1cfd66c3a181 47 float* Base::getErreur_orientation_ptr() { return &m_erreur_orientation; }
xav_jann1 0:1cfd66c3a181 48 float* Base::getPID_PWM_distance_ptr() { return &m_pwm_distance; }
xav_jann1 0:1cfd66c3a181 49 float* Base::getPID_PWM_orientation_ptr() { return &m_pwm_orientation; }
xav_jann1 0:1cfd66c3a181 50 float* Base::getPID_PWM_gauche_ptr() { return &m_pwm_gauche; }
xav_jann1 0:1cfd66c3a181 51 float* Base::getPID_PWM_droite_ptr() { return &m_pwm_droite; }
xav_jann1 0:1cfd66c3a181 52
xav_jann1 0:1cfd66c3a181 53 /*** Déplacement ***/
xav_jann1 0:1cfd66c3a181 54
xav_jann1 0:1cfd66c3a181 55 // Start:
xav_jann1 0:1cfd66c3a181 56 void Base::start() {
xav_jann1 0:1cfd66c3a181 57 m_moteurs_isEnabled = true;
xav_jann1 0:1cfd66c3a181 58 }
xav_jann1 0:1cfd66c3a181 59
xav_jann1 0:1cfd66c3a181 60 void Base::stop() {
xav_jann1 0:1cfd66c3a181 61 m_moteurs_isEnabled = false;
xav_jann1 0:1cfd66c3a181 62
xav_jann1 0:1cfd66c3a181 63 // Arrêt des moteurs:
xav_jann1 0:1cfd66c3a181 64 m_moteurG->stop();
xav_jann1 0:1cfd66c3a181 65 m_moteurD->stop();
xav_jann1 0:1cfd66c3a181 66 }
xav_jann1 0:1cfd66c3a181 67
xav_jann1 0:1cfd66c3a181 68
xav_jann1 0:1cfd66c3a181 69 // Stop:
xav_jann1 0:1cfd66c3a181 70
xav_jann1 0:1cfd66c3a181 71 // Avant: (en mm)
xav_jann1 0:1cfd66c3a181 72 void Base::forward(float d) {
xav_jann1 0:1cfd66c3a181 73 m_pos_distance += d;
xav_jann1 0:1cfd66c3a181 74 setDistanceTo(m_pos_distance);
xav_jann1 0:1cfd66c3a181 75 }
xav_jann1 0:1cfd66c3a181 76
xav_jann1 0:1cfd66c3a181 77 // Arrière: (en mm)
xav_jann1 0:1cfd66c3a181 78 void Base::backward(float d) {
xav_jann1 0:1cfd66c3a181 79 m_pos_distance -= d;
xav_jann1 0:1cfd66c3a181 80 setDistanceTo(m_pos_distance);
xav_jann1 0:1cfd66c3a181 81 }
xav_jann1 0:1cfd66c3a181 82
xav_jann1 0:1cfd66c3a181 83 // Tourner: (en °)
xav_jann1 0:1cfd66c3a181 84 void Base::turn(float o) {
xav_jann1 0:1cfd66c3a181 85 m_pos_orientation += o;
xav_jann1 0:1cfd66c3a181 86 setOrientationTo(m_pos_orientation);
xav_jann1 0:1cfd66c3a181 87 }
xav_jann1 0:1cfd66c3a181 88
xav_jann1 0:1cfd66c3a181 89 // Renvoie le signe de la valeur:
xav_jann1 0:1cfd66c3a181 90 template <typename T> int sign(T val) {
xav_jann1 0:1cfd66c3a181 91 return (T(0) < val) - (val < T(0));
xav_jann1 0:1cfd66c3a181 92 }
xav_jann1 0:1cfd66c3a181 93
xav_jann1 0:1cfd66c3a181 94
xav_jann1 0:1cfd66c3a181 95 // Mise à jour de l'odometrie et de l'asservissement:
xav_jann1 0:1cfd66c3a181 96 void Base::update() {
xav_jann1 0:1cfd66c3a181 97 // Mise à jour de la position:
xav_jann1 0:1cfd66c3a181 98 m_odometrie->update();
xav_jann1 0:1cfd66c3a181 99
xav_jann1 0:1cfd66c3a181 100 // Calcul de l'erreur de distance:
xav_jann1 0:1cfd66c3a181 101 float distance = m_odometrie->getRealDistance();
xav_jann1 0:1cfd66c3a181 102 float erreur_distance = m_asservissement_distance.computeError(distance);
xav_jann1 0:1cfd66c3a181 103
xav_jann1 0:1cfd66c3a181 104 // Calcul de l'erreur d'orientation:
xav_jann1 0:1cfd66c3a181 105 float theta = m_odometrie->getTheta();
xav_jann1 0:1cfd66c3a181 106 float erreur_theta = m_asservissement_orientation.computeError(theta);
xav_jann1 0:1cfd66c3a181 107
xav_jann1 0:1cfd66c3a181 108 // Correction de l'erreur:
xav_jann1 0:1cfd66c3a181 109 if(abs(erreur_distance) > m_max_erreur_distance) erreur_distance = m_max_erreur_distance * sign(erreur_distance);
xav_jann1 0:1cfd66c3a181 110 if(abs(erreur_theta) > m_max_erreur_orientation) erreur_theta = m_max_erreur_orientation * sign(erreur_theta);
xav_jann1 0:1cfd66c3a181 111
xav_jann1 0:1cfd66c3a181 112 // PWM correspondant à la distance et à l'orientation:
xav_jann1 0:1cfd66c3a181 113 float pwm_distance = (erreur_distance / m_max_erreur_distance) * m_distance_orientation_ratio;
xav_jann1 0:1cfd66c3a181 114 float pwm_theta = (erreur_theta / m_max_erreur_orientation) * (1.0f - m_distance_orientation_ratio);
xav_jann1 0:1cfd66c3a181 115
xav_jann1 0:1cfd66c3a181 116 // PWM à envoyer aux moteurs:
xav_jann1 0:1cfd66c3a181 117 float pwm_droite = (pwm_distance + pwm_theta) * m_pwm_dynamic;
xav_jann1 0:1cfd66c3a181 118 float pwm_gauche = (pwm_distance - pwm_theta) * m_pwm_dynamic;
xav_jann1 0:1cfd66c3a181 119
xav_jann1 0:1cfd66c3a181 120 // Commande des moteurs:
xav_jann1 0:1cfd66c3a181 121 if (m_moteurs_isEnabled == true) {
xav_jann1 0:1cfd66c3a181 122 m_moteurD->turn(pwm_droite);
xav_jann1 0:1cfd66c3a181 123 m_moteurG->turn(pwm_gauche);
xav_jann1 0:1cfd66c3a181 124 }
xav_jann1 0:1cfd66c3a181 125
xav_jann1 0:1cfd66c3a181 126 // Sauvegarde / Debug:
xav_jann1 0:1cfd66c3a181 127 m_pwm_distance = pwm_distance;
xav_jann1 0:1cfd66c3a181 128 m_pwm_orientation = pwm_theta;
xav_jann1 0:1cfd66c3a181 129 m_erreur_distance = erreur_distance;
xav_jann1 0:1cfd66c3a181 130 m_erreur_orientation = erreur_theta;
xav_jann1 0:1cfd66c3a181 131 m_pwm_droite = pwm_droite;
xav_jann1 0:1cfd66c3a181 132 m_pwm_gauche = pwm_gauche;
xav_jann1 0:1cfd66c3a181 133 }
xav_jann1 0:1cfd66c3a181 134
xav_jann1 0:1cfd66c3a181 135 // Getters:
xav_jann1 0:1cfd66c3a181 136 float Base::getErreurDistance() { return m_erreur_distance; }
xav_jann1 0:1cfd66c3a181 137 float Base::getErreurOrientation() { return m_erreur_orientation; }
xav_jann1 0:1cfd66c3a181 138
xav_jann1 0:1cfd66c3a181 139 //float Base::moveTo(float X, float Y);
xav_jann1 0:1cfd66c3a181 140 //float Base::setAngle();
xav_jann1 0:1cfd66c3a181 141
xav_jann1 0:1cfd66c3a181 142 //float Base::turn(float angle);