Revision 10:0714feaaaee1, committed 2020-09-11

Comitter:

Nanaud

Date:

Fri Sep 11 10:56:08 2020 +0000

Parent:

9:b3d4b3d51eb7

Child:

11:e62133022f88

Commit message:

11.09.20

Changed in this revision

--- a/debug.h	Thu Jul 30 19:43:16 2020 +0000
+++ b/debug.h	Fri Sep 11 10:56:08 2020 +0000
@@ -20,5 +20,6 @@
 
 void StringToAngle(char *cmd);
 void StringToVitesse(char *cmd);
+void StringToDist(char *cmd);
 void affUltrasons();
 void affCodeurs();

--- a/debugBT.cpp	Thu Jul 30 19:43:16 2020 +0000
+++ b/debugBT.cpp	Fri Sep 11 10:56:08 2020 +0000
@@ -36,6 +36,10 @@
                 StringToVitesse(cmd);
                 cmdType=0;
                 break;
+            case 3 :  // Commande distance a parcourir
+                StringToDist(cmd);
+                cmdType=0;
+                break;
             default : // Commande par défaut
                 cmdChoice(cmd);
         }

--- a/debugPC.cpp	Thu Jul 30 19:43:16 2020 +0000
+++ b/debugPC.cpp	Fri Sep 11 10:56:08 2020 +0000
@@ -9,7 +9,9 @@
 bool aff_cd[4];
 bool m_dis = true;    // 0 : START // 1 : STOP
 bool m_avance = false;   // 0 : BACKWARD // 1 : FORWARD
+bool mtrt_sens = false;
 int cmdType=0;
+long comptG_saved = 0, comptD_saved = 0; // Sauvegarde nombre de tics codeurs
 
 void serialIT() // avec Tera Term
 {
@@ -39,6 +41,10 @@
                 StringToVitesse(cmd);
                 cmdType=0;
                 break;
+            case 3 : // Commande distance a parcourir
+                StringToDist(cmd);
+                cmdType=0;
+                break;
             default : // Commande par défaut
                 cmdChoice(cmd);
         }
@@ -76,6 +82,9 @@
         "mtdr", //14
         "mtvt", //15
         "mtrt", //16
+        "cofr", //17
+        "cofa", //18
+        "parc", //19
         0
     };
 
@@ -94,7 +103,7 @@
             pc.printf("\n\n\r###HELP###\n\r");
             pc.printf("usao : Affichage resultats capteurs a ultrasons\n\r");
             pc.printf("usxo : Affichage resultat capteur ultrasons x\n\r");
-            pc.printf("tdrv : Lance la fonction test_drv()\n\r");
+            pc.printf("tdrv : Appelle une fonction test\n\r");
             pc.printf("cdon : Affichage resultats codeurs\n\r");
             pc.printf("cdgx : Affichage resultats codeur gauche phase x (a ou b)\n\r");
             pc.printf("tagl : Tourne le robot sur lui-meme avec un angle a preciser\r\n");
@@ -102,12 +111,15 @@
             pc.printf("mtdr : Changement de de sens de rotation des moteurs\r\n");
             pc.printf("mtvt : Changement de la vitesse de rotation (vitesse a preciser)\r\n");
             pc.printf("mtrt : Sens de rotation des moteurs opposes\r\n");
+            pc.printf("cofr : Enregistrement nb tics et reset\r\n");
+            pc.printf("cofa : Affichage nb tics\r\n");
+            pc.printf("parc : Distance a parcourir\r\n");
             pc.printf("\n\r");
             // bt
             bt.printf("\n\n\r###HELP###\n\r");
             bt.printf("usao : Affichage resultats capteurs a ultrasons\n\r");
             bt.printf("usxo : Affichage resultat capteur ultrasons x\n\r");
-            bt.printf("tdrv : Lance la fonction test_drv()\n\r");
+            bt.printf("tdrv : Appelle une fonction test\n\r");
             bt.printf("cdon : Affichage resultats codeurs\n\r");
             bt.printf("cdgx : Affichage resultats codeur gauche phase x (a ou b)\n\r");
             bt.printf("tagl : Tourne le robot sur lui-meme avec un angle a preciser\r\n");
@@ -115,6 +127,9 @@
             bt.printf("mtdr : Changement de de sens de rotation des moteurs\r\n");
             bt.printf("mtvt : Changement de la vitesse de rotation (vitesse a preciser)\r\n");
             bt.printf("mtrt : Sens de rotation des moteurs opposes\r\n");
+            bt.printf("cofr : Enregistrement nb tics et reset\r\n");
+            bt.printf("cofa : Affichage nb tics\r\n");
+            bt.printf("parc : Distance a parcourir\r\n");
             bt.printf("\n\r");
             break;
         case 1:     //usao
@@ -160,7 +175,10 @@
         case 8:     //tdrv
             pc.printf("Fonction test_drv()\n\r");
             bt.printf("Fonction test_drv()\n\r");
-            test_drv();
+            //test_drv();
+            comptG = 0;
+            comptD = 0;
+            //test1();
             break;
         case 9:     //cdon
             pc.printf("Results ALL Encoders ON/OFF\n\r");
@@ -220,11 +238,35 @@
             bt.printf("Vitesse en mm/s (sous la forme xxxx): ");
             cmdType=2;
             break;
-        case 16:
-            pc.printf("Rotation du robot");
-            bt.printf("Rotation du robot");
-            motGauche_fwd();
-            motDroite_bck();
+        case 16:    // mtrt
+            pc.printf("Rotation du robot - Changement de sens");
+            bt.printf("Rotation du robot - Changement de sens");
+            mtrt_sens = !mtrt_sens;
+            if(mtrt_sens) {
+                motGauche_fwd();
+                motDroite_bck();
+            } else {
+                motGauche_bck();
+                motDroite_fwd();
+            }
+            break;
+        case 17 :
+            comptG_saved = comptG;
+            comptD_saved = comptD;
+            comptG=0;
+            comptD=0;
+            pc.printf("comptG et comptD sauvegardes\r\n");
+            bt.printf("comptG et comptD sauvegardes\r\n");
+            break;
+        case 18 :
+            pc.printf("comptG_saved = %ld\r\n",comptG_saved);
+            pc.printf("comptD_saved = %ld\r\n",comptD_saved);
+            bt.printf("comptG_saved = %ld\r\n",comptG_saved);
+            bt.printf("comptD_saved = %ld\r\n",comptD_saved);
+            break;
+        case 19 : //
+            bt.printf("Distance a parcourir : ");
+            cmdType=3;
             break;
         default:
             pc.printf("Commande invalide\n\r");
@@ -240,9 +282,12 @@
     cmd2 += (cmd[2]-48)*1;
 
     if (cmd2>=0 && cmd2<=360) {
-        pc.printf("\r\nCommande envoyee au robot : %d\r\n",cmd2);
-        bt.printf("\r\nCommande envoyee au robot : %d\r\n",cmd2);
-        testAngle(cmd2);
+        //consigneAngle = cmd2;
+        //bt.printf("\r\nCommande envoyee au robot : %lf\r\n",consigneAngle);
+        comptG=0; // Reset des compteurs
+        comptD=0;
+        //calculIntervalles();
+        //state=1;
     } else {
         pc.printf("\r\nAngle incorrect\r\n\n");
         bt.printf("\r\nAngle incorrect\r\n\n");
@@ -250,6 +295,26 @@
     }
 }
 
+void StringToDist(char *cmd)
+{
+    int cmd4 = 0;
+    cmd4 += (cmd[0]-48)*1000;
+    cmd4 += (cmd[1]-48)*100;
+    cmd4 += (cmd[2]-48)*10;
+    cmd4 += (cmd[3]-48)*1;
+
+    if (cmd4>=0 && cmd4<=9999) {
+        //consigneDistance = cmd4;
+        //pc.printf("\r\nCommande envoyee au robot : %lf\r\n",consigneDistance);
+        //bt.printf("\r\nCommande envoyee au robot : %lf\r\n",consigneDistance);
+
+    } else {
+        pc.printf("\r\nDistance incorrecte\r\n\n");
+        bt.printf("\r\nDistance incorrecte\r\n\n");
+        cmd4 = 0;
+    }
+}
+
 void StringToVitesse(char *cmd)
 {
     for(int y=0; y<Lcmd ; y++) {

--- a/main.cpp	Thu Jul 30 19:43:16 2020 +0000
+++ b/main.cpp	Fri Sep 11 10:56:08 2020 +0000
@@ -27,9 +27,10 @@
     bt.baud(9600);
     bt.format(8,SerialBase::None,1);
 
-    ticker_US.attach(&captUS_trig,0.2); // On apelle cette fonction toutes 0.2 secondes
-    ticker_affUS.attach(&affUltrasons,1.0);
+    //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);
+    //tickTest.attach(&test4,0.1);
 
     // Init capteurs à ultrasons
     captUS_init();

--- a/motors.cpp	Thu Jul 30 19:43:16 2020 +0000
+++ b/motors.cpp	Fri Sep 11 10:56:08 2020 +0000
@@ -7,44 +7,75 @@
     motGauche_fwd();
     //motDroite_fwd();
     motDroite_bck();
-    drvGauche.moveLinSpeed(0.05);
-    drvDroite.moveLinSpeed(0.05);
-    mode = 0b111; // M0, M1 et M2 sont à 1
+    drvGauche.moveLinSpeed(0.01);
+    drvDroite.moveLinSpeed(0.01);
+    //mode = 0b111; // M0, M1 et M2 sont à 1
+
+    mode_M0 = 1;
+    //mode_M1 = 1;
+    //mode_M2 = 1;
+    
+    //mot_en();
 }
 
 // ENABLE/DISABLE // Les deux modules ont le même enable
+
 void mot_en()
 {
-    drvGauche.setEnable(START);
-    //drvDroite.setEnable(START);
+    motG_en();
+    motD_en();
 }
 
 void mot_dis()
 {
+    motG_dis();
+    motD_dis();
+}
+
+void motG_en()
+{
+    drvGauche.setEnable(START);
+}
+
+void motD_en()
+{
+    drvDroite.setEnable(START);
+}
+
+void motG_dis()
+{
     drvGauche.setEnable(STOP);
-    //drvDroite.setEnable(STOP);
+}
+
+void motD_dis()
+{
+    drvDroite.setEnable(STOP);
 }
 
 // FORWARD
 void motGauche_fwd()
 {
     drvGauche.setDir(FORWARD);
+    //drvGauche.setDir(BACKWARD);
 }
 
 void motDroite_fwd()
 {
     drvDroite.setDir(BACKWARD);
+    //drvDroite.setDir(FORWARD);
 }
 
 // BACKWARD
 void motGauche_bck()
 {
     drvGauche.setDir(BACKWARD);
+    //drvGauche.setDir(FORWARD);
 }
 
 void motDroite_bck()
 {
     drvDroite.setDir(FORWARD);
+    //drvDroite.setDir(BACKWARD);
 }
 
 
@@ -52,6 +83,7 @@
 //
 void test_drv()
 {
+    /*
     mot_en();
     motGauche_fwd();
     motDroite_fwd();
@@ -62,6 +94,49 @@
     motDroite_bck();
     wait(2);
     mot_dis();
+    */
+
+    /*
+    mot_en();
+    motGauche_fwd();
+    motDroite_fwd();
+    wait(2);
+    mot_dis();
+    */
+
+    mot_en();
+    mot_acc();
+    wait(2);
+    mot_dec();
+    mot_dis();
+
+    drvDroite.moveLinSpeed(0.050);
 }
 
-void testAngle(int cmdAngle){}
+void mot_acc()
+{
+    double i = 0;
+
+    while (i < vitesseSAT) {
+        bt.printf("mot_acc() => i = %lf\r\n",i);
+        drvDroite.moveLinSpeed(i);
+        drvGauche.moveLinSpeed(i);
+        i+=0.005;
+        wait_ms(10);
+    }
+}
+
+void mot_dec()
+{
+    double i = vitesseSAT;
+
+    while (i > 0) {
+        bt.printf("mot_dec() => i = %lf\r\n",i);
+        drvDroite.moveLinSpeed(i);
+        drvGauche.moveLinSpeed(i);
+        i-=0.005;
+        wait_ms(10);
+    }
+}
+
+void testAngle(int cmdAngle) {}

--- a/motors.h	Thu Jul 30 19:43:16 2020 +0000
+++ b/motors.h	Fri Sep 11 10:56:08 2020 +0000
@@ -5,6 +5,10 @@
 
 void mot_en();
 void mot_dis();
+void motG_en();
+void motD_en();
+void motG_dis();
+void motD_dis();
 void motGauche_fwd();
 void motDroite_fwd();
 void motGauche_bck();
@@ -12,5 +16,7 @@
 
 void test_drv();
 void testAngle(int cmdAngle);
-double mot_acc(double vit,double distanceParcourue, double distAfaire);
-double mot_dec(double vit,double distanceParcourue, double distAfaire);
\ No newline at end of file
+//double mot_acc(double vit,double distanceParcourue, double distAfaire);
+//double mot_dec(double vit,double distanceParcourue, double distAfaire);
+void mot_acc();
+void mot_dec();

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

--- a/odo_asserv.h	Thu Jul 30 19:43:16 2020 +0000
+++ b/odo_asserv.h	Fri Sep 11 10:56:08 2020 +0000
@@ -2,12 +2,9 @@
 
 // extern
 // CODEURS
-//extern long cpt_cdgA;
-//extern long cpt_cddA;
 extern long comptG;
 extern long comptD;
 
-
 // Prototypes
 void cdgaRise();
-void cddaRise();
+void cddaRise();
\ No newline at end of file

--- a/pins.cpp	Thu Jul 30 19:43:16 2020 +0000
+++ b/pins.cpp	Fri Sep 11 10:56:08 2020 +0000
@@ -16,14 +16,18 @@
 #define STEP1 PA_6
 #define STEP2 PB_6
 #define DIR1 PC_2
-#define DIR2 PH_1
-#define EN PC_3
+#define DIR2 PC_3
+#define EN1 PA_15
+#define EN2 PA_14
 #define diametreRoue 72 //51.45
 #define rayonRoue (diametreRoue/2)
 #define nbPas 6400 //1000
-BusOut mode(PB_0, PC_1, PC_0); // LSB ... MSB
-DRV8825 drvGauche(EN,DIR1,STEP1,rayonRoue,nbPas); 
-DRV8825 drvDroite(EN,DIR2,STEP2,rayonRoue,nbPas); 
+//BusOut mode(PB_7, PC_13, PC_14); // LSB ... MSB
+DigitalOut mode_M0(PB_7);
+//DigitalOut mode_M1(PC_13); // Inutile
+//DigitalOut mode_M2(PC_14); // Hardware
+DRV8825 drvGauche(EN1,DIR1,STEP1,rayonRoue,nbPas); 
+DRV8825 drvDroite(EN2,DIR2,STEP2,rayonRoue,nbPas); 
 
 // Codeurs (Réf : LPJ3806-1000BM-G5-24E)
 InterruptIn cdgA(PA_8); // Codeur de gauche

--- a/pins.h	Thu Jul 30 19:43:16 2020 +0000
+++ b/pins.h	Fri Sep 11 10:56:08 2020 +0000
@@ -11,11 +11,11 @@
 
 // #define
 #define Pi 3.14159265359
-#define NbPulseCodeur 1000
-#define ecartCodeuse 245 // Distance en mm entre les deux roues codeuses
+//#define NbPulseCodeur 1000
+//#define ecartCodeuse 245 // Distance en mm entre les deux roues codeuses
 #define diametreRoueCodeuse 51.450 // Diamètre de la roue codeuse en mm
 #define perimetreRoueCodeuse (diametreRoueCodeuse * Pi)
-#define vtSAT 0.250 // m/s 
+#define vitesseSAT 0.250 // m/s 
 #define perimetreE Pi*ecartCodeuse
 
 extern InterruptIn btn;
@@ -30,7 +30,10 @@
 extern InterruptIn echo6;
 
 // Drivers DRV8825 
-extern BusOut mode;
+//extern BusOut mode;
+extern DigitalOut mode_M0;
+//extern DigitalOut mode_M1;
+//extern DigitalOut mode_M2;
 extern DRV8825 drvGauche; 
 extern DRV8825 drvDroite;