debugPC.cpp

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