AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_capteur_US

Capteur_US

Dependencies:   mbed DRV8825

odo_asserv.cpp@13:a72b0752aa6f, 2020-09-11 (annotated)

Committer:
Nanaud
Date:
Fri Sep 11 14:15:57 2020 +0000
Revision:
13:a72b0752aa6f
Parent:
12:2c312916a621
Child:
14:dd3c756c6d48
Vers odo 2

Who changed what in which revision?

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