AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_us2

Test

Dependencies:   mbed DRV8825

odo_asserv.cpp@10:0714feaaaee1, 2020-09-11 (annotated)

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