AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_capteur_US

Capteur_US

Dependencies:   mbed DRV8825

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

Committer:
Nanaud
Date:
Fri Sep 11 11:11:56 2020 +0000
Revision:
11:e62133022f88
Parent:
10:0714feaaaee1
Child:
12:2c312916a621
11.09.20

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
Nanaud 10:0714feaaaee1 6 // Coeff à définir empiriquement
Nanaud 10:0714feaaaee1 7 const double coeffGLong = 5.956, coeffDLong = -5.956; // constantes permettant la transformation tic/millimètre
Nanaud 11:e62133022f88 8 //const double coeffGAngl = 12.4516203705, coeffDAngl = 12.725; // constantes permettant la transformation tic/degré
Nanaud 11:e62133022f88 9 const double coeffGAngl = 12*2*Pi*53791, coeffDAngl = 12*2*Pi*54972; // constantes permettant la transformation tic/radian
Nanaud 6:ea6b30c4bb01 10
Nanaud 6:ea6b30c4bb01 11 long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur
Nanaud 6:ea6b30c4bb01 12
Nanaud 6:ea6b30c4bb01 13 ///// INTERRUPTIONS CODEURS
Nanaud 6:ea6b30c4bb01 14
Nanaud 6:ea6b30c4bb01 15 void cdgaRise()
Nanaud 6:ea6b30c4bb01 16 {
Nanaud 6:ea6b30c4bb01 17 if(cdgB) comptG++;
Nanaud 6:ea6b30c4bb01 18 else comptG--;
Nanaud 6:ea6b30c4bb01 19 }
Nanaud 2:094c09903a9c 20
Nanaud 6:ea6b30c4bb01 21 void cddaRise()
Nanaud 6:ea6b30c4bb01 22 {
Nanaud 6:ea6b30c4bb01 23 if(cddB) comptD++;
Nanaud 6:ea6b30c4bb01 24 else comptD--;
Nanaud 6:ea6b30c4bb01 25 }
Nanaud 6:ea6b30c4bb01 26
Nanaud 10:0714feaaaee1 27 ///// ODOMÉTRIE
Nanaud 2:094c09903a9c 28
Nanaud 10:0714feaaaee1 29 // Variables et constantes
Nanaud 10:0714feaaaee1 30 #define NbPulseCodeur 1000
Nanaud 10:0714feaaaee1 31 #define entraxe 245
Nanaud 6:ea6b30c4bb01 32
Nanaud 10:0714feaaaee1 33 float x = 0, y = 0, phi = 0;
Nanaud 10:0714feaaaee1 34 float x0 = 0, y0 = 0, phi0 = 0;
Nanaud 10:0714feaaaee1 35 float dDist = 0, dAngl = 0;
Nanaud 10:0714feaaaee1 36 float distG = 0, distD = 0; // Distance parcourue par chaque roue
Nanaud 6:ea6b30c4bb01 37
Nanaud 10:0714feaaaee1 38 void odometrie()
Nanaud 10:0714feaaaee1 39 {
Nanaud 10:0714feaaaee1 40 x0 = x;
Nanaud 10:0714feaaaee1 41 y0 = y;
Nanaud 10:0714feaaaee1 42 phi0 = phi;
Nanaud 6:ea6b30c4bb01 43
Nanaud 10:0714feaaaee1 44 dDist = ((comptG * coeffGLong) + (comptD * coeffDLong)) / 2;
Nanaud 10:0714feaaaee1 45 dAngl = (comptD * coeffDAngl) - (comptG * coeffGAngl);
Nanaud 10:0714feaaaee1 46
Nanaud 10:0714feaaaee1 47 x = x0 + dDist * cos(dAngl);
Nanaud 10:0714feaaaee1 48 y = y0 + dDist * sin(dAngl);
Nanaud 10:0714feaaaee1 49 phi = phi0 + dAngl;
Nanaud 6:ea6b30c4bb01 50 }
Nanaud 6:ea6b30c4bb01 51
Nanaud 10:0714feaaaee1 52
Nanaud 10:0714feaaaee1 53 ///// CONSIGNE ANGLE
Nanaud 10:0714feaaaee1 54
Nanaud 10:0714feaaaee1 55 void angle()
Nanaud 10:0714feaaaee1 56 {
Nanaud 10:0714feaaaee1 57
Nanaud 10:0714feaaaee1 58 }