Diff: odo_asserv.cpp

Revision:

6:ea6b30c4bb01

Parent:

5:34ed652f8c31

Child:

10:0714feaaaee1

--- 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;
+    }
+}
+