AresENSEA-CDF2020
/
AresCDFMainCode_us2
Test
debugPC.cpp@22:f891c2bce091, 2020-10-27 (annotated)
- Committer:
- Nanaud
- Date:
- Tue Oct 27 17:27:33 2020 +0000
- Revision:
- 22:f891c2bce091
- Parent:
- 21:e5f0f5abb5ae
- Child:
- 23:a74135a0271d
Homologation J-2
Who changed what in which revision?
User | Revision | Line number | New 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 | 22:f891c2bce091 | 294 | pc.printf("indice = %d\n\r", indice); |
Nanaud | 22:f891c2bce091 | 295 | pc.printf("dist 1 = %lf\n\r", distt[0]); |
Nanaud | 22:f891c2bce091 | 296 | pc.printf("dist 2 = %lf\n\r", distt[1]); |
Nanaud | 22:f891c2bce091 | 297 | pc.printf("dist 6 = %lf\n\r", distt[5]); |
Nanaud | 22:f891c2bce091 | 298 | pc.printf("---\n\r"); |
Nanaud | 22:f891c2bce091 | 299 | pc.printf("dist 3 = %lf\n\r", distt[2]); |
Nanaud | 22:f891c2bce091 | 300 | pc.printf("dist 4 = %lf\n\r", distt[3]); |
Nanaud | 22:f891c2bce091 | 301 | pc.printf("dist 5 = %lf\n\r", distt[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 | 19:c419033c0967 | 309 | bt.printf("indice = %d\n\r", indice); |
Nanaud | 22:f891c2bce091 | 310 | bt.printf("dist 1 = %lf\n\r", distt[0]); |
Nanaud | 22:f891c2bce091 | 311 | bt.printf("dist 2 = %lf\n\r", distt[1]); |
Nanaud | 22:f891c2bce091 | 312 | bt.printf("dist 6 = %lf\n\r", distt[5]); |
Nanaud | 22:f891c2bce091 | 313 | bt.printf("---\n\r"); |
Nanaud | 22:f891c2bce091 | 314 | bt.printf("dist 3 = %lf\n\r", distt[2]); |
Nanaud | 22:f891c2bce091 | 315 | bt.printf("dist 4 = %lf\n\r", distt[3]); |
Nanaud | 22:f891c2bce091 | 316 | bt.printf("dist 5 = %lf\n\r", distt[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 | 20:7d206773f39e | 394 | // Données bruts |
Nanaud | 20:7d206773f39e | 395 | |
Nanaud | 21:e5f0f5abb5ae | 396 | /* |
Nanaud | 4:ad9b7355332e | 397 | // pc |
Nanaud | 21:e5f0f5abb5ae | 398 | if(aff_US[0]) pc.printf("Tps US1 = %5.0f uS\n\r", us_diff[0]); |
Nanaud | 21:e5f0f5abb5ae | 399 | if(aff_US[1]) pc.printf("Tps US2 = %5.0f uS\n\r", us_diff[1]); |
Nanaud | 21:e5f0f5abb5ae | 400 | if(aff_US[2]) pc.printf("Tps US3 = %5.0f uS\n\r", us_diff[2]); |
Nanaud | 21:e5f0f5abb5ae | 401 | if(aff_US[3]) pc.printf("Tps US4 = %5.0f uS\n\r", us_diff[3]); |
Nanaud | 21:e5f0f5abb5ae | 402 | if(aff_US[4]) pc.printf("Tps US5 = %5.0f uS\n\r", us_diff[4]); |
Nanaud | 21:e5f0f5abb5ae | 403 | if(aff_US[5]) pc.printf("Tps US6 = %5.0f uS\n\r", us_diff[5]); |
Nanaud | 4:ad9b7355332e | 404 | 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 | 405 | |
Nanaud | 4:ad9b7355332e | 406 | // bt |
Nanaud | 21:e5f0f5abb5ae | 407 | if(aff_US[0]) bt.printf("Tps US1 = %5.0f uS\n\r", us_diff[0]); |
Nanaud | 21:e5f0f5abb5ae | 408 | if(aff_US[1]) bt.printf("Tps US2 = %5.0f uS\n\r", us_diff[1]); |
Nanaud | 21:e5f0f5abb5ae | 409 | if(aff_US[2]) bt.printf("Tps US3 = %5.0f uS\n\r", us_diff[2]); |
Nanaud | 21:e5f0f5abb5ae | 410 | if(aff_US[3]) bt.printf("Tps US4 = %5.0f uS\n\r", us_diff[3]); |
Nanaud | 21:e5f0f5abb5ae | 411 | if(aff_US[4]) bt.printf("Tps US5 = %5.0f uS\n\r", us_diff[4]); |
Nanaud | 21:e5f0f5abb5ae | 412 | if(aff_US[5]) bt.printf("Tps US6 = %5.0f uS\n\r", us_diff[5]); |
Nanaud | 4:ad9b7355332e | 413 | if(aff_US[0]||aff_US[1]||aff_US[2]||aff_US[3]||aff_US[4]||aff_US[5]) bt.printf("\n\r"); |
Nanaud | 20:7d206773f39e | 414 | */ |
Nanaud | 20:7d206773f39e | 415 | |
Nanaud | 21:e5f0f5abb5ae | 416 | |
Nanaud | 20:7d206773f39e | 417 | // Distances |
Nanaud | 20:7d206773f39e | 418 | |
Nanaud | 20:7d206773f39e | 419 | // pc |
Nanaud | 22:f891c2bce091 | 420 | /* |
Nanaud | 22:f891c2bce091 | 421 | pc.printf("US[1] = %lf\n\r", us_diff[0]); |
Nanaud | 22:f891c2bce091 | 422 | pc.printf("US[2] = %lf\n\r", us_diff[1]); |
Nanaud | 22:f891c2bce091 | 423 | pc.printf("US[3] = %lf\n\r", us_diff[2]); |
Nanaud | 22:f891c2bce091 | 424 | pc.printf("US[4] = %lf\n\r", us_diff[3]); |
Nanaud | 22:f891c2bce091 | 425 | pc.printf("US[5] = %lf\n\r", us_diff[4]); |
Nanaud | 22:f891c2bce091 | 426 | pc.printf("US[6] = %lf\n\r", us_diff[5]); |
Nanaud | 20:7d206773f39e | 427 | pc.printf("\n\r"); |
Nanaud | 22:f891c2bce091 | 428 | */ |
Nanaud | 22:f891c2bce091 | 429 | |
Nanaud | 22:f891c2bce091 | 430 | ///* |
Nanaud | 22:f891c2bce091 | 431 | pc.printf("distance[1] = %lf\n\r", distt[0]); |
Nanaud | 22:f891c2bce091 | 432 | pc.printf("distance[2] = %lf\n\r", distt[1]); |
Nanaud | 22:f891c2bce091 | 433 | pc.printf("distance[3] = %lf\n\r", distt[2]); |
Nanaud | 22:f891c2bce091 | 434 | pc.printf("distance[4] = %lf\n\r", distt[3]); |
Nanaud | 22:f891c2bce091 | 435 | pc.printf("distance[5] = %lf\n\r", distt[4]); |
Nanaud | 22:f891c2bce091 | 436 | pc.printf("distance[6] = %lf\n\r", distt[5]); |
Nanaud | 22:f891c2bce091 | 437 | pc.printf("\n\r"); |
Nanaud | 22:f891c2bce091 | 438 | //*/ |
Nanaud | 20:7d206773f39e | 439 | |
Nanaud | 20:7d206773f39e | 440 | // bt |
Nanaud | 20:7d206773f39e | 441 | /* |
Nanaud | 20:7d206773f39e | 442 | bt.printf("distance[0] = %f\n\r", ::distance[0]); |
Nanaud | 20:7d206773f39e | 443 | bt.printf("distance[1] = %f\n\r", ::distance[1]); |
Nanaud | 20:7d206773f39e | 444 | bt.printf("distance[2] = %f\n\r", ::distance[2]); |
Nanaud | 20:7d206773f39e | 445 | bt.printf("distance[3] = %f\n\r", ::distance[3]); |
Nanaud | 20:7d206773f39e | 446 | bt.printf("distance[4] = %f\n\r", ::distance[4]); |
Nanaud | 20:7d206773f39e | 447 | bt.printf("distance[5] = %f\n\r", ::distance[5]); |
Nanaud | 20:7d206773f39e | 448 | */ |
Nanaud | 3:3ba377aafdfd | 449 | } |
Nanaud | 3:3ba377aafdfd | 450 | |
Nanaud | 3:3ba377aafdfd | 451 | void affCodeurs() |
Nanaud | 3:3ba377aafdfd | 452 | { |
Nanaud | 6:ea6b30c4bb01 | 453 | if(aff_cd[0]) pc.printf("comptG = %d\n\r", comptG); |
Nanaud | 6:ea6b30c4bb01 | 454 | if(aff_cd[1]) pc.printf("comptD = %d\n\r", comptD); |
Nanaud | 5:34ed652f8c31 | 455 | |
Nanaud | 6:ea6b30c4bb01 | 456 | if(aff_cd[0]) bt.printf("comptG = %d\n\r", comptG); |
Nanaud | 6:ea6b30c4bb01 | 457 | if(aff_cd[1]) bt.printf("comptD = %d\n\r", comptD); |
Nanaud | 3:3ba377aafdfd | 458 | } |
plmir | 12:2c312916a621 | 459 | |
Nanaud | 16:ae65ce77b1f9 | 460 | /* |
plmir | 12:2c312916a621 | 461 | void affOdo() |
plmir | 12:2c312916a621 | 462 | { |
Nanaud | 14:dd3c756c6d48 | 463 | if(aff_odo[0]) bt.printf("x = %f\n\r", xB); |
Nanaud | 14:dd3c756c6d48 | 464 | if(aff_odo[1]) bt.printf("y = %f\n\r", yB); |
Nanaud | 14:dd3c756c6d48 | 465 | if(aff_odo[2]) bt.printf("phi = %f\n\r", phiB*180/_PI_); |
plmir | 12:2c312916a621 | 466 | } |
Nanaud | 16:ae65ce77b1f9 | 467 | */ |