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/Odometrie.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 "Odometrie.h"
xav_jann1 0:1cfd66c3a181 2
xav_jann1 0:1cfd66c3a181 3 Odometrie::Odometrie(Encodeur* encodeurG, Encodeur* encodeurD, float entraxe) {
xav_jann1 0:1cfd66c3a181 4 m_encodeurG = encodeurG;
xav_jann1 0:1cfd66c3a181 5 m_encodeurD = encodeurD;
xav_jann1 0:1cfd66c3a181 6
xav_jann1 0:1cfd66c3a181 7 m_L = 0.0f;
xav_jann1 0:1cfd66c3a181 8 m_X = 0.0f;
xav_jann1 0:1cfd66c3a181 9 m_Y = 0.0f;
xav_jann1 0:1cfd66c3a181 10 m_Theta = 0; //3.14159265359; // PI
xav_jann1 0:1cfd66c3a181 11 m_vitesse = 0.0f;
xav_jann1 0:1cfd66c3a181 12 m_distance = 0.0f;
xav_jann1 0:1cfd66c3a181 13
xav_jann1 0:1cfd66c3a181 14 m_prev_encodeurG_count = m_encodeurG->getTotalCount();
xav_jann1 0:1cfd66c3a181 15 m_prev_encodeurD_count = m_encodeurD->getTotalCount();
xav_jann1 0:1cfd66c3a181 16
xav_jann1 0:1cfd66c3a181 17 // Nombre d'impulsions par mm parcourus:
xav_jann1 0:1cfd66c3a181 18 m_ticks_par_mm = m_encodeurD->getTicks_par_mm();
xav_jann1 0:1cfd66c3a181 19
xav_jann1 0:1cfd66c3a181 20 // Entraxe des 2 encodeurs en ticks:
xav_jann1 0:1cfd66c3a181 21 m_entraxe_ticks = entraxe * m_ticks_par_mm;
xav_jann1 0:1cfd66c3a181 22
xav_jann1 0:1cfd66c3a181 23 printf("Ticks par mm : %f\n\r", m_ticks_par_mm);
xav_jann1 0:1cfd66c3a181 24 printf("Entraxe en ticks : %f\n\r", m_entraxe_ticks);
xav_jann1 0:1cfd66c3a181 25 }
xav_jann1 0:1cfd66c3a181 26
xav_jann1 0:1cfd66c3a181 27 // Getters:
xav_jann1 0:1cfd66c3a181 28 float Odometrie::getX() { return m_X; }
xav_jann1 0:1cfd66c3a181 29 float Odometrie::getY() { return m_Y; }
xav_jann1 0:1cfd66c3a181 30 float Odometrie::getTheta() { return m_Theta; }
xav_jann1 0:1cfd66c3a181 31 float Odometrie::getDistance() { return m_distance; }
xav_jann1 0:1cfd66c3a181 32 float Odometrie::getRealDistance() { return m_distance / m_ticks_par_mm; }
xav_jann1 0:1cfd66c3a181 33 float Odometrie::getVitesse() { return m_vitesse; }
xav_jann1 0:1cfd66c3a181 34
xav_jann1 0:1cfd66c3a181 35 // Pointeurs:
xav_jann1 0:1cfd66c3a181 36 float* Odometrie::getX_ptr() { return &m_X; }
xav_jann1 0:1cfd66c3a181 37 float* Odometrie::getY_ptr() { return &m_Y; }
xav_jann1 0:1cfd66c3a181 38 float* Odometrie::getTheta_ptr() { return &m_Theta; }
xav_jann1 0:1cfd66c3a181 39 float* Odometrie::getDistance_ptr() { return &m_distance; }
xav_jann1 0:1cfd66c3a181 40 float* Odometrie::getVitesse_ptr() { return &m_vitesse; }
xav_jann1 0:1cfd66c3a181 41
xav_jann1 0:1cfd66c3a181 42 // Selon le ClubElek:
xav_jann1 0:1cfd66c3a181 43 void Odometrie::update() {
xav_jann1 0:1cfd66c3a181 44
xav_jann1 0:1cfd66c3a181 45 // Récupère les compteurs des encodeurs:
xav_jann1 0:1cfd66c3a181 46 int encodeurG_count = m_encodeurG->getTotalCount();
xav_jann1 0:1cfd66c3a181 47 int encodeurD_count = m_encodeurD->getTotalCount();
xav_jann1 0:1cfd66c3a181 48
xav_jann1 0:1cfd66c3a181 49 // Différences de ticks depuis le dernier update():
xav_jann1 0:1cfd66c3a181 50 int dEncodeurG = encodeurG_count - m_prev_encodeurG_count;
xav_jann1 0:1cfd66c3a181 51 int dEncodeurD = encodeurD_count - m_prev_encodeurD_count;
xav_jann1 0:1cfd66c3a181 52
xav_jann1 0:1cfd66c3a181 53 // Différences de distance et d'angle:
xav_jann1 0:1cfd66c3a181 54 float dL = (dEncodeurD + dEncodeurG) / 2.0f;
xav_jann1 0:1cfd66c3a181 55 float dTheta = (dEncodeurD - dEncodeurG) / 2.0f;
xav_jann1 0:1cfd66c3a181 56
xav_jann1 0:1cfd66c3a181 57 // Nouvel Angle:
xav_jann1 0:1cfd66c3a181 58 m_Theta += dTheta / m_entraxe_ticks;
xav_jann1 0:1cfd66c3a181 59
xav_jann1 0:1cfd66c3a181 60 // Nouvelles Positions X et Y:
xav_jann1 0:1cfd66c3a181 61 float dX = dL * cos(m_Theta);
xav_jann1 0:1cfd66c3a181 62 float dY = dL * sin(m_Theta);
xav_jann1 0:1cfd66c3a181 63 m_X += dX / m_ticks_par_mm;
xav_jann1 0:1cfd66c3a181 64 m_Y += dY / m_ticks_par_mm;
xav_jann1 0:1cfd66c3a181 65
xav_jann1 0:1cfd66c3a181 66 // Vitesse:
xav_jann1 0:1cfd66c3a181 67 m_vitesse = dL;
xav_jann1 0:1cfd66c3a181 68
xav_jann1 0:1cfd66c3a181 69 // Distance :
xav_jann1 0:1cfd66c3a181 70 m_distance += dL;
xav_jann1 0:1cfd66c3a181 71
xav_jann1 0:1cfd66c3a181 72 // Sauvegarde:
xav_jann1 0:1cfd66c3a181 73 m_prev_encodeurG_count = encodeurG_count;
xav_jann1 0:1cfd66c3a181 74 m_prev_encodeurD_count = encodeurD_count;
xav_jann1 0:1cfd66c3a181 75 }
xav_jann1 0:1cfd66c3a181 76
xav_jann1 0:1cfd66c3a181 77
xav_jann1 0:1cfd66c3a181 78