Revision 3:3ba377aafdfd, committed 2020-07-15

Comitter:

Nanaud

Date:

Wed Jul 15 17:51:04 2020 +0000

Parent:

2:094c09903a9c

Child:

4:ad9b7355332e

Commit message:

Correction odometrie, Ajout fonction debug pour test de rotation;

Changed in this revision

--- a/DRV8825.lib	Thu Jul 09 21:03:01 2020 +0000
+++ b/DRV8825.lib	Wed Jul 15 17:51:04 2020 +0000
@@ -1,1 +1,1 @@
-https://os.mbed.com/teams/AresENSEA-CDF2020/code/DRV8825/#41e1779ef9e4
+https://os.mbed.com/teams/AresENSEA-CDF2020/code/DRV8825/#3615573bfe43

--- a/debug.cpp	Thu Jul 09 21:03:01 2020 +0000
+++ b/debug.cpp	Wed Jul 15 17:51:04 2020 +0000
@@ -7,66 +7,78 @@
 Ticker ticker_affcd;
 bool aff_US[6];
 bool aff_cd[4];
+int cmdType=0;
 
-void serialIT(){
+void serialIT()
+{
     //pc.printf("\n\rserialIT on\n\r");
     static int i=0;
     static char buffer[10]=""; // Tableau qui contient la chaine de caractère rentrée dans le terminal.
     static char cmd[Lcmd]=""; // Variable qui retient que les premiers caractères qui représentent la commande.
     char tampon = pc.getc();
-    
-    if((tampon >= 48 && tampon <=57) || (tampon>=97 && tampon<=122) || tampon==13)
-    {
+
+    if((tampon >= 48 && tampon <=57) || (tampon>=97 && tampon<=122) || tampon==13) {
         buffer[i]=tampon; // Ajout du caractère dans le tableau buffer
         pc.putc(buffer[i]); // Réécriture sur le terminal du caractère envoyé
         i++;
     }
 
-    if(buffer[i-1]=='\r'){ // Attente un appui sur la touche entrée
+    if(buffer[i-1]=='\r') { // Attente un appui sur la touche entrée
         i=0;
         copieTab(buffer,cmd); // Sauvegarde la commande dans le tableau cmd
-        cmdChoice(cmd); 
-    }  
+
+        switch(cmdType) {
+            case 1 : // Commande test angle
+                StringToAngle(cmd);
+                cmdType=0;
+                break;
+            default : // Commande par défaut
+                cmdChoice(cmd);
+        }
+    }
 }
 
-void copieTab(char *tab1,char *tab2){ // Fonction qui recopie un tableau dans un autre
+void copieTab(char *tab1,char *tab2)   // Fonction qui recopie un tableau dans un autre
+{
     //pc.printf("\n\rcopieTab on\n\r");
     //pc.printf("\n\r");
-    for(int j=0;j<Lcmd;j++){
+    for(int j=0; j<Lcmd; j++) {
         tab2[j]=tab1[j];
         //pc.printf("%c",tab2[j]);
     }
     //pc.printf("\n\r");
 }
 
-void cmdChoice(char *cmd){ // Fonction qui permet de choisir de l'activation d'une commande
+void cmdChoice(char *cmd)   // Fonction qui permet de choisir de l'activation d'une commande
+{
     //pc.printf("\n\rcmdChoice on\n\rcmd =");
-    
-    const char *options[]={
-    "help", //0
-    "usao", //1
-    "us1o", //2
-    "us2o", //3
-    "us3o", //4
-    "us4o", //5
-    "us5o", //6
-    "us6o", //7
-    "tdrv", //8
-    "cdon", //9
-    "cdga", //10
-    "cdgb", //11
-    0
+
+    const char *options[]= {
+        "help", //0
+        "usao", //1
+        "us1o", //2
+        "us2o", //3
+        "us3o", //4
+        "us4o", //5
+        "us5o", //6
+        "us6o", //7
+        "tdrv", //8
+        "cdon", //9
+        "cdga", //10
+        "cdgb", //11
+        "tagl", //12
+        0
     };
-    
+
     long option=-1;
-    
-    for (long a=0; options[a] && option<0; a++){
-        if (!strcmp(cmd,options[a]) || strcmp(cmd,options[a])==1){ // strcmp(cmd,options[a])==1 permet de contourner le problème, à revoir !!!
+
+    for (long a=0; options[a] && option<0; a++) {
+        if (!strcmp(cmd,options[a]) || strcmp(cmd,options[a])==1) { // strcmp(cmd,options[a])==1 permet de contourner le problème, à revoir !!!
             option=a;
         }
         //pc.printf("res = %d",strcmp(cmd,options[a]));
     }
-            
+
     switch (option) {
         case 0:     //help
             pc.printf("\n\n\r###HELP###\n\r");
@@ -75,12 +87,17 @@
             pc.printf("tdrv : Lance la fonction test_drv()\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");
             pc.printf("\n\r");
             break;
         case 1:     //usao
             pc.printf("Capt US ALL ON/OFF\n\r");
-            aff_US[0]=!aff_US[0];aff_US[1]=!aff_US[1];aff_US[2]=!aff_US[2];
-            aff_US[3]=!aff_US[3];aff_US[4]=!aff_US[4];aff_US[5]=!aff_US[5];
+            aff_US[0]=!aff_US[0];
+            aff_US[1]=!aff_US[1];
+            aff_US[2]=!aff_US[2];
+            aff_US[3]=!aff_US[3];
+            aff_US[4]=!aff_US[4];
+            aff_US[5]=!aff_US[5];
             break;
         case 2:     //us1o
             pc.printf("Capt US 1 ON/OFF\n\r");
@@ -123,23 +140,45 @@
             pc.printf("Results Encoder Left B ON/OFF\n\r");
             aff_cd[1]=!aff_cd[1];
             break;
+        case 12:    //tagl
+            pc.printf("Cmd Angle robot\r\n");
+            pc.printf("Angle (entre 0 et 360 degres sous la forme xxx): ");
+            cmdType=1;
+            break;
         default:
             pc.printf("Commande invalide\n\r");
     }
 }
 
-void affUltrasons(){
-    if(aff_US[0]) printf("Tps US1 = %5.0f uS\n\r", us_out[0]); 
-    if(aff_US[1]) printf("Tps US2 = %5.0f uS\n\r", us_out[1]); 
-    if(aff_US[2]) printf("Tps US3 = %5.0f uS\n\r", us_out[2]); 
-    if(aff_US[3]) printf("Tps US4 = %5.0f uS\n\r", us_out[3]); 
-    if(aff_US[4]) printf("Tps US5 = %5.0f uS\n\r", us_out[4]); 
-    if(aff_US[5]) printf("Tps US6 = %5.0f uS\n\r", us_out[5]);
-    if(aff_US[0]||aff_US[1]||aff_US[2]||aff_US[3]||aff_US[4]||aff_US[5]) printf("\n\r"); 
+void StringToAngle(char *cmd)
+{
+    int cmd2 = 0;
+    cmd2 += (cmd[0]-48)*100;
+    cmd2 += (cmd[1]-48)*10;
+    cmd2 += (cmd[2]-48)*1;
+
+    if (cmd2>=0 && cmd2<=360) {
+        pc.printf("\r\nCommande envoyee au robot : %d\r\n",cmd2);
+        testAngle(cmd2);
+    } else {
+        pc.printf("\r\nAngle incorrect\r\n\n");
+        cmd2 = 0;
+    }
 }
 
-void affCodeurs(){
-    if(aff_cd[0]) printf("CdgA = %d\n\r", cpt_cdgA);   
-    //if(aff_cd[1]) printf("CdgB = %d\n\r", cpt_cdgB); 
-}    
-    
\ No newline at end of file
+void affUltrasons()
+{
+    if(aff_US[0]) printf("Tps US1 = %5.0f uS\n\r", us_out[0]);
+    if(aff_US[1]) printf("Tps US2 = %5.0f uS\n\r", us_out[1]);
+    if(aff_US[2]) printf("Tps US3 = %5.0f uS\n\r", us_out[2]);
+    if(aff_US[3]) printf("Tps US4 = %5.0f uS\n\r", us_out[3]);
+    if(aff_US[4]) printf("Tps US5 = %5.0f uS\n\r", us_out[4]);
+    if(aff_US[5]) printf("Tps US6 = %5.0f uS\n\r", us_out[5]);
+    if(aff_US[0]||aff_US[1]||aff_US[2]||aff_US[3]||aff_US[4]||aff_US[5]) printf("\n\r");
+}
+
+void affCodeurs()
+{
+    if(aff_cd[0]) printf("CdgA = %d\n\r", cpt_cdgA);
+    //if(aff_cd[1]) printf("CdgB = %d\n\r", cpt_cdgB);
+}

--- a/debug.h	Thu Jul 09 21:03:01 2020 +0000
+++ b/debug.h	Wed Jul 15 17:51:04 2020 +0000
@@ -14,5 +14,7 @@
 void serialIT();
 void copieTab(char *tab1,char *tab2);
 void cmdChoice(char *cmd);
+
+void StringToAngle(char *cmd);
 void affUltrasons();
 void affCodeurs();
\ No newline at end of file

--- a/main.cpp	Thu Jul 09 21:03:01 2020 +0000
+++ b/main.cpp	Wed Jul 15 17:51:04 2020 +0000
@@ -35,7 +35,7 @@
     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);
-    aff_cd[0]=!aff_cd[0];
+    //aff_cd[0]=!aff_cd[0];
     
     while(1) {}
 }

--- a/motors.cpp	Thu Jul 09 21:03:01 2020 +0000
+++ b/motors.cpp	Wed Jul 15 17:51:04 2020 +0000
@@ -5,16 +5,6 @@
     mot_dis();
 }
 
-void test_drv(){
-    mot_en();
-    motGauche_fwd();
-    drvGauche.move(1);  
-    wait(2);
-    motGauche_bck();
-    wait(2);
-    mot_dis();
-}
-
 // ENABLE/DISABLE // Les deux modules ont le même enable
 void mot_en(){
     drvGauche.setEnable(START);
@@ -42,4 +32,20 @@
 
 void motDroite_bck(){
     drvDroite.setDir(BACKWARD);
+}
+
+// TESTS
+
+void test_drv(){
+    mot_en();
+    motGauche_fwd();
+    drvGauche.moveLinSpeed(1);  
+    wait(2);
+    motGauche_bck();
+    wait(2);
+    mot_dis();
+}
+
+void testAngle(int cmdAngle){
+    
 }
\ No newline at end of file

--- a/motors.h	Thu Jul 09 21:03:01 2020 +0000
+++ b/motors.h	Wed Jul 15 17:51:04 2020 +0000
@@ -2,11 +2,13 @@
 
 // Prototypes
 void drv_init();
-void test_drv();
 
 void mot_en();
 void mot_dis();
 void motGauche_fwd();
 void motDroite_fwd();
 void motGauche_bck();
-void motDroite_bck();
\ No newline at end of file
+void motDroite_bck();
+
+void test_drv();
+void testAngle(int cmdAngle);
\ No newline at end of file

--- a/odo_asserv.cpp	Thu Jul 09 21:03:01 2020 +0000
+++ b/odo_asserv.cpp	Wed Jul 15 17:51:04 2020 +0000
@@ -2,28 +2,28 @@
 #include "pins.h"
 #define Pi 3.14159265359
 
-#define ecart 120 // Distance en mm entre les deux roues motrices
+#define capt 1000 // Nb d'impulsions
+#define ecart 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)
 
 // Variables globales
 // cpt_cdgA est le compteur d'impulsion du codeur de gauche
 // cpt_cddA est le compteur d'impulsion du codeur de droite
-int posX, posY; // Position et orientation 
+float posX, posY; // Position et orientation 
 float theta; 
 
 void odometrie(){
 
-    int distG = (cpt_cdgA * perimetreRoueCodeuse) / 1000; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
-    int distD = (cpt_cddA * perimetreRoueCodeuse) / 1000; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
+    float distG = (cpt_cdgA * perimetreRoueCodeuse) / capt; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
+    float distD = (cpt_cddA * perimetreRoueCodeuse) / capt; // 1000 est le nombre d'impulsions par tour de la roue codeuse 
     
-    int distRobot = (distG + distD) / 2; // Distance parcourue par le robot (en mm)
-    int rayon = (ecart /2) * ((distD + distG) / (distD - distD)); // Trajectoire s'apparente à un cercle
+    float distRobot = (distG + distD) / 2; // Distance parcourue par le robot (en mm)
+    float rayon = (ecart /2) * ((distD + distG) / (distD - distD)); // Trajectoire s'apparente à un cercle
     
-    int dTheta = distRobot / rayon; // Changement d'orientation => Commande
-    
-    int posX0 = posX - rayon*cos(theta); 
-    int posY0 = posY - rayon*sin(theta);
+    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);