AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_capteur_US

Capteur_US

Dependencies:   mbed DRV8825

odo_asserv.cpp@6:ea6b30c4bb01, 2020-07-26 (annotated)

Committer:
Nanaud
Date:
Sun Jul 26 09:24:31 2020 +0000
Revision:
6:ea6b30c4bb01
Parent:
5:34ed652f8c31
Child:
10:0714feaaaee1
Fusion des fichiers codeurs et odo_asserv

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 6:ea6b30c4bb01 5 const double coeffGLong = 1, coeffDLong = 1; // constantes permettant la transformation tic/millimètre => à définir
Nanaud 6:ea6b30c4bb01 6 const double coeffGAngl = 1, coeffDAngl = 1; // constantes permettant la transformation tic/degrés => à définir
Nanaud 6:ea6b30c4bb01 7
Nanaud 6:ea6b30c4bb01 8 long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur
Nanaud 6:ea6b30c4bb01 9
Nanaud 6:ea6b30c4bb01 10 double xR = 0, yR = 0; // position du robot
Nanaud 6:ea6b30c4bb01 11 double xC = 0, yC = 0; // position de la cible du robot
Nanaud 6:ea6b30c4bb01 12
Nanaud 6:ea6b30c4bb01 13 double dDist = 0, dAngl = 0; // delta position et angle
Nanaud 6:ea6b30c4bb01 14
Nanaud 6:ea6b30c4bb01 15 double orientation = 0; // cap du robot
Nanaud 6:ea6b30c4bb01 16 double consigneOrientation = 0; // angle entre le robot et la cible
Nanaud 6:ea6b30c4bb01 17
Nanaud 6:ea6b30c4bb01 18 int signe = 1; // variable permettant de savoir si la cible est à gauche ou à droite du robot
Nanaud 6:ea6b30c4bb01 19
Nanaud 6:ea6b30c4bb01 20 double distanceCible = 0; // distance entre le robot et la cible
Nanaud 6:ea6b30c4bb01 21
Nanaud 6:ea6b30c4bb01 22 double coeffP = 1, coeffD = 1; // paramètre de l'asservissement en angle du robot
Nanaud 6:ea6b30c4bb01 23
Nanaud 6:ea6b30c4bb01 24 double erreurAngle = 0, erreurPre = 0, deltaErreur = 0; // variables utilisées pour asservir le robot
Nanaud 6:ea6b30c4bb01 25
Nanaud 6:ea6b30c4bb01 26 float cmdG =0, cmdD=0; // Commandes à envoyer aux moteurs
Nanaud 6:ea6b30c4bb01 27
Nanaud 6:ea6b30c4bb01 28 /////
Nanaud 6:ea6b30c4bb01 29
Nanaud 6:ea6b30c4bb01 30 ///// INTERRUPTIONS CODEURS
Nanaud 6:ea6b30c4bb01 31
Nanaud 6:ea6b30c4bb01 32 void cdgaRise()
Nanaud 6:ea6b30c4bb01 33 {
Nanaud 6:ea6b30c4bb01 34 if(cdgB) comptG++;
Nanaud 6:ea6b30c4bb01 35 else comptG--;
Nanaud 6:ea6b30c4bb01 36 }
Nanaud 2:094c09903a9c 37
Nanaud 6:ea6b30c4bb01 38 void cddaRise()
Nanaud 6:ea6b30c4bb01 39 {
Nanaud 6:ea6b30c4bb01 40 if(cddB) comptD++;
Nanaud 6:ea6b30c4bb01 41 else comptD--;
Nanaud 6:ea6b30c4bb01 42 }
Nanaud 6:ea6b30c4bb01 43
Nanaud 6:ea6b30c4bb01 44 /////
Nanaud 6:ea6b30c4bb01 45
Nanaud 6:ea6b30c4bb01 46 ///// ODOMETRIE
Nanaud 6:ea6b30c4bb01 47
Nanaud 6:ea6b30c4bb01 48 void mvtsRobot()
Nanaud 6:ea6b30c4bb01 49 {
Nanaud 6:ea6b30c4bb01 50 // Calcul variations de position en distance et en angle
Nanaud 6:ea6b30c4bb01 51 dDist =(coeffGLong*comptG + coeffDLong*comptD)/2;
Nanaud 6:ea6b30c4bb01 52 dAngl = coeffDAngl*comptD - coeffGAngl*comptG;
Nanaud 2:094c09903a9c 53
Nanaud 6:ea6b30c4bb01 54 // Actualisation de la position du robot en xy et en orientation
Nanaud 6:ea6b30c4bb01 55 xR += dDist*cos(dAngl);
Nanaud 6:ea6b30c4bb01 56 yR += dDist*sin(dAngl);
Nanaud 6:ea6b30c4bb01 57 orientation += dAngl;
Nanaud 6:ea6b30c4bb01 58
Nanaud 6:ea6b30c4bb01 59 // Calcul distance séparant le robot et la cible
Nanaud 6:ea6b30c4bb01 60 distanceCible = sqrt((xC-xR)*(xC-xR)+(yC-yR)*(yC-yR));
Nanaud 6:ea6b30c4bb01 61
Nanaud 6:ea6b30c4bb01 62 // On regarde si la cible est à gauche ou à droite du robot
Nanaud 6:ea6b30c4bb01 63 if(yR > yC) {
Nanaud 6:ea6b30c4bb01 64 signe = 1; // Inversé, non ?
Nanaud 6:ea6b30c4bb01 65 } else {
Nanaud 6:ea6b30c4bb01 66 signe = -1;
Nanaud 6:ea6b30c4bb01 67 }
Nanaud 6:ea6b30c4bb01 68
Nanaud 6:ea6b30c4bb01 69 // Calcul angle entre le robot et la cible
Nanaud 6:ea6b30c4bb01 70 consigneOrientation = signe * acos((xC-xR)/((xC-xR)*(xC-xR)*(yC-yR)*(yC-yR)));
Nanaud 2:094c09903a9c 71
Nanaud 6:ea6b30c4bb01 72 // On détermine les commandes à envoyer aux moteurs
Nanaud 6:ea6b30c4bb01 73 cmdD = abs((float)distanceCible);
Nanaud 6:ea6b30c4bb01 74 if(cmdD>255)
Nanaud 6:ea6b30c4bb01 75 {
Nanaud 6:ea6b30c4bb01 76 cmdD = 255;
Nanaud 6:ea6b30c4bb01 77 }
Nanaud 6:ea6b30c4bb01 78 cmdG = cmdD;
Nanaud 2:094c09903a9c 79
Nanaud 6:ea6b30c4bb01 80 // Calcul de l'erreur
Nanaud 6:ea6b30c4bb01 81 erreurAngle = consigneOrientation - orientation;
Nanaud 6:ea6b30c4bb01 82
Nanaud 6:ea6b30c4bb01 83 // Calcul de la différence entre l'erreur au coup précédent et l'erreur actuelle.
Nanaud 6:ea6b30c4bb01 84 deltaErreur = erreurAngle - erreurPre;
Nanaud 6:ea6b30c4bb01 85
Nanaud 6:ea6b30c4bb01 86 // Mise en mémoire de l'erreur actuelle
Nanaud 6:ea6b30c4bb01 87 erreurPre = erreurAngle;
Nanaud 6:ea6b30c4bb01 88
Nanaud 6:ea6b30c4bb01 89 // Calcul de la valeur à envoyer aux moteurs pour tourner
Nanaud 6:ea6b30c4bb01 90 int deltaCommande = coeffP*erreurAngle + coeffD * deltaErreur;
Nanaud 6:ea6b30c4bb01 91
Nanaud 6:ea6b30c4bb01 92 cmdG += deltaCommande;
Nanaud 6:ea6b30c4bb01 93 cmdD -= deltaCommande;
Nanaud 6:ea6b30c4bb01 94
Nanaud 6:ea6b30c4bb01 95 if(cmdD>255)
Nanaud 6:ea6b30c4bb01 96 {
Nanaud 6:ea6b30c4bb01 97 cmdD = 255;
Nanaud 6:ea6b30c4bb01 98 }else if(cmdD < 0)
Nanaud 6:ea6b30c4bb01 99 {
Nanaud 6:ea6b30c4bb01 100 cmdD = 0;
Nanaud 6:ea6b30c4bb01 101 }
Nanaud 6:ea6b30c4bb01 102
Nanaud 6:ea6b30c4bb01 103 if(cmdG>255)
Nanaud 6:ea6b30c4bb01 104 {
Nanaud 6:ea6b30c4bb01 105 cmdG = 255;
Nanaud 6:ea6b30c4bb01 106 }else if(cmdG < 0)
Nanaud 6:ea6b30c4bb01 107 {
Nanaud 6:ea6b30c4bb01 108 cmdG = 0;
Nanaud 6:ea6b30c4bb01 109 }
Nanaud 6:ea6b30c4bb01 110 }
Nanaud 6:ea6b30c4bb01 111