AresENSEA-CDF2020 / Mbed 2 deprecated AresCDFMainCode_capteur_US

Capteur_US

Dependencies:   mbed DRV8825

debugPC.cpp@10:0714feaaaee1, 2020-09-11 (annotated)

Committer:
Nanaud
Date:
Fri Sep 11 10:56:08 2020 +0000
Revision:
10:0714feaaaee1
Parent:
8:eb9bbab312ff
Child:
12:2c312916a621
11.09.20

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