AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_us2

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 DRV8825

odo_asserv_2.cpp@12:2c312916a621, 2020-09-11 (annotated)

Committer:
plmir
Date:
Fri Sep 11 13:47:56 2020 +0000
Revision:
12:2c312916a621
Odometrie

Who changed what in which revision?

UserRevisionLine numberNew contents of line
plmir 12:2c312916a621 1 //Nom du fichier : odo_asserv_2.cpp
plmir 12:2c312916a621 2 #include "pins.h"
plmir 12:2c312916a621 3
plmir 12:2c312916a621 4 ///// VARIABLES
plmir 12:2c312916a621 5
plmir 12:2c312916a621 6 Ticker ticker_odo_2;
plmir 12:2c312916a621 7
plmir 12:2c312916a621 8 // Coeff à définir empiriquement
plmir 12:2c312916a621 9 const double coeffGLong = 5.956, coeffDLong = -5.956; // constantes permettant la transformation tic/millimètre
plmir 12:2c312916a621 10 //const double coeffGAngl = 12.4516203705, coeffDAngl = 12.725; // constantes permettant la transformation tic/degré
plmir 12:2c312916a621 11 const double coeffGAngl = 12*2*Pi*53791, coeffDAngl = 12*2*Pi*54972; // constantes permettant la transformation tic/radian
plmir 12:2c312916a621 12
plmir 12:2c312916a621 13 long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur
plmir 12:2c312916a621 14
plmir 12:2c312916a621 15 ///// INTERRUPTIONS CODEURS
plmir 12:2c312916a621 16
plmir 12:2c312916a621 17 void cdgaRise()
plmir 12:2c312916a621 18 {
plmir 12:2c312916a621 19 if(cdgB) comptG++;
plmir 12:2c312916a621 20 else comptG--;
plmir 12:2c312916a621 21 }
plmir 12:2c312916a621 22
plmir 12:2c312916a621 23 void cddaRise()
plmir 12:2c312916a621 24 {
plmir 12:2c312916a621 25 if(cddB) comptD++;
plmir 12:2c312916a621 26 else comptD--;
plmir 12:2c312916a621 27 }
plmir 12:2c312916a621 28
plmir 12:2c312916a621 29 ///// ODOMÉTRIE
plmir 12:2c312916a621 30
plmir 12:2c312916a621 31 // Variables et constantes
plmir 12:2c312916a621 32 #define NbPulseCodeur 1000
plmir 12:2c312916a621 33 #define entraxe 245
plmir 12:2c312916a621 34
plmir 12:2c312916a621 35 float x = 0, y = 0, phi = 0;
plmir 12:2c312916a621 36 float phi_deg;
plmir 12:2c312916a621 37 float x0 = 0, y0 = 0, phi0 = 0;
plmir 12:2c312916a621 38 float dDist = 0, dAngl = 0;
plmir 12:2c312916a621 39 float distG = 0, distD = 0; // Distance parcourue par chaque roue
plmir 12:2c312916a621 40
plmir 12:2c312916a621 41 void odometrie()
plmir 12:2c312916a621 42 {
plmir 12:2c312916a621 43 x0 = x;
plmir 12:2c312916a621 44 y0 = y;
plmir 12:2c312916a621 45 phi0 = phi;
plmir 12:2c312916a621 46
plmir 12:2c312916a621 47 dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2;
plmir 12:2c312916a621 48 dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl)) / entraxe;
plmir 12:2c312916a621 49
plmir 12:2c312916a621 50 x = x0 + dDist * cos(phi0);
plmir 12:2c312916a621 51 y = y0 + dDist * sin(phi0);
plmir 12:2c312916a621 52 phi = phi0 + dAngl;
plmir 12:2c312916a621 53 phi_deg = phi*180/Pi;
plmir 12:2c312916a621 54
plmir 12:2c312916a621 55
plmir 12:2c312916a621 56 comptG = 0;
plmir 12:2c312916a621 57 comptD = 0;
plmir 12:2c312916a621 58 }
plmir 12:2c312916a621 59
plmir 12:2c312916a621 60
plmir 12:2c312916a621 61 ///// CONSIGNE ANGLE
plmir 12:2c312916a621 62
plmir 12:2c312916a621 63 void angle()
plmir 12:2c312916a621 64 {
plmir 12:2c312916a621 65
plmir 12:2c312916a621 66 }