AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode

Code principal du projet

Dependencies:   mbed DRV8825

debugPC.cpp@26:bb2b778bd351, 2020-11-03 (annotated)

Committer:
Nanaud
Date:
Tue Nov 03 16:29:13 2020 +0000
Revision:
26:bb2b778bd351
Parent:
25:869b1c1f51a7 
UI

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Nanaud 4:ad9b7355332e 1 // Nom du fichier : debugPC.cpp
Nanaud 0:dc036b67c87c 2 #include "pins.h"
Nanaud 0:dc036b67c87c 3
Nanaud 22:f891c2bce091 4 // Variables globales
Nanaud 1:2fe8c402ee79 5 Serial pc(USBTX, USBRX);
Nanaud 22:f891c2bce091 6 Ticker Ticker_affUS;
plmir 12:2c312916a621 7
Nanaud 1:2fe8c402ee79 8 bool aff_US[6];
Nanaud 1:2fe8c402ee79 9 bool aff_cd[4];
plmir 12:2c312916a621 10 bool aff_odo[3];
Nanaud 5:34ed652f8c31 11 bool m_dis = true; // 0 : START // 1 : STOP
Nanaud 5:34ed652f8c31 12 bool m_avance = false; // 0 : BACKWARD // 1 : FORWARD
Nanaud 10:0714feaaaee1 13 bool mtrt_sens = false;
Nanaud 3:3ba377aafdfd 14 int cmdType=0;
Nanaud 10:0714feaaaee1 15 long comptG_saved = 0, comptD_saved = 0; // Sauvegarde nombre de tics codeurs
Nanaud 1:2fe8c402ee79 16
Nanaud 4:ad9b7355332e 17 void serialIT() // avec Tera Term
Nanaud 3:3ba377aafdfd 18 {
Nanaud 0:dc036b67c87c 19 //pc.printf("\n\rserialIT on\n\r");
Nanaud 0:dc036b67c87c 20 static int i=0;
Nanaud 0:dc036b67c87c 21 static char buffer[10]=""; // Tableau qui contient la chaine de caractère rentrée dans le terminal.
Nanaud 0:dc036b67c87c 22 static char cmd[Lcmd]=""; // Variable qui retient que les premiers caractères qui représentent la commande.
Nanaud 4:ad9b7355332e 23
Nanaud 0:dc036b67c87c 24 char tampon = pc.getc();
Nanaud 3:3ba377aafdfd 25
Nanaud 3:3ba377aafdfd 26 if((tampon >= 48 && tampon <=57) || (tampon>=97 && tampon<=122) || tampon==13) {
Nanaud 0:dc036b67c87c 27 buffer[i]=tampon; // Ajout du caractère dans le tableau buffer
Nanaud 0:dc036b67c87c 28 pc.putc(buffer[i]); // Réécriture sur le terminal du caractère envoyé
Nanaud 0:dc036b67c87c 29 i++;
Nanaud 0:dc036b67c87c 30 }
Nanaud 0:dc036b67c87c 31
Nanaud 3:3ba377aafdfd 32 if(buffer[i-1]=='\r') { // Attente un appui sur la touche entrée
Nanaud 0:dc036b67c87c 33 i=0;
Nanaud 0:dc036b67c87c 34 copieTab(buffer,cmd); // Sauvegarde la commande dans le tableau cmd
Nanaud 3:3ba377aafdfd 35
Nanaud 3:3ba377aafdfd 36 switch(cmdType) {
Nanaud 3:3ba377aafdfd 37 case 1 : // Commande test angle
Nanaud 3:3ba377aafdfd 38 StringToAngle(cmd);
Nanaud 3:3ba377aafdfd 39 cmdType=0;
Nanaud 3:3ba377aafdfd 40 break;
Nanaud 5:34ed652f8c31 41 case 2 : // Commande test vitesse
Nanaud 5:34ed652f8c31 42 StringToVitesse(cmd);
Nanaud 5:34ed652f8c31 43 cmdType=0;
Nanaud 5:34ed652f8c31 44 break;
Nanaud 10:0714feaaaee1 45 case 3 : // Commande distance a parcourir
Nanaud 10:0714feaaaee1 46 StringToDist(cmd);
Nanaud 10:0714feaaaee1 47 cmdType=0;
Nanaud 10:0714feaaaee1 48 break;
Nanaud 3:3ba377aafdfd 49 default : // Commande par défaut
Nanaud 3:3ba377aafdfd 50 cmdChoice(cmd);
Nanaud 3:3ba377aafdfd 51 }
Nanaud 3:3ba377aafdfd 52 }
Nanaud 0:dc036b67c87c 53 }
Nanaud 0:dc036b67c87c 54
Nanaud 3:3ba377aafdfd 55 void copieTab(char *tab1,char *tab2) // Fonction qui recopie un tableau dans un autre
Nanaud 3:3ba377aafdfd 56 {
Nanaud 0:dc036b67c87c 57 //pc.printf("\n\rcopieTab on\n\r");
Nanaud 0:dc036b67c87c 58 //pc.printf("\n\r");
Nanaud 3:3ba377aafdfd 59 for(int j=0; j<Lcmd; j++) {
Nanaud 0:dc036b67c87c 60 tab2[j]=tab1[j];
Nanaud 0:dc036b67c87c 61 //pc.printf("%c",tab2[j]);
Nanaud 0:dc036b67c87c 62 }
Nanaud 0:dc036b67c87c 63 //pc.printf("\n\r");
Nanaud 0:dc036b67c87c 64 }
Nanaud 0:dc036b67c87c 65
Nanaud 3:3ba377aafdfd 66 void cmdChoice(char *cmd) // Fonction qui permet de choisir de l'activation d'une commande
Nanaud 3:3ba377aafdfd 67 {
Nanaud 3:3ba377aafdfd 68 const char *options[]= {
Nanaud 3:3ba377aafdfd 69 "help", //0
Nanaud 3:3ba377aafdfd 70 "usao", //1
Nanaud 3:3ba377aafdfd 71 "us1o", //2
Nanaud 3:3ba377aafdfd 72 "us2o", //3
Nanaud 3:3ba377aafdfd 73 "us3o", //4
Nanaud 3:3ba377aafdfd 74 "us4o", //5
Nanaud 3:3ba377aafdfd 75 "us5o", //6
Nanaud 3:3ba377aafdfd 76 "us6o", //7
Nanaud 3:3ba377aafdfd 77 "tdrv", //8
Nanaud 3:3ba377aafdfd 78 "cdon", //9
Nanaud 3:3ba377aafdfd 79 "cdga", //10
Nanaud 3:3ba377aafdfd 80 "cdgb", //11
Nanaud 3:3ba377aafdfd 81 "tagl", //12
Nanaud 5:34ed652f8c31 82 "mten", //13
Nanaud 5:34ed652f8c31 83 "mtdr", //14
Nanaud 5:34ed652f8c31 84 "mtvt", //15
Nanaud 8:eb9bbab312ff 85 "mtrt", //16
Nanaud 10:0714feaaaee1 86 "cofr", //17
Nanaud 10:0714feaaaee1 87 "cofa", //18
Nanaud 10:0714feaaaee1 88 "parc", //19
plmir 12:2c312916a621 89 "odom", //20
Nanaud 3:3ba377aafdfd 90 0
Nanaud 0:dc036b67c87c 91 };
Nanaud 3:3ba377aafdfd 92
Nanaud 0:dc036b67c87c 93 long option=-1;
Nanaud 3:3ba377aafdfd 94
Nanaud 3:3ba377aafdfd 95 for (long a=0; options[a] && option<0; a++) {
Nanaud 3:3ba377aafdfd 96 if (!strcmp(cmd,options[a]) || strcmp(cmd,options[a])==1) { // strcmp(cmd,options[a])==1 permet de contourner le problème, à revoir !!!
Nanaud 0:dc036b67c87c 97 option=a;
Nanaud 0:dc036b67c87c 98 }
Nanaud 4:ad9b7355332e 99 //pc.printf("res = %d\r\n",strcmp(cmd,options[a]));
Nanaud 0:dc036b67c87c 100 }
Nanaud 3:3ba377aafdfd 101
Nanaud 0:dc036b67c87c 102 switch (option) {
Nanaud 0:dc036b67c87c 103 case 0: //help
Nanaud 4:ad9b7355332e 104 // pc
Nanaud 0:dc036b67c87c 105 pc.printf("\n\n\r###HELP###\n\r");
Nanaud 0:dc036b67c87c 106 pc.printf("usao : Affichage resultats capteurs a ultrasons\n\r");
Nanaud 0:dc036b67c87c 107 pc.printf("usxo : Affichage resultat capteur ultrasons x\n\r");
Nanaud 10:0714feaaaee1 108 pc.printf("tdrv : Appelle une fonction test\n\r");
Nanaud 0:dc036b67c87c 109 pc.printf("cdon : Affichage resultats codeurs\n\r");
Nanaud 0:dc036b67c87c 110 pc.printf("cdgx : Affichage resultats codeur gauche phase x (a ou b)\n\r");
Nanaud 3:3ba377aafdfd 111 pc.printf("tagl : Tourne le robot sur lui-meme avec un angle a preciser\r\n");
Nanaud 5:34ed652f8c31 112 pc.printf("mten : Activation/Desactivation des moteurs\r\n");
Nanaud 5:34ed652f8c31 113 pc.printf("mtdr : Changement de de sens de rotation des moteurs\r\n");
Nanaud 5:34ed652f8c31 114 pc.printf("mtvt : Changement de la vitesse de rotation (vitesse a preciser)\r\n");
Nanaud 8:eb9bbab312ff 115 pc.printf("mtrt : Sens de rotation des moteurs opposes\r\n");
Nanaud 10:0714feaaaee1 116 pc.printf("cofr : Enregistrement nb tics et reset\r\n");
Nanaud 10:0714feaaaee1 117 pc.printf("cofa : Affichage nb tics\r\n");
Nanaud 10:0714feaaaee1 118 pc.printf("parc : Distance a parcourir\r\n");
Nanaud 14:dd3c756c6d48 119 pc.printf("odom : Afficher les valeurs pour l'odometrie\r\n");
Nanaud 0:dc036b67c87c 120 pc.printf("\n\r");
Nanaud 4:ad9b7355332e 121 // bt
Nanaud 4:ad9b7355332e 122 bt.printf("\n\n\r###HELP###\n\r");
Nanaud 4:ad9b7355332e 123 bt.printf("usao : Affichage resultats capteurs a ultrasons\n\r");
Nanaud 4:ad9b7355332e 124 bt.printf("usxo : Affichage resultat capteur ultrasons x\n\r");
Nanaud 10:0714feaaaee1 125 bt.printf("tdrv : Appelle une fonction test\n\r");
Nanaud 4:ad9b7355332e 126 bt.printf("cdon : Affichage resultats codeurs\n\r");
Nanaud 4:ad9b7355332e 127 bt.printf("cdgx : Affichage resultats codeur gauche phase x (a ou b)\n\r");
Nanaud 4:ad9b7355332e 128 bt.printf("tagl : Tourne le robot sur lui-meme avec un angle a preciser\r\n");
Nanaud 5:34ed652f8c31 129 bt.printf("mten : Activation/Desactivation des moteurs\r\n");
Nanaud 5:34ed652f8c31 130 bt.printf("mtdr : Changement de de sens de rotation des moteurs\r\n");
Nanaud 5:34ed652f8c31 131 bt.printf("mtvt : Changement de la vitesse de rotation (vitesse a preciser)\r\n");
Nanaud 8:eb9bbab312ff 132 bt.printf("mtrt : Sens de rotation des moteurs opposes\r\n");
Nanaud 10:0714feaaaee1 133 bt.printf("cofr : Enregistrement nb tics et reset\r\n");
Nanaud 10:0714feaaaee1 134 bt.printf("cofa : Affichage nb tics\r\n");
Nanaud 10:0714feaaaee1 135 bt.printf("parc : Distance a parcourir\r\n");
Nanaud 14:dd3c756c6d48 136 bt.printf("odom : Afficher les valeurs pour l'odometrie\r\n");
Nanaud 4:ad9b7355332e 137 bt.printf("\n\r");
Nanaud 0:dc036b67c87c 138 break;
Nanaud 0:dc036b67c87c 139 case 1: //usao
Nanaud 0:dc036b67c87c 140 pc.printf("Capt US ALL ON/OFF\n\r");
Nanaud 4:ad9b7355332e 141 bt.printf("Capt US ALL ON/OFF\n\r");
Nanaud 3:3ba377aafdfd 142 aff_US[0]=!aff_US[0];
Nanaud 3:3ba377aafdfd 143 aff_US[1]=!aff_US[1];
Nanaud 3:3ba377aafdfd 144 aff_US[2]=!aff_US[2];
Nanaud 3:3ba377aafdfd 145 aff_US[3]=!aff_US[3];
Nanaud 3:3ba377aafdfd 146 aff_US[4]=!aff_US[4];
Nanaud 3:3ba377aafdfd 147 aff_US[5]=!aff_US[5];
Nanaud 0:dc036b67c87c 148 break;
Nanaud 0:dc036b67c87c 149 case 2: //us1o
Nanaud 0:dc036b67c87c 150 pc.printf("Capt US 1 ON/OFF\n\r");
Nanaud 4:ad9b7355332e 151 bt.printf("Capt US 1 ON/OFF\n\r");
Nanaud 0:dc036b67c87c 152 aff_US[0]=!aff_US[0];
Nanaud 0:dc036b67c87c 153 break;
Nanaud 0:dc036b67c87c 154 case 3: //us2o
Nanaud 0:dc036b67c87c 155 pc.printf("Capt US 2 ON/OFF\n\r");
Nanaud 4:ad9b7355332e 156 bt.printf("Capt US 2 ON/OFF\n\r");
Nanaud 0:dc036b67c87c 157 aff_US[1]=!aff_US[1];
Nanaud 0:dc036b67c87c 158 break;
Nanaud 0:dc036b67c87c 159 case 4: //us3o
Nanaud 0:dc036b67c87c 160 pc.printf("Capt US 3 ON/OFF\n\r");
Nanaud 4:ad9b7355332e 161 bt.printf("Capt US 3 ON/OFF\n\r");
Nanaud 0:dc036b67c87c 162 aff_US[2]=!aff_US[2];
Nanaud 0:dc036b67c87c 163 break;
Nanaud 0:dc036b67c87c 164 case 5: //us4o
Nanaud 0:dc036b67c87c 165 pc.printf("Capt US 4 ON/OFF\n\r");
Nanaud 4:ad9b7355332e 166 bt.printf("Capt US 4 ON/OFF\n\r");
Nanaud 0:dc036b67c87c 167 aff_US[3]=!aff_US[3];
Nanaud 0:dc036b67c87c 168 break;
Nanaud 0:dc036b67c87c 169 case 6: //us5o
Nanaud 0:dc036b67c87c 170 pc.printf("Capt US 5 ON/OFF\n\r");
Nanaud 4:ad9b7355332e 171 bt.printf("Capt US 5 ON/OFF\n\r");
Nanaud 0:dc036b67c87c 172 aff_US[4]=!aff_US[4];
Nanaud 0:dc036b67c87c 173 break;
Nanaud 0:dc036b67c87c 174 case 7: //us6o
Nanaud 0:dc036b67c87c 175 pc.printf("Capt US 6 ON/OFF\n\r");
Nanaud 4:ad9b7355332e 176 bt.printf("Capt US 6 ON/OFF\n\r");
Nanaud 0:dc036b67c87c 177 aff_US[5]=!aff_US[5];
Nanaud 0:dc036b67c87c 178 break;
Nanaud 0:dc036b67c87c 179 case 8: //tdrv
Nanaud 0:dc036b67c87c 180 pc.printf("Fonction test_drv()\n\r");
Nanaud 4:ad9b7355332e 181 bt.printf("Fonction test_drv()\n\r");
Nanaud 10:0714feaaaee1 182 //test_drv();
Nanaud 16:ae65ce77b1f9 183 //comptG = 0;
Nanaud 16:ae65ce77b1f9 184 //comptD = 0;
Nanaud 10:0714feaaaee1 185 //test1();
Nanaud 18:48246daf0c06 186 //consigneOrientation = (180*3.1415)/180;
Nanaud 19:c419033c0967 187 //xC = (double)0;
Nanaud 19:c419033c0967 188 //yC = (double)100;
Nanaud 19:c419033c0967 189 //fnc=1;
Nanaud 20:7d206773f39e 190
Nanaud 20:7d206773f39e 191 /*
Nanaud 19:c419033c0967 192 indice++;
Nanaud 19:c419033c0967 193 fnc = objEtape[indice];
Nanaud 19:c419033c0967 194 xC = objX[indice];
Nanaud 19:c419033c0967 195 yC = objY[indice];
Nanaud 19:c419033c0967 196 mot_en();
Nanaud 20:7d206773f39e 197 */
Nanaud 0:dc036b67c87c 198 break;
Nanaud 0:dc036b67c87c 199 case 9: //cdon
Nanaud 0:dc036b67c87c 200 pc.printf("Results ALL Encoders ON/OFF\n\r");
Nanaud 4:ad9b7355332e 201 bt.printf("Results ALL Encoders ON/OFF\n\r");
Nanaud 0:dc036b67c87c 202 aff_cd[0]=!aff_cd[0];
Nanaud 0:dc036b67c87c 203 aff_cd[1]=!aff_cd[1];
Nanaud 0:dc036b67c87c 204 break;
Nanaud 0:dc036b67c87c 205 case 10: //cdga
Nanaud 0:dc036b67c87c 206 pc.printf("Results Encoder Left A ON/OFF\n\r");
Nanaud 4:ad9b7355332e 207 bt.printf("Results Encoder Left A ON/OFF\n\r");
Nanaud 0:dc036b67c87c 208 aff_cd[0]=!aff_cd[0];
Nanaud 0:dc036b67c87c 209 break;
Nanaud 0:dc036b67c87c 210 case 11: //cdgb
Nanaud 0:dc036b67c87c 211 pc.printf("Results Encoder Left B ON/OFF\n\r");
Nanaud 4:ad9b7355332e 212 bt.printf("Results Encoder Left B ON/OFF\n\r");
Nanaud 0:dc036b67c87c 213 aff_cd[1]=!aff_cd[1];
Nanaud 0:dc036b67c87c 214 break;
Nanaud 3:3ba377aafdfd 215 case 12: //tagl
Nanaud 3:3ba377aafdfd 216 pc.printf("Cmd Angle robot\r\n");
Nanaud 3:3ba377aafdfd 217 pc.printf("Angle (entre 0 et 360 degres sous la forme xxx): ");
Nanaud 4:ad9b7355332e 218 bt.printf("Cmd Angle robot\r\n");
Nanaud 4:ad9b7355332e 219 bt.printf("Angle (entre 0 et 360 degres sous la forme xxx): ");
Nanaud 3:3ba377aafdfd 220 cmdType=1;
Nanaud 3:3ba377aafdfd 221 break;
Nanaud 5:34ed652f8c31 222 case 13: // mten
Nanaud 5:34ed652f8c31 223 if(m_dis) {
Nanaud 5:34ed652f8c31 224 mot_en();
Nanaud 5:34ed652f8c31 225 m_dis=!m_dis;
Nanaud 5:34ed652f8c31 226 pc.printf("Motors Enable\r\n");
Nanaud 5:34ed652f8c31 227 bt.printf("Motors Enable\r\n");
Nanaud 5:34ed652f8c31 228 } else {
Nanaud 5:34ed652f8c31 229 mot_dis();
Nanaud 5:34ed652f8c31 230 m_dis=!m_dis;
Nanaud 5:34ed652f8c31 231 pc.printf("Motors Disable\r\n");
Nanaud 5:34ed652f8c31 232 bt.printf("Motors Disable\r\n");
Nanaud 5:34ed652f8c31 233 }
Nanaud 5:34ed652f8c31 234 break;
Nanaud 5:34ed652f8c31 235 case 14: // mtdr
Nanaud 5:34ed652f8c31 236 if(m_avance) {
Nanaud 5:34ed652f8c31 237 pc.printf("Changement de direction : En avant\r\n");
Nanaud 5:34ed652f8c31 238 bt.printf("Changement de direction : En avant\r\n");
Nanaud 5:34ed652f8c31 239 motGauche_fwd();
Nanaud 5:34ed652f8c31 240 motDroite_fwd();
Nanaud 5:34ed652f8c31 241 m_avance=!m_avance;
Nanaud 5:34ed652f8c31 242 } else {
Nanaud 5:34ed652f8c31 243 pc.printf("Changement de direction : En arriere\r\n");
Nanaud 5:34ed652f8c31 244 bt.printf("Changement de direction : En arriere\r\n");
Nanaud 5:34ed652f8c31 245 motGauche_bck();
Nanaud 5:34ed652f8c31 246 motDroite_bck();
Nanaud 5:34ed652f8c31 247 m_avance=!m_avance;
Nanaud 5:34ed652f8c31 248 }
Nanaud 5:34ed652f8c31 249 break;
Nanaud 5:34ed652f8c31 250 case 15: // mtvt
Nanaud 5:34ed652f8c31 251 pc.printf("Cmd Vitesse robot\r\n");
Nanaud 5:34ed652f8c31 252 pc.printf("Vitesse en mm/s (sous la forme xxxx): ");
Nanaud 5:34ed652f8c31 253 bt.printf("Cmd Vitesse robot\r\n");
Nanaud 5:34ed652f8c31 254 bt.printf("Vitesse en mm/s (sous la forme xxxx): ");
Nanaud 5:34ed652f8c31 255 cmdType=2;
Nanaud 5:34ed652f8c31 256 break;
Nanaud 10:0714feaaaee1 257 case 16: // mtrt
Nanaud 10:0714feaaaee1 258 pc.printf("Rotation du robot - Changement de sens");
Nanaud 10:0714feaaaee1 259 bt.printf("Rotation du robot - Changement de sens");
Nanaud 10:0714feaaaee1 260 mtrt_sens = !mtrt_sens;
Nanaud 10:0714feaaaee1 261 if(mtrt_sens) {
Nanaud 10:0714feaaaee1 262 motGauche_fwd();
Nanaud 10:0714feaaaee1 263 motDroite_bck();
Nanaud 10:0714feaaaee1 264 } else {
Nanaud 10:0714feaaaee1 265 motGauche_bck();
Nanaud 10:0714feaaaee1 266 motDroite_fwd();
Nanaud 10:0714feaaaee1 267 }
Nanaud 10:0714feaaaee1 268 break;
Nanaud 10:0714feaaaee1 269 case 17 :
Nanaud 10:0714feaaaee1 270 comptG_saved = comptG;
Nanaud 10:0714feaaaee1 271 comptD_saved = comptD;
Nanaud 10:0714feaaaee1 272 comptG=0;
Nanaud 10:0714feaaaee1 273 comptD=0;
Nanaud 10:0714feaaaee1 274 pc.printf("comptG et comptD sauvegardes\r\n");
Nanaud 10:0714feaaaee1 275 bt.printf("comptG et comptD sauvegardes\r\n");
Nanaud 10:0714feaaaee1 276 break;
Nanaud 10:0714feaaaee1 277 case 18 :
Nanaud 10:0714feaaaee1 278 pc.printf("comptG_saved = %ld\r\n",comptG_saved);
Nanaud 10:0714feaaaee1 279 pc.printf("comptD_saved = %ld\r\n",comptD_saved);
Nanaud 10:0714feaaaee1 280 bt.printf("comptG_saved = %ld\r\n",comptG_saved);
Nanaud 10:0714feaaaee1 281 bt.printf("comptD_saved = %ld\r\n",comptD_saved);
Nanaud 10:0714feaaaee1 282 break;
Nanaud 10:0714feaaaee1 283 case 19 : //
Nanaud 10:0714feaaaee1 284 bt.printf("Distance a parcourir : ");
Nanaud 10:0714feaaaee1 285 cmdType=3;
Nanaud 8:eb9bbab312ff 286 break;
Nanaud 14:dd3c756c6d48 287 case 20: // odom
Nanaud 16:ae65ce77b1f9 288 pc.printf("Odometrie :\n\r");
Nanaud 16:ae65ce77b1f9 289 pc.printf("x = %lf\n\r", x);
Nanaud 16:ae65ce77b1f9 290 pc.printf("y = %lf\n\r", y);
Nanaud 16:ae65ce77b1f9 291 pc.printf("O = %lf\n\r", O);
Nanaud 22:f891c2bce091 292 pc.printf("consigneAngl = %lf\n\r", consigneOrientation);
Nanaud 22:f891c2bce091 293 pc.printf("consigneDist = %lf\n\r", distanceCible);
Nanaud 25:869b1c1f51a7 294 pc.printf("indice = %d\n\r", indiceStrategie);
Nanaud 24:be2b2be6907b 295 //pc.printf("dist 1 = %d\n\r", ttt[0]);
Nanaud 24:be2b2be6907b 296 //pc.printf("dist 2 = %d\n\r", ttt[1]);
Nanaud 24:be2b2be6907b 297 //pc.printf("dist 6 = %d\n\r", ttt[5]);
Nanaud 22:f891c2bce091 298 pc.printf("---\n\r");
Nanaud 24:be2b2be6907b 299 //pc.printf("dist 3 = %d\n\r", ttt[2]);
Nanaud 24:be2b2be6907b 300 //pc.printf("dist 4 = %d\n\r", ttt[3]);
Nanaud 24:be2b2be6907b 301 //pc.printf("dist 5 = %d\n\r", ttt[4]);
Nanaud 16:ae65ce77b1f9 302
Nanaud 14:dd3c756c6d48 303 bt.printf("Odometrie :\n\r");
Nanaud 16:ae65ce77b1f9 304 bt.printf("x = %lf\n\r", x);
Nanaud 16:ae65ce77b1f9 305 bt.printf("y = %lf\n\r", y);
Nanaud 16:ae65ce77b1f9 306 bt.printf("O = %lf\n\r", O);
Nanaud 16:ae65ce77b1f9 307 bt.printf("consigneAngl = %lf\n\r", consigneOrientation);
Nanaud 16:ae65ce77b1f9 308 bt.printf("consigneDist = %lf\n\r", distanceCible);
Nanaud 25:869b1c1f51a7 309 bt.printf("indice = %d\n\r", indiceStrategie);
Nanaud 25:869b1c1f51a7 310 bt.printf("dist 1 = %d\n\r", us_dist[0]);
Nanaud 25:869b1c1f51a7 311 bt.printf("dist 2 = %d\n\r", us_dist[1]);
Nanaud 25:869b1c1f51a7 312 bt.printf("dist 6 = %d\n\r", us_dist[5]);
Nanaud 22:f891c2bce091 313 bt.printf("---\n\r");
Nanaud 25:869b1c1f51a7 314 bt.printf("dist 3 = %d\n\r", us_dist[2]);
Nanaud 25:869b1c1f51a7 315 bt.printf("dist 4 = %d\n\r", us_dist[3]);
Nanaud 25:869b1c1f51a7 316 bt.printf("dist 5 = %d\n\r", us_dist[4]);
Nanaud 22:f891c2bce091 317 bt.printf("distanceMem = %lf\n\r", distanceMem);
Nanaud 22:f891c2bce091 318 bt.printf("distancePlus = %lf\n\r", distancePlus);
plmir 12:2c312916a621 319 break;
Nanaud 0:dc036b67c87c 320 default:
Nanaud 0:dc036b67c87c 321 pc.printf("Commande invalide\n\r");
Nanaud 4:ad9b7355332e 322 bt.printf("Commande invalide\n\r");
Nanaud 0:dc036b67c87c 323 }
Nanaud 0:dc036b67c87c 324 }
Nanaud 0:dc036b67c87c 325
Nanaud 3:3ba377aafdfd 326 void StringToAngle(char *cmd)
Nanaud 3:3ba377aafdfd 327 {
Nanaud 3:3ba377aafdfd 328 int cmd2 = 0;
Nanaud 3:3ba377aafdfd 329 cmd2 += (cmd[0]-48)*100;
Nanaud 3:3ba377aafdfd 330 cmd2 += (cmd[1]-48)*10;
Nanaud 3:3ba377aafdfd 331 cmd2 += (cmd[2]-48)*1;
Nanaud 3:3ba377aafdfd 332
Nanaud 3:3ba377aafdfd 333 if (cmd2>=0 && cmd2<=360) {
Nanaud 16:ae65ce77b1f9 334 consigneOrientation = (cmd2*3.1415)/180;
Nanaud 16:ae65ce77b1f9 335 motGauche_bck();
Nanaud 16:ae65ce77b1f9 336 motDroite_fwd();
Nanaud 16:ae65ce77b1f9 337 mot_en();
Nanaud 16:ae65ce77b1f9 338 bt.printf("\r\nCommande envoyee au robot : %lf\r\n",consigneOrientation);
Nanaud 3:3ba377aafdfd 339 } else {
Nanaud 3:3ba377aafdfd 340 pc.printf("\r\nAngle incorrect\r\n\n");
Nanaud 4:ad9b7355332e 341 bt.printf("\r\nAngle incorrect\r\n\n");
Nanaud 3:3ba377aafdfd 342 cmd2 = 0;
Nanaud 3:3ba377aafdfd 343 }
Nanaud 0:dc036b67c87c 344 }
Nanaud 0:dc036b67c87c 345
Nanaud 10:0714feaaaee1 346 void StringToDist(char *cmd)
Nanaud 10:0714feaaaee1 347 {
Nanaud 10:0714feaaaee1 348 int cmd4 = 0;
Nanaud 10:0714feaaaee1 349 cmd4 += (cmd[0]-48)*1000;
Nanaud 10:0714feaaaee1 350 cmd4 += (cmd[1]-48)*100;
Nanaud 10:0714feaaaee1 351 cmd4 += (cmd[2]-48)*10;
Nanaud 10:0714feaaaee1 352 cmd4 += (cmd[3]-48)*1;
Nanaud 10:0714feaaaee1 353
Nanaud 10:0714feaaaee1 354 if (cmd4>=0 && cmd4<=9999) {
Nanaud 10:0714feaaaee1 355 //consigneDistance = cmd4;
Nanaud 10:0714feaaaee1 356 //pc.printf("\r\nCommande envoyee au robot : %lf\r\n",consigneDistance);
Nanaud 10:0714feaaaee1 357 //bt.printf("\r\nCommande envoyee au robot : %lf\r\n",consigneDistance);
Nanaud 10:0714feaaaee1 358
Nanaud 10:0714feaaaee1 359 } else {
Nanaud 10:0714feaaaee1 360 pc.printf("\r\nDistance incorrecte\r\n\n");
Nanaud 10:0714feaaaee1 361 bt.printf("\r\nDistance incorrecte\r\n\n");
Nanaud 10:0714feaaaee1 362 cmd4 = 0;
Nanaud 10:0714feaaaee1 363 }
Nanaud 10:0714feaaaee1 364 }
Nanaud 10:0714feaaaee1 365
Nanaud 5:34ed652f8c31 366 void StringToVitesse(char *cmd)
Nanaud 5:34ed652f8c31 367 {
Nanaud 5:34ed652f8c31 368 for(int y=0; y<Lcmd ; y++) {
Nanaud 5:34ed652f8c31 369 bt.printf("%d ",cmd[y]);
Nanaud 5:34ed652f8c31 370 }
Nanaud 5:34ed652f8c31 371 bt.printf("\r\n");
Nanaud 5:34ed652f8c31 372
Nanaud 5:34ed652f8c31 373 float cmd3 = 0;
Nanaud 5:34ed652f8c31 374 cmd3 += (cmd[0]-48)*1000;
Nanaud 5:34ed652f8c31 375 cmd3 += (cmd[1]-48)*100;
Nanaud 5:34ed652f8c31 376 cmd3 += (cmd[2]-48)*10;
Nanaud 5:34ed652f8c31 377 cmd3 += (cmd[3]-48)*1;
Nanaud 5:34ed652f8c31 378
Nanaud 5:34ed652f8c31 379 if (cmd3>=0 && cmd3<=9999) {
Nanaud 5:34ed652f8c31 380 cmd3 = cmd3 / 1000;
Nanaud 5:34ed652f8c31 381 pc.printf("\r\nCommande envoyee au robot : %.3f m/s\r\n",cmd3);
Nanaud 5:34ed652f8c31 382 bt.printf("\r\nCommande envoyee au robot : %.3f m/s\r\n",cmd3);
Nanaud 5:34ed652f8c31 383 drvGauche.moveLinSpeed(cmd3);
Nanaud 5:34ed652f8c31 384 drvDroite.moveLinSpeed(cmd3);
Nanaud 5:34ed652f8c31 385 } else {
Nanaud 5:34ed652f8c31 386 pc.printf("\r\nVitesse incorrect\r\n\n");
Nanaud 5:34ed652f8c31 387 bt.printf("\r\nVitesse incorrect\r\n\n");
Nanaud 5:34ed652f8c31 388 cmd3 = 0;
Nanaud 5:34ed652f8c31 389 }
Nanaud 5:34ed652f8c31 390 }
Nanaud 5:34ed652f8c31 391
Nanaud 3:3ba377aafdfd 392 void affUltrasons()
Nanaud 3:3ba377aafdfd 393 {
Nanaud 25:869b1c1f51a7 394 /* Rien */
Nanaud 3:3ba377aafdfd 395 }
Nanaud 3:3ba377aafdfd 396
Nanaud 3:3ba377aafdfd 397 void affCodeurs()
Nanaud 3:3ba377aafdfd 398 {
Nanaud 6:ea6b30c4bb01 399 if(aff_cd[0]) pc.printf("comptG = %d\n\r", comptG);
Nanaud 6:ea6b30c4bb01 400 if(aff_cd[1]) pc.printf("comptD = %d\n\r", comptD);
Nanaud 5:34ed652f8c31 401
Nanaud 6:ea6b30c4bb01 402 if(aff_cd[0]) bt.printf("comptG = %d\n\r", comptG);
Nanaud 6:ea6b30c4bb01 403 if(aff_cd[1]) bt.printf("comptD = %d\n\r", comptD);
Nanaud 3:3ba377aafdfd 404 }
plmir 12:2c312916a621 405
Nanaud 16:ae65ce77b1f9 406 /*
plmir 12:2c312916a621 407 void affOdo()
plmir 12:2c312916a621 408 {
Nanaud 14:dd3c756c6d48 409 if(aff_odo[0]) bt.printf("x = %f\n\r", xB);
Nanaud 14:dd3c756c6d48 410 if(aff_odo[1]) bt.printf("y = %f\n\r", yB);
Nanaud 14:dd3c756c6d48 411 if(aff_odo[2]) bt.printf("phi = %f\n\r", phiB*180/_PI_);
plmir 12:2c312916a621 412 }
Nanaud 16:ae65ce77b1f9 413 */