debugPC.cpp

00001 // Nom du fichier : debugPC.cpp
00002 #include "pins.h"
00003 
00004 // Variables globales & timers
00005 Serial pc(USBTX, USBRX);
00006 Ticker ticker_affUS;
00007 Ticker ticker_affcd;
00008 Ticker ticker_affodo;
00009 
00010 bool aff_US[6];
00011 bool aff_cd[4];
00012 bool aff_odo[3];
00013 bool m_dis = true;    // 0 : START // 1 : STOP
00014 bool m_avance = false;   // 0 : BACKWARD // 1 : FORWARD
00015 bool mtrt_sens = false;
00016 int cmdType=0;
00017 long comptG_saved = 0, comptD_saved = 0; // Sauvegarde nombre de tics codeurs
00018 
00019 void serialIT() // avec Tera Term
00020 {
00021     //pc.printf("\n\rserialIT on\n\r");
00022     static int i=0;
00023     static char buffer[10]=""; // Tableau qui contient la chaine de caractère rentrée dans le terminal.
00024     static char cmd[Lcmd]=""; // Variable qui retient que les premiers caractères qui représentent la commande.
00025 
00026     char tampon = pc.getc();
00027 
00028     if((tampon >= 48 && tampon <=57) || (tampon>=97 && tampon<=122) || tampon==13) {
00029         buffer[i]=tampon; // Ajout du caractère dans le tableau buffer
00030         pc.putc(buffer[i]); // Réécriture sur le terminal du caractère envoyé
00031         i++;
00032     }
00033 
00034     if(buffer[i-1]=='\r') { // Attente un appui sur la touche entrée
00035         i=0;
00036         copieTab(buffer,cmd); // Sauvegarde la commande dans le tableau cmd
00037 
00038         switch(cmdType) {
00039             case 1 : // Commande test angle
00040                 StringToAngle(cmd);
00041                 cmdType=0;
00042                 break;
00043             case 2 : // Commande test vitesse
00044                 StringToVitesse(cmd);
00045                 cmdType=0;
00046                 break;
00047             case 3 : // Commande distance a parcourir
00048                 StringToDist(cmd);
00049                 cmdType=0;
00050                 break;
00051             default : // Commande par défaut
00052                 cmdChoice(cmd);
00053         }
00054     }
00055 }
00056 
00057 void copieTab(char *tab1,char *tab2)   // Fonction qui recopie un tableau dans un autre
00058 {
00059     //pc.printf("\n\rcopieTab on\n\r");
00060     //pc.printf("\n\r");
00061     for(int j=0; j<Lcmd; j++) {
00062         tab2[j]=tab1[j];
00063         //pc.printf("%c",tab2[j]);
00064     }
00065     //pc.printf("\n\r");
00066 }
00067 
00068 void cmdChoice(char *cmd)   // Fonction qui permet de choisir de l'activation d'une commande
00069 {
00070     const char *options[]= {
00071         "help", //0
00072         "usao", //1
00073         "us1o", //2
00074         "us2o", //3
00075         "us3o", //4
00076         "us4o", //5
00077         "us5o", //6
00078         "us6o", //7
00079         "tdrv", //8
00080         "cdon", //9
00081         "cdga", //10
00082         "cdgb", //11
00083         "tagl", //12
00084         "mten", //13
00085         "mtdr", //14
00086         "mtvt", //15
00087         "mtrt", //16
00088         "cofr", //17
00089         "cofa", //18
00090         "parc", //19
00091         "odom", //20
00092         0
00093     };
00094 
00095     long option=-1;
00096 
00097     for (long a=0; options[a] && option<0; a++) {
00098         if (!strcmp(cmd,options[a]) || strcmp(cmd,options[a])==1) { // strcmp(cmd,options[a])==1 permet de contourner le problème, à revoir !!!
00099             option=a;
00100         }
00101         //pc.printf("res = %d\r\n",strcmp(cmd,options[a]));
00102     }
00103 
00104     switch (option) {
00105         case 0:     //help
00106             // pc
00107             pc.printf("\n\n\r###HELP###\n\r");
00108             pc.printf("usao : Affichage resultats capteurs a ultrasons\n\r");
00109             pc.printf("usxo : Affichage resultat capteur ultrasons x\n\r");
00110             pc.printf("tdrv : Appelle une fonction test\n\r");
00111             pc.printf("cdon : Affichage resultats codeurs\n\r");
00112             pc.printf("cdgx : Affichage resultats codeur gauche phase x (a ou b)\n\r");
00113             pc.printf("tagl : Tourne le robot sur lui-meme avec un angle a preciser\r\n");
00114             pc.printf("mten : Activation/Desactivation des moteurs\r\n");
00115             pc.printf("mtdr : Changement de de sens de rotation des moteurs\r\n");
00116             pc.printf("mtvt : Changement de la vitesse de rotation (vitesse a preciser)\r\n");
00117             pc.printf("mtrt : Sens de rotation des moteurs opposes\r\n");
00118             pc.printf("cofr : Enregistrement nb tics et reset\r\n");
00119             pc.printf("cofa : Affichage nb tics\r\n");
00120             pc.printf("parc : Distance a parcourir\r\n");
00121             pc.printf("odom : Afficher les valeurs pour l'odometrie\r\n");
00122             pc.printf("\n\r");
00123             // bt
00124             bt.printf("\n\n\r###HELP###\n\r");
00125             bt.printf("usao : Affichage resultats capteurs a ultrasons\n\r");
00126             bt.printf("usxo : Affichage resultat capteur ultrasons x\n\r");
00127             bt.printf("tdrv : Appelle une fonction test\n\r");
00128             bt.printf("cdon : Affichage resultats codeurs\n\r");
00129             bt.printf("cdgx : Affichage resultats codeur gauche phase x (a ou b)\n\r");
00130             bt.printf("tagl : Tourne le robot sur lui-meme avec un angle a preciser\r\n");
00131             bt.printf("mten : Activation/Desactivation des moteurs\r\n");
00132             bt.printf("mtdr : Changement de de sens de rotation des moteurs\r\n");
00133             bt.printf("mtvt : Changement de la vitesse de rotation (vitesse a preciser)\r\n");
00134             bt.printf("mtrt : Sens de rotation des moteurs opposes\r\n");
00135             bt.printf("cofr : Enregistrement nb tics et reset\r\n");
00136             bt.printf("cofa : Affichage nb tics\r\n");
00137             bt.printf("parc : Distance a parcourir\r\n");
00138             bt.printf("odom : Afficher les valeurs pour l'odometrie\r\n");
00139             bt.printf("\n\r");
00140             break;
00141         case 1:     //usao
00142             pc.printf("Capt US ALL ON/OFF\n\r");
00143             bt.printf("Capt US ALL ON/OFF\n\r");
00144             aff_US[0]=!aff_US[0];
00145             aff_US[1]=!aff_US[1];
00146             aff_US[2]=!aff_US[2];
00147             aff_US[3]=!aff_US[3];
00148             aff_US[4]=!aff_US[4];
00149             aff_US[5]=!aff_US[5];
00150             break;
00151         case 2:     //us1o
00152             pc.printf("Capt US 1 ON/OFF\n\r");
00153             bt.printf("Capt US 1 ON/OFF\n\r");
00154             aff_US[0]=!aff_US[0];
00155             break;
00156         case 3:     //us2o
00157             pc.printf("Capt US 2 ON/OFF\n\r");
00158             bt.printf("Capt US 2 ON/OFF\n\r");
00159             aff_US[1]=!aff_US[1];
00160             break;
00161         case 4:     //us3o
00162             pc.printf("Capt US 3 ON/OFF\n\r");
00163             bt.printf("Capt US 3 ON/OFF\n\r");
00164             aff_US[2]=!aff_US[2];
00165             break;
00166         case 5:     //us4o
00167             pc.printf("Capt US 4 ON/OFF\n\r");
00168             bt.printf("Capt US 4 ON/OFF\n\r");
00169             aff_US[3]=!aff_US[3];
00170             break;
00171         case 6:     //us5o
00172             pc.printf("Capt US 5 ON/OFF\n\r");
00173             bt.printf("Capt US 5 ON/OFF\n\r");
00174             aff_US[4]=!aff_US[4];
00175             break;
00176         case 7:     //us6o
00177             pc.printf("Capt US 6 ON/OFF\n\r");
00178             bt.printf("Capt US 6 ON/OFF\n\r");
00179             aff_US[5]=!aff_US[5];
00180             break;
00181         case 8:     //tdrv
00182             pc.printf("Fonction test_drv()\n\r");
00183             bt.printf("Fonction test_drv()\n\r");
00184             //test_drv();
00185             comptG = 0;
00186             comptD = 0;
00187             //test1();
00188             break;
00189         case 9:     //cdon
00190             pc.printf("Results ALL Encoders ON/OFF\n\r");
00191             bt.printf("Results ALL Encoders ON/OFF\n\r");
00192             aff_cd[0]=!aff_cd[0];
00193             aff_cd[1]=!aff_cd[1];
00194             break;
00195         case 10:    //cdga
00196             pc.printf("Results Encoder Left A ON/OFF\n\r");
00197             bt.printf("Results Encoder Left A ON/OFF\n\r");
00198             aff_cd[0]=!aff_cd[0];
00199             break;
00200         case 11:    //cdgb
00201             pc.printf("Results Encoder Left B ON/OFF\n\r");
00202             bt.printf("Results Encoder Left B ON/OFF\n\r");
00203             aff_cd[1]=!aff_cd[1];
00204             break;
00205         case 12:    //tagl
00206             pc.printf("Cmd Angle robot\r\n");
00207             pc.printf("Angle (entre 0 et 360 degres sous la forme xxx): ");
00208             bt.printf("Cmd Angle robot\r\n");
00209             bt.printf("Angle (entre 0 et 360 degres sous la forme xxx): ");
00210             cmdType=1;
00211             break;
00212         case 13:    // mten
00213             if(m_dis) {
00214                 mot_en();
00215                 m_dis=!m_dis;
00216                 pc.printf("Motors Enable\r\n");
00217                 bt.printf("Motors Enable\r\n");
00218             } else {
00219                 mot_dis();
00220                 m_dis=!m_dis;
00221                 pc.printf("Motors Disable\r\n");
00222                 bt.printf("Motors Disable\r\n");
00223             }
00224             break;
00225         case 14:    // mtdr
00226             if(m_avance) {
00227                 pc.printf("Changement de direction : En avant\r\n");
00228                 bt.printf("Changement de direction : En avant\r\n");
00229                 motGauche_fwd();
00230                 motDroite_fwd();
00231                 m_avance=!m_avance;
00232             } else {
00233                 pc.printf("Changement de direction : En arriere\r\n");
00234                 bt.printf("Changement de direction : En arriere\r\n");
00235                 motGauche_bck();
00236                 motDroite_bck();
00237                 m_avance=!m_avance;
00238             }
00239             break;
00240         case 15:    // mtvt
00241             pc.printf("Cmd Vitesse robot\r\n");
00242             pc.printf("Vitesse en mm/s (sous la forme xxxx): ");
00243             bt.printf("Cmd Vitesse robot\r\n");
00244             bt.printf("Vitesse en mm/s (sous la forme xxxx): ");
00245             cmdType=2;
00246             break;
00247         case 16:    // mtrt
00248             pc.printf("Rotation du robot - Changement de sens");
00249             bt.printf("Rotation du robot - Changement de sens");
00250             mtrt_sens = !mtrt_sens;
00251             if(mtrt_sens) {
00252                 motGauche_fwd();
00253                 motDroite_bck();
00254             } else {
00255                 motGauche_bck();
00256                 motDroite_fwd();
00257             }
00258             break;
00259         case 17 :
00260             comptG_saved = comptG;
00261             comptD_saved = comptD;
00262             comptG=0;
00263             comptD=0;
00264             pc.printf("comptG et comptD sauvegardes\r\n");
00265             bt.printf("comptG et comptD sauvegardes\r\n");
00266             break;
00267         case 18 :
00268             pc.printf("comptG_saved = %ld\r\n",comptG_saved);
00269             pc.printf("comptD_saved = %ld\r\n",comptD_saved);
00270             bt.printf("comptG_saved = %ld\r\n",comptG_saved);
00271             bt.printf("comptD_saved = %ld\r\n",comptD_saved);
00272             break;
00273         case 19 : //
00274             bt.printf("Distance a parcourir : ");
00275             cmdType=3;
00276             break;
00277         case 20: // odom
00278             bt.printf("Odometrie :\n\r");
00279             bt.printf("xB = %f\n\r", xB);
00280             bt.printf("yB = %f\n\r", yB);
00281             bt.printf("phiB = %f\n\r", phiB*180/_PI_);
00282             break;
00283         default:
00284             pc.printf("Commande invalide\n\r");
00285             bt.printf("Commande invalide\n\r");
00286     }
00287 }
00288 
00289 void StringToAngle(char *cmd)
00290 {
00291     int cmd2 = 0;
00292     cmd2 += (cmd[0]-48)*100;
00293     cmd2 += (cmd[1]-48)*10;
00294     cmd2 += (cmd[2]-48)*1;
00295 
00296     if (cmd2>=0 && cmd2<=360) {
00297         //consigneAngle = cmd2;
00298         //bt.printf("\r\nCommande envoyee au robot : %lf\r\n",consigneAngle);
00299         comptG=0; // Reset des compteurs
00300         comptD=0;
00301         //calculIntervalles();
00302         //state=1;
00303     } else {
00304         pc.printf("\r\nAngle incorrect\r\n\n");
00305         bt.printf("\r\nAngle incorrect\r\n\n");
00306         cmd2 = 0;
00307     }
00308 }
00309 
00310 void StringToDist(char *cmd)
00311 {
00312     int cmd4 = 0;
00313     cmd4 += (cmd[0]-48)*1000;
00314     cmd4 += (cmd[1]-48)*100;
00315     cmd4 += (cmd[2]-48)*10;
00316     cmd4 += (cmd[3]-48)*1;
00317 
00318     if (cmd4>=0 && cmd4<=9999) {
00319         //consigneDistance = cmd4;
00320         //pc.printf("\r\nCommande envoyee au robot : %lf\r\n",consigneDistance);
00321         //bt.printf("\r\nCommande envoyee au robot : %lf\r\n",consigneDistance);
00322 
00323     } else {
00324         pc.printf("\r\nDistance incorrecte\r\n\n");
00325         bt.printf("\r\nDistance incorrecte\r\n\n");
00326         cmd4 = 0;
00327     }
00328 }
00329 
00330 void StringToVitesse(char *cmd)
00331 {
00332     for(int y=0; y<Lcmd ; y++) {
00333         bt.printf("%d ",cmd[y]);
00334     }
00335     bt.printf("\r\n");
00336 
00337     float cmd3 = 0;
00338     cmd3 += (cmd[0]-48)*1000;
00339     cmd3 += (cmd[1]-48)*100;
00340     cmd3 += (cmd[2]-48)*10;
00341     cmd3 += (cmd[3]-48)*1;
00342 
00343     if (cmd3>=0 && cmd3<=9999) {
00344         cmd3 = cmd3 / 1000;
00345         pc.printf("\r\nCommande envoyee au robot : %.3f m/s\r\n",cmd3);
00346         bt.printf("\r\nCommande envoyee au robot : %.3f m/s\r\n",cmd3);
00347         drvGauche.moveLinSpeed(cmd3);
00348         drvDroite.moveLinSpeed(cmd3);
00349     } else {
00350         pc.printf("\r\nVitesse incorrect\r\n\n");
00351         bt.printf("\r\nVitesse incorrect\r\n\n");
00352         cmd3 = 0;
00353     }
00354 }
00355 
00356 void affUltrasons()
00357 {
00358     // pc
00359     if(aff_US[0]) pc.printf("Tps US1 = %5.0f uS\n\r", us_out[0]);
00360     if(aff_US[1]) pc.printf("Tps US2 = %5.0f uS\n\r", us_out[1]);
00361     if(aff_US[2]) pc.printf("Tps US3 = %5.0f uS\n\r", us_out[2]);
00362     if(aff_US[3]) pc.printf("Tps US4 = %5.0f uS\n\r", us_out[3]);
00363     if(aff_US[4]) pc.printf("Tps US5 = %5.0f uS\n\r", us_out[4]);
00364     if(aff_US[5]) pc.printf("Tps US6 = %5.0f uS\n\r", us_out[5]);
00365     if(aff_US[0]||aff_US[1]||aff_US[2]||aff_US[3]||aff_US[4]||aff_US[5]) pc.printf("\n\r");
00366 
00367     // bt
00368     if(aff_US[0]) bt.printf("Tps US1 = %5.0f uS\n\r", us_out[0]);
00369     if(aff_US[1]) bt.printf("Tps US2 = %5.0f uS\n\r", us_out[1]);
00370     if(aff_US[2]) bt.printf("Tps US3 = %5.0f uS\n\r", us_out[2]);
00371     if(aff_US[3]) bt.printf("Tps US4 = %5.0f uS\n\r", us_out[3]);
00372     if(aff_US[4]) bt.printf("Tps US5 = %5.0f uS\n\r", us_out[4]);
00373     if(aff_US[5]) bt.printf("Tps US6 = %5.0f uS\n\r", us_out[5]);
00374     if(aff_US[0]||aff_US[1]||aff_US[2]||aff_US[3]||aff_US[4]||aff_US[5]) bt.printf("\n\r");
00375 }
00376 
00377 void affCodeurs()
00378 {
00379     if(aff_cd[0]) pc.printf("comptG = %d\n\r", comptG);
00380     if(aff_cd[1]) pc.printf("comptD = %d\n\r", comptD);
00381 
00382     if(aff_cd[0]) bt.printf("comptG = %d\n\r", comptG);
00383     if(aff_cd[1]) bt.printf("comptD = %d\n\r", comptD);
00384 }
00385 
00386 void affOdo()
00387 {
00388     if(aff_odo[0]) bt.printf("x = %f\n\r", xB);
00389     if(aff_odo[1]) bt.printf("y = %f\n\r", yB);
00390     if(aff_odo[2]) bt.printf("phi = %f\n\r", phiB*180/_PI_);
00391 }