AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
Diff: odo_asserv.cpp
- Revision:
- 6:ea6b30c4bb01
- Parent:
- 5:34ed652f8c31
- Child:
- 10:0714feaaaee1
diff -r 34ed652f8c31 -r ea6b30c4bb01 odo_asserv.cpp --- a/odo_asserv.cpp Tue Jul 21 19:33:38 2020 +0000 +++ b/odo_asserv.cpp Sun Jul 26 09:24:31 2020 +0000 @@ -1,25 +1,111 @@ //Nom du fichier : odo_asserv.cpp #include "pins.h" -// Variables globales -// cpt_cdgA est le compteur d'impulsion du codeur de gauche -// cpt_cddA est le compteur d'impulsion du codeur de droite -float posX, posY; // Position et orientation -float theta; +///// 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 odometrie(){ +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; - float distG = (cpt_cdgA * perimetreRoueCodeuse) / NbPulseCodeur; // 1000 est le nombre d'impulsions par tour de la roue codeuse - float distD = (cpt_cddA * perimetreRoueCodeuse) / NbPulseCodeur; // 1000 est le nombre d'impulsions par tour de la roue codeuse + // 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))); - float distRobot = (distG + distD) / 2; // Distance parcourue par le robot (en mm) - float rayon = (ecartCodeuse /2) * ((distD + distG) / (distD - distD)); // Trajectoire s'apparente à un cercle + // On détermine les commandes à envoyer aux moteurs + cmdD = abs((float)distanceCible); + if(cmdD>255) + { + cmdD = 255; + } + cmdG = cmdD; - float dTheta = distRobot / rayon; // Changement d'orientation => Commande - float posX0 = posX - rayon*cos(theta); - float posY0 = posY - rayon*sin(theta); - - theta = theta + dTheta; // Mise à jour des coordonnées - posX = posX0 + rayon*cos(theta); - posY = posY0 + rayon*sin(theta); -} \ No newline at end of file + // 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; + } +} +