AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
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?
User | Revision | Line number | New 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 | } |