AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_capteur_US

Capteur_US

Dependencies:   mbed DRV8825

odo_asserv.cpp@16:4c0b1647e8ae, 2020-10-13 (annotated)

Committer:
g0dd4
Date:
Tue Oct 13 14:50:31 2020 +0000
Revision:
16:4c0b1647e8ae
Parent:
14:dd3c756c6d48 
Conversion du temps en distance ; Detection d'un obstacle; Changement de base; Test_fonctionnelle ;

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 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 */