AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
Diff: odo_asserv.cpp
- Revision:
- 10:0714feaaaee1
- Parent:
- 6:ea6b30c4bb01
- Child:
- 11:e62133022f88
diff -r b3d4b3d51eb7 -r 0714feaaaee1 odo_asserv.cpp --- a/odo_asserv.cpp Thu Jul 30 19:43:16 2020 +0000 +++ b/odo_asserv.cpp Fri Sep 11 10:56:08 2020 +0000 @@ -2,31 +2,13 @@ #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 + +// Coeff à définir empiriquement +const double coeffGLong = 5.956, coeffDLong = -5.956; // constantes permettant la transformation tic/millimètre +const double coeffGAngl = 12.75, coeffDAngl = 12.44; // constantes permettant la transformation tic/degré 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() @@ -41,71 +23,35 @@ 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; +///// ODOMÉTRIE - // Actualisation de la position du robot en xy et en orientation - xR += dDist*cos(dAngl); - yR += dDist*sin(dAngl); - orientation += dAngl; +// Variables et constantes +#define NbPulseCodeur 1000 +#define entraxe 245 - // 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; - } +float x = 0, y = 0, phi = 0; +float x0 = 0, y0 = 0, phi0 = 0; +float dDist = 0, dAngl = 0; +float distG = 0, distD = 0; // Distance parcourue par chaque roue - // 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; +void odometrie() +{ + x0 = x; + y0 = y; + phi0 = phi; - // 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; - } + dDist = ((comptG * coeffGLong) + (comptD * coeffDLong)) / 2; + dAngl = (comptD * coeffDAngl) - (comptG * coeffGAngl); + + x = x0 + dDist * cos(dAngl); + y = y0 + dDist * sin(dAngl); + phi = phi0 + dAngl; } + +///// CONSIGNE ANGLE + +void angle() +{ + +} \ No newline at end of file