Diff: odo_asserv.cpp

Revision:

10:0714feaaaee1

Parent:

6:ea6b30c4bb01

Child:

11:e62133022f88

--- 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