Revision 16:ae65ce77b1f9, committed 2020-10-02

Comitter:

Nanaud

Date:

Fri Oct 02 21:09:45 2020 +0000

Parent:

14:dd3c756c6d48

Child:

17:176a1b4a2fa8

Commit message:

02/10/2020

Changed in this revision

--- a/debugPC.cpp	Wed Sep 16 12:31:54 2020 +0000
+++ b/debugPC.cpp	Fri Oct 02 21:09:45 2020 +0000
@@ -182,9 +182,11 @@
             pc.printf("Fonction test_drv()\n\r");
             bt.printf("Fonction test_drv()\n\r");
             //test_drv();
-            comptG = 0;
-            comptD = 0;
+            //comptG = 0;
+            //comptD = 0;
             //test1();
+            consigneOrientation = (90*3.1415)/180;
+            mot_en();
             break;
         case 9:     //cdon
             pc.printf("Results ALL Encoders ON/OFF\n\r");
@@ -275,10 +277,23 @@
             cmdType=3;
             break;
         case 20: // odom
+            pc.printf("Odometrie :\n\r");
+            pc.printf("x = %lf\n\r", x);
+            pc.printf("y = %lf\n\r", y);
+            pc.printf("O = %lf\n\r", O);
+            //pc.printf("O = %lf\n\r", (O*180)/_PI_);
+            pc.printf("vitG = %lf\n\r", vitG);
+            pc.printf("vitD = %lf\n\r", vitD);
+
             bt.printf("Odometrie :\n\r");
-            bt.printf("xB = %f\n\r", xB);
-            bt.printf("yB = %f\n\r", yB);
-            bt.printf("phiB = %f\n\r", phiB*180/_PI_);
+            bt.printf("x = %lf\n\r", x);
+            bt.printf("y = %lf\n\r", y);
+            bt.printf("O = %lf\n\r", O);
+            bt.printf("consigneAngl = %lf\n\r", consigneOrientation);
+            bt.printf("consigneDist = %lf\n\r", distanceCible);
+            //bt.printf("O = %lf\n\r", (O*180)/_PI_);
+            bt.printf("vitG = %lf\n\r", vitG);
+            bt.printf("vitD = %lf\n\r", vitD);
             break;
         default:
             pc.printf("Commande invalide\n\r");
@@ -294,12 +309,11 @@
     cmd2 += (cmd[2]-48)*1;
 
     if (cmd2>=0 && cmd2<=360) {
-        //consigneAngle = cmd2;
-        //bt.printf("\r\nCommande envoyee au robot : %lf\r\n",consigneAngle);
-        comptG=0; // Reset des compteurs
-        comptD=0;
-        //calculIntervalles();
-        //state=1;
+        consigneOrientation = (cmd2*3.1415)/180;
+        motGauche_bck();
+        motDroite_fwd();
+        mot_en();
+        bt.printf("\r\nCommande envoyee au robot : %lf\r\n",consigneOrientation);
     } else {
         pc.printf("\r\nAngle incorrect\r\n\n");
         bt.printf("\r\nAngle incorrect\r\n\n");
@@ -383,9 +397,11 @@
     if(aff_cd[1]) bt.printf("comptD = %d\n\r", comptD);
 }
 
+/*
 void affOdo()
 {
     if(aff_odo[0]) bt.printf("x = %f\n\r", xB);
     if(aff_odo[1]) bt.printf("y = %f\n\r", yB);
     if(aff_odo[2]) bt.printf("phi = %f\n\r", phiB*180/_PI_);
 }
+*/

--- a/main.cpp	Wed Sep 16 12:31:54 2020 +0000
+++ b/main.cpp	Fri Oct 02 21:09:45 2020 +0000
@@ -30,8 +30,9 @@
 
     //ticker_US.attach(&captUS_trig,0.2); // On apelle cette fonction toutes 0.2 secondes
     //ticker_affUS.attach(&affUltrasons,1.0);
-    ticker_affcd.attach(&affCodeurs,1.0);
-    //ticker_odo.attach(&odoGeonobot1,0.2);
+    //ticker_affcd.attach(&affCodeurs,1.0);
+    //ticker_odo.attach(&odo2,0.02);
+    //ticker_asserv.attach(&asserv,0.020);
     //ticker_affodo.attach(&affOdo,1.0);
 
     // Init capteurs à ultrasons
@@ -59,6 +60,6 @@
     cddA.mode(PullUp);
 
     while(1) {
-        
+        //odoGeonobot1();
     }
 }

--- a/motors.cpp	Wed Sep 16 12:31:54 2020 +0000
+++ b/motors.cpp	Fri Oct 02 21:09:45 2020 +0000
@@ -4,9 +4,11 @@
 void drv_init()
 {
     mot_dis();
-    motGauche_fwd();
-    //motDroite_fwd();
-    motDroite_bck();
+    motGauche_bck();
+    motDroite_fwd();
+
+    //motGauche_fwd();
+    //motDroite_bck();
     drvGauche.moveLinSpeed(0.01);
     drvDroite.moveLinSpeed(0.01);
     //mode = 0b111; // M0, M1 et M2 sont à 1
@@ -14,7 +16,7 @@
     mode_M0 = 1;
     //mode_M1 = 1;
     //mode_M2 = 1;
-    
+
     //mot_en();
 }
 
@@ -78,6 +80,16 @@
     //drvDroite.setDir(BACKWARD);
 }
 
+void vitesseMotG(int vitesse)
+{
+    drvGauche.moveLinSpeed((double)vitesse/1000);
+}
+
+void vitesseMotD(int vitesse)
+{
+    drvDroite.moveLinSpeed((double)vitesse/1000);
+}
+
 
 // FONCTIONS TESTS
 //

--- a/motors.h	Wed Sep 16 12:31:54 2020 +0000
+++ b/motors.h	Fri Oct 02 21:09:45 2020 +0000
@@ -13,6 +13,8 @@
 void motDroite_fwd();
 void motGauche_bck();
 void motDroite_bck();
+void vitesseMotG(int vitesse);
+void vitesseMotD(int vitesse);
 
 void test_drv();
 void testAngle(int cmdAngle);

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

--- a/odo_asserv.h	Wed Sep 16 12:31:54 2020 +0000
+++ b/odo_asserv.h	Fri Oct 02 21:09:45 2020 +0000
@@ -2,8 +2,16 @@
 
 // extern
 // CODEURS
+extern const double coeffGLong;
+extern const double coeffDLong;
+extern const double coeffGAngl;
+extern const double coeffDAngl;
 extern long comptG;
 extern long comptD;
+extern double distanceCible;
+extern double consigneOrientation;
+extern double vitG;
+extern double vitD;
 
 // Prototypes
 void cdgaRise();
@@ -11,8 +19,11 @@
 
 //ODOMETRIE
 extern Ticker ticker_odo;
-void odoGeonobot1();
-void odoGeonobot2();
-extern double xB;
-extern double yB;
-extern double phiB;
+extern Ticker ticker_asserv;
+void odo1();
+void odo2();
+void odo3();
+void asserv();
+extern double x;
+extern double y;
+extern double O;

--- a/pins.cpp	Wed Sep 16 12:31:54 2020 +0000
+++ b/pins.cpp	Fri Oct 02 21:09:45 2020 +0000
@@ -19,7 +19,7 @@
 #define DIR2 PC_3
 #define EN1 PA_15
 #define EN2 PA_14
-#define diametreRoue 72 //51.45
+#define diametreRoue 51.45
 #define rayonRoue (diametreRoue/2)
 #define nbPas 6400 //1000
 //BusOut mode(PB_7, PC_13, PC_14); // LSB ... MSB