AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
odo_asserv.cpp@14:dd3c756c6d48, 2020-09-16 (annotated)
- Committer:
- Nanaud
- Date:
- Wed Sep 16 12:31:54 2020 +0000
- Revision:
- 14:dd3c756c6d48
- Parent:
- 13:a72b0752aa6f
- Child:
- 16:ae65ce77b1f9
Odo 16.09.2020
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 | 14:dd3c756c6d48 | 3 | //#define _PI_ 3.14159265359 |
Nanaud | 2:094c09903a9c | 4 | |
Nanaud | 6:ea6b30c4bb01 | 5 | ///// VARIABLES |
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 | 14:dd3c756c6d48 | 10 | //const double coeffGAngl = 53791/(12*2*_PI_), coeffDAngl = 54972/(12*2*_PI_); // constantes permettant la transformation tic/radian |
Nanaud | 14:dd3c756c6d48 | 11 | const double coeffGAngl = 713.425, coeffDAngl = 729.089; // 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 | 14:dd3c756c6d48 | 15 | |
Nanaud | 6:ea6b30c4bb01 | 16 | ///// INTERRUPTIONS CODEURS |
Nanaud | 6:ea6b30c4bb01 | 17 | |
Nanaud | 6:ea6b30c4bb01 | 18 | void cdgaRise() |
Nanaud | 6:ea6b30c4bb01 | 19 | { |
Nanaud | 6:ea6b30c4bb01 | 20 | if(cdgB) comptG++; |
Nanaud | 6:ea6b30c4bb01 | 21 | else comptG--; |
Nanaud | 6:ea6b30c4bb01 | 22 | } |
Nanaud | 2:094c09903a9c | 23 | |
Nanaud | 6:ea6b30c4bb01 | 24 | void cddaRise() |
Nanaud | 6:ea6b30c4bb01 | 25 | { |
Nanaud | 6:ea6b30c4bb01 | 26 | if(cddB) comptD++; |
Nanaud | 6:ea6b30c4bb01 | 27 | else comptD--; |
Nanaud | 6:ea6b30c4bb01 | 28 | } |
Nanaud | 6:ea6b30c4bb01 | 29 | |
Nanaud | 14:dd3c756c6d48 | 30 | ///* |
Nanaud | 14:dd3c756c6d48 | 31 | ///// 1) ODOMÉTRIE Geonobot : Approximation par segment de droite |
Nanaud | 2:094c09903a9c | 32 | |
Nanaud | 14:dd3c756c6d48 | 33 | #define entraxe 245 // Distance entre les roues codeuses |
Nanaud | 14:dd3c756c6d48 | 34 | double xB = 0, yB = 0, phiB = 0; // Nouvelles coordonnées |
Nanaud | 14:dd3c756c6d48 | 35 | double xA = 0, yA = 0, phiA = 0; // Dernières coordonnées calculées |
Nanaud | 14:dd3c756c6d48 | 36 | double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation |
Nanaud | 13:a72b0752aa6f | 37 | double distG = 0, distD = 0; // Distance parcourue par chaque roue |
Nanaud | 6:ea6b30c4bb01 | 38 | |
Nanaud | 14:dd3c756c6d48 | 39 | void odoGeonobot1() |
Nanaud | 10:0714feaaaee1 | 40 | { |
Nanaud | 14:dd3c756c6d48 | 41 | xA = xB; |
Nanaud | 14:dd3c756c6d48 | 42 | yA = yB; |
Nanaud | 14:dd3c756c6d48 | 43 | phiA = phiB; |
Nanaud | 6:ea6b30c4bb01 | 44 | |
plmir | 12:2c312916a621 | 45 | dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2; |
plmir | 12:2c312916a621 | 46 | dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl)) / entraxe; |
Nanaud | 10:0714feaaaee1 | 47 | |
Nanaud | 14:dd3c756c6d48 | 48 | xB = xA + dDist * cos(phiA); |
Nanaud | 14:dd3c756c6d48 | 49 | yB = yA + dDist * sin(phiA); |
Nanaud | 14:dd3c756c6d48 | 50 | phiB = phiA + dAngl; |
Nanaud | 14:dd3c756c6d48 | 51 | |
plmir | 12:2c312916a621 | 52 | comptG = 0; |
plmir | 12:2c312916a621 | 53 | comptD = 0; |
Nanaud | 14:dd3c756c6d48 | 54 | } |
Nanaud | 14:dd3c756c6d48 | 55 | //*/ |
Nanaud | 14:dd3c756c6d48 | 56 | |
Nanaud | 14:dd3c756c6d48 | 57 | /* |
Nanaud | 14:dd3c756c6d48 | 58 | ///// 2) ODOMÉTRIE Geonobot : Approximation par arc de cercle |
Nanaud | 14:dd3c756c6d48 | 59 | |
Nanaud | 14:dd3c756c6d48 | 60 | #define entraxe 245 // Distance entre les roues codeuses |
Nanaud | 14:dd3c756c6d48 | 61 | double xB = 0, yB = 0, phiB = 0; // Nouvelles coordonnées |
Nanaud | 14:dd3c756c6d48 | 62 | double xA = 0, yA = 0, phiA = 0; // Dernières coordonnées calculées |
Nanaud | 14:dd3c756c6d48 | 63 | double xO = 0, yO = 0; // Centre du cercle de rayon R |
Nanaud | 14:dd3c756c6d48 | 64 | double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation |
Nanaud | 14:dd3c756c6d48 | 65 | double distG = 0, distD = 0; // Distance parcourue par chaque roue |
Nanaud | 14:dd3c756c6d48 | 66 | double rayon = 0; |
Nanaud | 14:dd3c756c6d48 | 67 | |
Nanaud | 14:dd3c756c6d48 | 68 | void odoGeonobot2() |
Nanaud | 14:dd3c756c6d48 | 69 | { |
Nanaud | 14:dd3c756c6d48 | 70 | xA = xB; |
Nanaud | 14:dd3c756c6d48 | 71 | yA = yB; |
Nanaud | 14:dd3c756c6d48 | 72 | phiA = phiB; |
Nanaud | 14:dd3c756c6d48 | 73 | |
Nanaud | 14:dd3c756c6d48 | 74 | dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2; |
Nanaud | 14:dd3c756c6d48 | 75 | dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl)) / entraxe; |
Nanaud | 14:dd3c756c6d48 | 76 | rayon = dDist / dAngl; |
Nanaud | 14:dd3c756c6d48 | 77 | |
Nanaud | 14:dd3c756c6d48 | 78 | xO = xA - R*sin(phiA); |
Nanaud | 14:dd3c756c6d48 | 79 | yO = yA + R*cos(phiA); |
Nanaud | 14:dd3c756c6d48 | 80 | |
Nanaud | 14:dd3c756c6d48 | 81 | phiB = phiA + dAngl; |
Nanaud | 14:dd3c756c6d48 | 82 | |
Nanaud | 14:dd3c756c6d48 | 83 | if (dAngl == 0) { |
Nanaud | 14:dd3c756c6d48 | 84 | xB = xO + R*sin(phiB); |
Nanaud | 14:dd3c756c6d48 | 85 | yB = yO + R*cos(phiB); |
Nanaud | 14:dd3c756c6d48 | 86 | } |
Nanaud | 14:dd3c756c6d48 | 87 | |
Nanaud | 14:dd3c756c6d48 | 88 | else { |
Nanaud | 14:dd3c756c6d48 | 89 | xB = xO + dDist*cos(phiB); |
Nanaud | 14:dd3c756c6d48 | 90 | yB = yO + dDist*sin(phiB); |
Nanaud | 14:dd3c756c6d48 | 91 | } |
Nanaud | 14:dd3c756c6d48 | 92 | |
Nanaud | 14:dd3c756c6d48 | 93 | comptG = 0; |
Nanaud | 14:dd3c756c6d48 | 94 | comptD = 0; |
Nanaud | 14:dd3c756c6d48 | 95 | } |
Nanaud | 14:dd3c756c6d48 | 96 | */ |