Diff: odo_asserv.cpp

Revision:

16:ae65ce77b1f9

Parent:

14:dd3c756c6d48

Child:

17:176a1b4a2fa8

--- a/odo_asserv.cpp	Wed Sep 16 12:31:54 2020 +0000
+++ b/odo_asserv.cpp	Fri Oct 02 21:09:45 2020 +0000
@@ -1,14 +1,15 @@
 //Nom du fichier : odo_asserv.cpp
 #include "pins.h"
-//#define _PI_ 3.14159265359
+
+#define VMAX 20
 
 ///// VARIABLES
 Ticker ticker_odo;
+Ticker ticker_asserv;
 
 // Coeff à définir empiriquement
-const double coeffGLong = 5.956, coeffDLong = -5.956; // constantes permettant la transformation tic/millimètre
-//const double coeffGAngl = 53791/(12*2*_PI_), coeffDAngl = 54972/(12*2*_PI_); // constantes permettant la transformation tic/radian
-const double coeffGAngl = 713.425, coeffDAngl = 729.089; // constantes permettant la transformation tic/radian
+const double coeffGLong = 5.956, coeffDLong = 5.956; // constantes permettant la transformation tic/millimètre
+const double coeffGAngl = 737.447, coeffDAngl = 748.057; // constantes permettant la transformation tic/radian
 
 long comptG = 0, comptD = 0; // nb de tics comptés pour chaque codeur
 
@@ -17,37 +18,65 @@
 
 void cdgaRise()
 {
-    if(cdgB) comptG++;
-    else comptG--;
+    if(cdgB) comptG--;
+    else comptG++;
 }
 
 void cddaRise()
 {
-    if(cddB) comptD++;
-    else comptD--;
+    if(cddB) comptD--;
+    else comptD++;
 }
 
+/*
+// odo1()
+double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
+double x = 0, y = 0, O = 0;
+
+
+void odo1()
+{
+    dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2;
+    dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl));
+
+    x += dDist * cos(dAngl);
+    y += dDist * sin(dAngl);
+    O += dAngl;
+
+    comptG = 0;
+    comptD = 0;
+}
+*/
+
 ///*
-///// 1) ODOMÉTRIE Geonobot : Approximation par segment de droite
+// odo2()
+//#define diametre 51.45 // 51.45 théorique
+//#define N 1000 // 1000 théorique
+#define entraxe 253 // 255 théorique
+//const double coeffG = ((double)(diametre/2)/(double)N)*2.0f*3.1415f;
+//const double coeffD = ((double)(diametre/2)/(double)N)*2.0f*3.1415f;
+const double coeffG = 0.16008537;
+const double coeffD = 0.16059957;
+double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
+double x = 0, y = 0, O = 0;
 
-#define entraxe 245 // Distance entre les roues codeuses
-double xB = 0, yB = 0, phiB = 0; // Nouvelles coordonnées
-double xA = 0, yA = 0, phiA = 0; // Dernières coordonnées calculées
-double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
-double distG = 0, distD = 0; // Distance parcourue par chaque roue
+double vitG = 0, vitD = 0;
+#define tpsTicker 0.020f
 
-void odoGeonobot1()
+void odo2()
 {
-    xA = xB;
-    yA = yB;
-    phiA = phiB;
+    vitG = (double) ((comptG * coeffG) / tpsTicker);
+    vitD = (double) ((comptD * coeffD) / tpsTicker);
+
+    dDist = (double) ((comptG * coeffG) + (comptD * coeffD)) / 2.0f;
+    dAngl = (double) ((comptD * coeffD) - (comptG * coeffG)) / entraxe;
 
-    dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2;
-    dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl)) / entraxe;
+    x += (double) dDist * cos(O);
+    y += (double) dDist * sin(O);
+    O += (double) dAngl;
 
-    xB = xA + dDist * cos(phiA);
-    yB = yA + dDist * sin(phiA);
-    phiB = phiA + dAngl;
+    if (O > 3.1415) O = O - (2.0f * 3.1415f);
+    if (O < -3.1415) O = O + (2.0f * 3.1415f);
 
     comptG = 0;
     comptD = 0;
@@ -55,42 +84,152 @@
 //*/
 
 /*
-///// 2) ODOMÉTRIE Geonobot : Approximation par arc de cercle
-
-#define entraxe 245 // Distance entre les roues codeuses
-double xB = 0, yB = 0, phiB = 0; // Nouvelles coordonnées
-double xA = 0, yA = 0, phiA = 0; // Dernières coordonnées calculées
-double xO = 0, yO = 0; // Centre du cercle de rayon R
-double dDist = 0, dAngl = 0; // Distance moyenne du robot et orientation
-double distG = 0, distD = 0; // Distance parcourue par chaque roue
-double rayon = 0;
-
-void odoGeonobot2()
-{
-    xA = xB;
-    yA = yB;
-    phiA = phiB;
+// odo3()
+#define diametre 51.45
+#define N 1000
+#define entraxe 255
+const double coeffG = 1/5.956;
+const double coeffD = 1/5.956;
 
-    dDist = ((comptG / coeffGLong) + (comptD / coeffDLong)) / 2;
-    dAngl = ((comptD / coeffDAngl) - (comptG / coeffGAngl)) / entraxe;
-    rayon = dDist / dAngl;
-
-    xO = xA - R*sin(phiA);
-    yO = yA + R*cos(phiA);
-
-    phiB = phiA + dAngl;
+void odo3()
+{
+    dDist = (double) ((comptG * coeffG) + (comptD * coeffD)) / 2.0f;
+    dAngl = (double) ((comptD * coeffD) - (comptG * coeffG)) / entraxe;
 
-    if (dAngl == 0) {
-        xB = xO + R*sin(phiB);
-        yB = yO + R*cos(phiB);
-    }
-
-    else {
-        xB = xO + dDist*cos(phiB);
-        yB = yO + dDist*sin(phiB);
-    }
+    x += (double) dDist * cos(O);
+    y += (double) dDist * sin(O);
+    O += (double) dAngl;
 
     comptG = 0;
     comptD = 0;
 }
 */
+
+double distanceCible = 0;
+double xC = 0, yC = -100; // x = xR et y = yR
+double consigneOrientation = 0;
+//double consigneOrientation = (90*3.1415)/180;
+int signe = 1;
+int cmdD = 0, cmdG = 0;
+double erreurAngle = 0;
+double erreurPre = 0;
+double deltaErreur = 0;
+const double coeffPro = 15.0; // 5.0 de base
+const double coeffDer = 3.0;
+bool phase_acc = 1;
+
+void asserv()
+{
+    odo2();
+
+
+    distanceCible = sqrt((xC-x)*(xC-x)+(yC-y)*(yC-y));
+
+    /*
+    if(y > yC) {
+        signe = -1;
+    } else {
+        signe = 1;
+    }
+
+    consigneOrientation = signe * acos((xC-x)/((xC-x)*(xC-x)*(yC-y)*(yC-y)));
+    */
+
+
+    cmdD = abs((int)distanceCible);
+    if(cmdD>VMAX) {
+        cmdD = VMAX;
+    }
+    cmdG = cmdD;
+
+
+    /*
+    motGauche_fwd();
+    motDroite_fwd();
+    vitesseMotG(cmdG);
+    vitesseMotD(cmdD);
+    */
+
+    erreurAngle =  consigneOrientation - O;
+
+    deltaErreur = erreurAngle - erreurPre;
+
+    erreurPre  = erreurAngle;
+
+    int deltaCommande = coeffPro * erreurAngle + coeffDer * deltaErreur;
+
+    motGauche_bck();
+    motDroite_fwd();
+
+    cmdG = 0;
+    cmdD = 0;
+
+    cmdG += deltaCommande;
+    cmdD -= deltaCommande;
+
+    if(cmdD>VMAX) {
+        cmdD =VMAX;
+    } else if(cmdD < -VMAX) {
+        cmdD = -VMAX;
+    }
+
+    if(cmdG>VMAX) {
+        cmdG =VMAX;
+    } else if(cmdG < -VMAX) {
+        cmdG = -VMAX;
+    }
+
+    /*
+    if (cmdD > 0) {
+        //motDroite_fwd();
+        motDroite_bck();
+    } else {
+        //motDroite_bck();
+        motDroite_fwd();
+    }
+
+    if (cmdG > 0) {
+        motGauche_fwd();
+        //motGauche_bck();
+    } else {
+        motGauche_bck();
+        //motGauche_fwd();
+    }
+    */
+
+    /*
+    if(cmdD>150) {
+        cmdD =150;
+    } else if(cmdD < 0) {
+        cmdD = 0;
+    }
+
+    if(cmdG>150) {
+        cmdG = 150;
+    } else if(cmdG < 0) {
+        cmdG = 0;
+    }
+    */
+
+    /*
+    if (consigneOrientation - O < 3.1415) {
+        motGauche_bck();
+        motDroite_fwd();
+    } else {
+        motGauche_fwd();
+        motDroite_bck();
+    }
+    */
+
+    if (phase_acc) {
+        cmdG += 1;
+        cmdD = cmdG;
+
+        if (cmdG >= VMAX) phase_acc = 0;
+    }
+
+    vitesseMotG(abs(cmdG));
+    vitesseMotD(abs(cmdD));
+
+    if (O > (consigneOrientation - (2*0.0174533)) && O < (consigneOrientation + (2*0.0174533))) mot_dis();
+}