AresENSEA-CDF2020
/
AresCDFMainCode_capteur_US
Capteur_US
odo_asserv.cpp
- Committer:
- Nanaud
- Date:
- 2020-07-26
- Revision:
- 6:ea6b30c4bb01
- Parent:
- 5:34ed652f8c31
- Child:
- 10:0714feaaaee1
File content as of revision 6:ea6b30c4bb01:
//Nom du fichier : odo_asserv.cpp #include "pins.h" ///// VARIABLES const double coeffGLong = 1, coeffDLong = 1; // constantes permettant la transformation tic/millimètre => à définir const double coeffGAngl = 1, coeffDAngl = 1; // constantes permettant la transformation tic/degrés => à définir long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur double xR = 0, yR = 0; // position du robot double xC = 0, yC = 0; // position de la cible du robot double dDist = 0, dAngl = 0; // delta position et angle double orientation = 0; // cap du robot double consigneOrientation = 0; // angle entre le robot et la cible int signe = 1; // variable permettant de savoir si la cible est à gauche ou à droite du robot double distanceCible = 0; // distance entre le robot et la cible double coeffP = 1, coeffD = 1; // paramètre de l'asservissement en angle du robot double erreurAngle = 0, erreurPre = 0, deltaErreur = 0; // variables utilisées pour asservir le robot float cmdG =0, cmdD=0; // Commandes à envoyer aux moteurs ///// ///// INTERRUPTIONS CODEURS void cdgaRise() { if(cdgB) comptG++; else comptG--; } void cddaRise() { if(cddB) comptD++; else comptD--; } ///// ///// ODOMETRIE void mvtsRobot() { // Calcul variations de position en distance et en angle dDist =(coeffGLong*comptG + coeffDLong*comptD)/2; dAngl = coeffDAngl*comptD - coeffGAngl*comptG; // Actualisation de la position du robot en xy et en orientation xR += dDist*cos(dAngl); yR += dDist*sin(dAngl); orientation += dAngl; // Calcul distance séparant le robot et la cible distanceCible = sqrt((xC-xR)*(xC-xR)+(yC-yR)*(yC-yR)); // On regarde si la cible est à gauche ou à droite du robot if(yR > yC) { signe = 1; // Inversé, non ? } else { signe = -1; } // Calcul angle entre le robot et la cible consigneOrientation = signe * acos((xC-xR)/((xC-xR)*(xC-xR)*(yC-yR)*(yC-yR))); // On détermine les commandes à envoyer aux moteurs cmdD = abs((float)distanceCible); if(cmdD>255) { cmdD = 255; } cmdG = cmdD; // Calcul de l'erreur erreurAngle = consigneOrientation - orientation; // Calcul de la différence entre l'erreur au coup précédent et l'erreur actuelle. deltaErreur = erreurAngle - erreurPre; // Mise en mémoire de l'erreur actuelle erreurPre = erreurAngle; // Calcul de la valeur à envoyer aux moteurs pour tourner int deltaCommande = coeffP*erreurAngle + coeffD * deltaErreur; cmdG += deltaCommande; cmdD -= deltaCommande; if(cmdD>255) { cmdD = 255; }else if(cmdD < 0) { cmdD = 0; } if(cmdG>255) { cmdG = 255; }else if(cmdG < 0) { cmdG = 0; } }