Chafik ZERROUKI
Pixi cachan
Dependencies: Encoder_Nucleo_32_bits PwmIn mbed
Fork of
v2
by 
Chafik ZERROUKI
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 /** Main Test Board 00002 * 00003 * \brief Programme de tests pour le robot NCR 2017 00004 * \author H. Angelis 00005 * \version alpha_1 00006 * \date 15/05/17 00007 * 00008 */ 00009 00010 #include "mbed.h" 00011 #include "PwmIn.h" 00012 #include "Nucleo_Encoder_16_bits.h" 00013 00014 #define BOUSSOLE_adress 0xC0 00015 #define PIXY_adress 0x54 00016 00017 #define CC_BLOCSIZE 14 00018 #define N_BLOCSIZE 12 00019 00020 #define N_BLOCCODE 0xAA55 00021 #define CC_BLOCCODE 0xAA56 00022 00023 typedef unsigned char Byte; 00024 typedef unsigned short Word; 00025 typedef unsigned long lWord; 00026 00027 typedef enum {S_monte = 1, S_descente = 0} T_SERVODIR; 00028 00029 typedef enum {none, begin, normal, colorCode, doubleZero} T_pixyState; 00030 00031 typedef union { 00032 lWord mot; 00033 Byte tab[4]; 00034 } T_tmpBuffer; 00035 00036 typedef union { 00037 Word mot; 00038 Byte tab[2]; 00039 } T_structBuffer; 00040 00041 typedef struct { 00042 Word checksum; 00043 Word signature; 00044 Word x; 00045 Word y; 00046 Word width; 00047 Word height; 00048 Word angle; 00049 } T_pixyCCBloc; 00050 00051 typedef struct { 00052 Word checksum; 00053 Word signature; 00054 Word x; 00055 Word y; 00056 Word width; 00057 Word height; 00058 } T_pixyNMBloc; 00059 00060 typedef union { 00061 Byte tab[14]; 00062 T_pixyCCBloc CCbloc; 00063 } T_pixyCCData; 00064 00065 typedef union { 00066 Byte tab[12]; 00067 T_pixyNMBloc NMbloc; 00068 } T_pixyNMData; 00069 00070 00071 00072 /** Liste des objets 00073 * 00074 * Serial #4 Pixy 00075 * Serial #2 Pc 00076 * 00077 * AnalogIn C1, C2, C3, LD1, LD2, SD1, SD2, Vbat 00078 * 00079 * DigitalOut Led1, Led2, Trig1, Trig2, Trig3, En, SensG, SensD 00080 * 00081 * InterruptIn IndexG, IndexD, Echo1, Echo2, Echo3, BP 00082 * 00083 * PwmOut Pwm_MG, Pwm_MD, Servo 00084 * 00085 * PwmIn PWMG, PWMD, PWMB 00086 * 00087 * I2C Bus_I2C 00088 * 00089 * SPI MotG, MotD 00090 * 00091 * Nucleo_Encoder_16_bits Gauche, Droite 00092 * 00093 * Ticker timer 00094 */ 00095 00096 /** Liste des PINs 00097 * 00098 * PIN MAP (ordre alphabetique) des PINs de la Nucléo 64 utilisée 00099 * PA_0 -> Pixy RX (Serial) 00100 * PA_1 -> Pixy TX (Serial) 00101 * PA_2 -> PC TX (Serial) 00102 * PA_3 -> PX RX (Serial) 00103 * PA_4 -> GP2 SD #2 (Analog In) 00104 * PA_5 -> LED1 (Digital Out) 00105 * PA_6 -> CNY3 (Analog In) 00106 * PA_7 -> CNY2 (Analog In) 00107 * PA_8 -> Servomoteur (PWM Out) 00108 * PA_9 -> US Trigger #3 (Digital Out) 00109 * PA_10 -> US Echo #1 (Pwm In) 00110 * PA_11 -> US Echo #2 (Pwm In) 00111 * PA_12 -> SS (SPI Slave Select) (Digital Out) 00112 * PA_13 00113 * PA_14 00114 * PA_15 -> Boussole (Pwm In) 00115 * 00116 * PB_0 -> GP2 SD #1 (Analog In) 00117 * PB_1 -> Position D (Pwm In) 00118 * PB_2 -> Position G (Pwm In) 00119 * PB_3 -> PWM Mot D (PWM Out) 00120 * PB_4 -> Enocdeur Droit A (QE) 00121 * PB_5 -> Enocdeur Droit B (QE) 00122 * PB_6 -> Enocdeur Gauche A (QE) 00123 * PB_7 -> Enocdeur Gauche B (QE) 00124 * PB_8 -> SCL (I2C) 00125 * PB_9 -> SDA (I2C) 00126 * PB_10 -> PWM Mot G (PWM Out) 00127 * PB_11 00128 * PB_12 -> US Echo #3 (Pwm In) 00129 * PB_13 -> SCK Encodeur D (SPI) 00130 * PB_14 -> MISO Encodeur D (SPI) 00131 * PB_15 -> MOSI Encodeur D (SPI) 00132 * 00133 * PC_0 -> GP2 LD #1 (Analog In) 00134 * PC_1 -> GP2 LD #2 (Analog In) 00135 * PC_2 -> US Trigger #2 (Digital Out) 00136 * PC_3 -> US Trigger #1 (Digital Out) 00137 * PC_4 -> CNY1 (Analog In) 00138 * PC_5 -> Vbat (Analog In) 00139 * PC_6 -> Dir Mot Droit (Digital Out) 00140 * PC_7 -> I (Encodeur Gauche) (IRQ In) 00141 * PC_8 -> Dir Mot Gauche (Digital Out) 00142 * PC_9 -> Enable Moteurs (Digital Out) 00143 * PC_10 -> SCK Encodeur G (SPI) 00144 * PC_11 -> MISO Encodeur G (SPI) 00145 * PC_12 -> MOSI Encodeur G (SPI) 00146 * PC_13 -> User BP (IRQ In) 00147 * PC_14 00148 * PC_15 00149 * 00150 * PD_1 00151 * PD_2 -> Led2 (Digital Out) 00152 */ 00153 00154 Serial Pixy (PA_0, PA_1, 230400); 00155 Serial Pc (PA_2, PA_3, 460800); 00156 00157 AnalogIn CNY1 (PC_4); 00158 AnalogIn CNY2 (PA_7); 00159 AnalogIn CNY3 (PA_6); 00160 AnalogIn LD1 (PC_0); 00161 AnalogIn LD2 (PC_1); 00162 AnalogIn SD1 (PB_0); 00163 AnalogIn SD2 (PA_4); 00164 AnalogIn Vbat (PC_5); 00165 00166 DigitalOut Led1 (PA_5); 00167 DigitalOut Led2 (PD_2); 00168 DigitalOut Trig1 (PC_3); 00169 DigitalOut Trig2 (PC_2); 00170 DigitalOut Trig3 (PA_9); 00171 DigitalOut En (PC_9); 00172 DigitalOut SensG (PC_8); 00173 DigitalOut SensD (PC_6); 00174 DigitalOut SS (PA_12); 00175 00176 InterruptIn Echo1 (PA_10); 00177 InterruptIn Echo2 (PA_11); 00178 InterruptIn Echo3 (PB_12); 00179 InterruptIn BP (PC_13); 00180 InterruptIn IG (PC_7); 00181 00182 PwmIn PWMG (PB_2); 00183 PwmIn PWMD (PB_1); 00184 PwmIn PWMB (PA_15); 00185 00186 PwmOut Pwm_MG (PB_10); 00187 PwmOut Pwm_MD (PB_3); 00188 PwmOut Servo (PA_8); 00189 00190 I2C Bus_I2C (PB_9, PB_8); 00191 00192 SPI MotG (PC_12, PC_11, PC_10); 00193 SPI MotD (PB_15, PB_14, PB_13); 00194 00195 Nucleo_Encoder_16_bits Gauche (TIM4); // A = PB_6, B = PB_7 00196 Nucleo_Encoder_16_bits Droite (TIM3); // A = PB_4, B = PB_5 00197 00198 Ticker tick; 00199 Timer times; 00200 00201 /** Liste des variables globales 00202 * 00203 * Tick -> (long) Compte le nombre de MS écoulé et déclenche l'exécution de la boucle en fonction du temps écoulé. 00204 * FlagIG -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue gauche 00205 * FlagID -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue droite 00206 * EchoXStart -> (long) Donne le temps en µs de début de l'impulsion d'écho de l'US n°X 00207 * DistUSX -> (float) Donne la distance en mm mesurée par l'US n°X 00208 */ 00209 00210 // Structure de temps 00211 lWord Tick = 0; 00212 00213 // Sémaphore d'interruption 00214 int FlagUS1 = 0, FlagUS2 = 0, FlagUS3 = 0, FlagPixy = 0, FlagPixyOverflow = 0; 00215 int FlagTick = 0, FlagTickLed = 0, BPPressed = 0; 00216 int Pixy_check = -1; 00217 00218 // Dialogue avec la Pixy 00219 T_pixyCCData Pixy_CCFIFO[20]; 00220 T_pixyNMData Pixy_NMFIFO[20]; 00221 Byte Pixy_CCObjet, Pixy_NMObjet; 00222 00223 // Gestion des capteurs Ultrason 00224 long Echo1Start, Echo2Start, Echo3Start; 00225 double DistUS1, DistUS2, DistUS3; 00226 00227 /** Liste des interruptions 00228 * 00229 */ 00230 00231 void tickTime() 00232 { 00233 Tick++; 00234 FlagTick = 1; 00235 if ((Tick%100)==0) FlagTickLed = 1; 00236 } 00237 00238 void BPevent () 00239 { 00240 BPPressed = 1; 00241 } 00242 00243 void echo1Rise () 00244 { 00245 Echo1Start = times.read_us(); 00246 } 00247 00248 void echo2Rise () 00249 { 00250 Echo2Start = times.read_us(); 00251 } 00252 00253 void echo3Rise () 00254 { 00255 Echo3Start = times.read_us(); 00256 } 00257 00258 void echo1Fall () 00259 { 00260 DistUS1 = (double)(times.read_us() - Echo1Start)/5.8; 00261 FlagUS1 = 1; 00262 } 00263 00264 void echo2Fall () 00265 { 00266 DistUS2 = (double)(times.read_us() - Echo2Start)/5.8; 00267 FlagUS2 = 1; 00268 } 00269 00270 void echo3Fall () 00271 { 00272 DistUS3 = (double)(times.read_us() - Echo3Start)/5.8; 00273 FlagUS3 = 1; 00274 } 00275 00276 void getPixyByte () 00277 { 00278 static T_tmpBuffer tmpBuffer; 00279 static T_structBuffer msgBuffer; 00280 static T_pixyState PIXY_state = none; 00281 static Byte byteCount = 0; 00282 static int PIXY_synced = 0, dummy; 00283 int i, somme; 00284 static Byte PIXY_nbCCObjet = 0, PIXY_wCCObjet = 0; 00285 static Byte PIXY_nbNMObjet = 0, PIXY_wNMObjet = 0; 00286 00287 Pixy_check = 0; 00288 00289 if (!PIXY_synced) { // On n'a pas trouvé le START (0x55aa0000) 00290 tmpBuffer.tab[byteCount] = Pixy.getc(); // On stocke l'octet reçu dans la première case dispo du tableau temporaire 00291 00292 if (byteCount < 3) { // Si on n'a pas encore reçu les 4 premier octets 00293 byteCount++; // On passe à la case suivante du tableau temporaire 00294 } else { // Lorsqu'on a 4 octets 00295 if (tmpBuffer.mot != 0xaa550000) { // Si le code n'est pas le code de START 00296 for (i=1; i<4; i++) tmpBuffer.tab[i-1] = tmpBuffer.tab[i]; // On décalle les cases du tableau 00297 byteCount = 3; // Et on attend le caractère suivant 00298 } else { // Si on a trouvé le code de START 00299 PIXY_synced = 1; // On passe en mode synchronisé 00300 PIXY_state = begin; 00301 byteCount = 0; 00302 } 00303 } 00304 } 00305 00306 if (PIXY_synced) { 00307 00308 switch (PIXY_state) { 00309 case begin : // l'aiguillage est là ! 00310 00311 msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus 00312 byteCount++; 00313 if (byteCount == 2) { // Quand on a 2 octets 00314 00315 if (msgBuffer.mot == 0xaa55) { // Si c'est un bloc normal (code 0xAA55) 00316 PIXY_state = normal; // On part vers le traitement spécifique 00317 } 00318 00319 if (msgBuffer.mot == 0xaa56) { // Si c'est un bloc Color Code (code 0xAA56) 00320 PIXY_state = colorCode; // On part vers le traitement spécifique 00321 } 00322 00323 if (msgBuffer.mot == 0) { // Si on a un debut de trame (code 0000) 00324 PIXY_state = doubleZero; // On part vers le traitement spécifique 00325 } 00326 if ((PIXY_state == begin) || (PIXY_state == none)) { // Si c'est autre chose 00327 PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé. 00328 PIXY_state = none; // Ceinture et bretelle 00329 } 00330 byteCount = 0; 00331 } 00332 break; 00333 00334 case normal : // Si on a un bloc normal 00335 00336 Pixy_NMFIFO[PIXY_wNMObjet].tab[byteCount] = Pixy.getc(); // On stocke les octets un à un dans la structure Bloc 00337 if (byteCount < 11) { // Tant que la structure n'est pas pleine 00338 byteCount++; // On passe à l'octet suivant 00339 } else { // Quand elle est pleine 00340 byteCount = 0; // On réinitialise 00341 PIXY_state = begin; // On retourne à l'aiguillage 00342 // On calcule la somme de contrôle 00343 somme = Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.signature + Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.x + Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.y + Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.width + Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.height; 00344 00345 if (somme == Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.checksum) { // Si le checksum est bon, on valide la réception 00346 if (PIXY_wNMObjet < 19) PIXY_wNMObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet 00347 else PIXY_wNMObjet = 0; 00348 if (PIXY_nbNMObjet < 19) PIXY_nbNMObjet++; // On dit que l'on a un objet CC de plus 00349 else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow 00350 } 00351 } 00352 break; 00353 00354 case colorCode : // Si on a un bloc colorCode 00355 00356 Pixy_CCFIFO[PIXY_wCCObjet].tab[byteCount] = dummy; // On stocke les octets un à un dans la structure CCBloc 00357 if (byteCount < 13) byteCount++; // tant que la structure n'est pas pleine on passe à l'octet suivant 00358 else { // Quand elle est pleine 00359 byteCount = 0; // On réinitialise 00360 PIXY_state = begin; // On retourne à l'aiguillage 00361 // On calcule la somme de contrôle 00362 somme = Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.signature + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.x + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.y + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.width + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.height + Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.angle; 00363 00364 if (somme == Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.checksum) { // Si le checksum est bon 00365 if (PIXY_wCCObjet < 19) PIXY_wCCObjet++; // On incrémente le pointeur d'écriture dans la FIFO CCObjet 00366 else PIXY_wCCObjet = 0; 00367 if (PIXY_nbCCObjet < 19) PIXY_nbCCObjet++; // On dit que l'on a un objet CC de plus à traiter 00368 else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow 00369 } 00370 } 00371 break; 00372 00373 case doubleZero : // Si on a reçu le code de début d'une nouvelle trame. 00374 00375 msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus 00376 byteCount++; 00377 if (byteCount == 2) { // Quand on a 2 octets 00378 if (msgBuffer.mot == 0xaa55) { // On doit impérativement trouver le code 0xAA55 00379 PIXY_state = begin; // Si c'est le cas, alors tout va bien et on va à l'aiguillage 00380 Pixy_NMObjet = PIXY_nbNMObjet; // On met à jour les variables pour le traitement 00381 Pixy_CCObjet = PIXY_nbCCObjet; 00382 PIXY_nbCCObjet = 0; 00383 PIXY_nbNMObjet = 0; 00384 FlagPixy = 1; // On valide le traitement de la trame précédente. 00385 } else { // Si on trouve autre chose 00386 PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé. 00387 PIXY_state = none; // Ceinture et bretelle 00388 } 00389 byteCount = 0; 00390 } 00391 break; 00392 } 00393 } 00394 } 00395 00396 00397 int main() 00398 { 00399 int i = 0, val_H_balle = 0, val_W_balle=0, max_val_W_balle = 0, indice_balle = 0, val_carre_balle=0, val_x_balle=0; 00400 int I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/; 00401 00402 Byte PIXY_rCCObjet = 0, PIXY_rNMObjet = 0; 00403 int PIXY_objet; 00404 00405 double SERVO_angle = 0, SERVO_angleMAX = 180, SERVO_angleMIN = 0; 00406 Word SERVO_pos; 00407 T_SERVODIR SERVO_dir = S_monte; 00408 00409 char MENU_choix = 0; 00410 00411 char BOUSSOLE_status[1] = {0}; 00412 char I2C_registerValue[4]; 00413 double BOUSSOLE_periode; 00414 00415 double CAP_I2C, CAP_PWM; 00416 double SD1_val, SD2_val, LD1_val, LD2_val, CNY1_val, CNY2_val, CNY3_val, Vbat_val; 00417 double SD1_dist, SD2_dist, LD1_dist, LD2_dist; 00418 00419 times.reset(); 00420 times.start(); 00421 00422 tick.attach(&tickTime, 0.001); 00423 00424 Bus_I2C.frequency (100000); 00425 00426 BP.rise (&BPevent); 00427 00428 Echo1.rise (&echo1Rise); 00429 Echo2.rise (&echo2Rise); 00430 Echo3.rise (&echo3Rise); 00431 Echo1.fall (&echo1Fall); 00432 Echo2.fall (&echo2Fall); 00433 Echo3.fall (&echo3Fall); 00434 00435 BP.enable_irq(); 00436 IG.enable_irq(); 00437 Echo1.enable_irq(); 00438 Echo2.enable_irq(); 00439 Echo3.enable_irq(); 00440 00441 Pixy.attach (&getPixyByte); 00442 00443 Pwm_MG.period_us(50); 00444 Pwm_MD.period_us(50); 00445 Pwm_MG = 0; 00446 Pwm_MD = 0; 00447 En = 0; 00448 SensG = 0; 00449 SensD = 0; 00450 Led2 = 0; 00451 00452 Servo.period_ms(20); 00453 00454 while(1) { 00455 00456 do { 00457 Led1 = 0; 00458 Pc.printf ("\n\n\n\n\rProgramme de test\n\n\rEntrez le code du test a effectuer :\n\n"); 00459 Pc.printf ("\r1- Capteurs Ultra Son (les 3)\n"); 00460 Pc.printf ("\r2- Boussole et I2C\n"); 00461 Pc.printf ("\r3- Capteurs GP2 (les 4)\n"); 00462 Pc.printf ("\r4- Capteurs CNY70 (les 3)\n"); 00463 Pc.printf ("\r5- VBAT\n"); 00464 Pc.printf ("\r6- Moteur Gauche -- NON CODE --\n"); 00465 Pc.printf ("\r7- Moteur Droit -- NON CODE --\n"); 00466 Pc.printf ("\r8- Servomoteur\n"); 00467 Pc.printf ("\r9- PIXY (CMUCAM5)\n\r"); 00468 MENU_choix = Pc.getc (); 00469 } while (((MENU_choix-'0')<1) || ((MENU_choix-'0')>9)); 00470 00471 switch (MENU_choix-'0') { 00472 00473 case 1 : 00474 Pc.printf ("\n\n\rTest des captreurs Ultrason\n"); 00475 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00476 do { 00477 00478 if (FlagTickLed) { 00479 Led1 = !Led1; 00480 FlagTickLed = 0; 00481 } 00482 00483 // Gestion des US 00484 if (((Tick%150)==0) && FlagTick) { 00485 Trig1 = 1; 00486 wait_us(20); 00487 Trig1 = 0; 00488 FlagTick = 0; 00489 } 00490 00491 if (((Tick%150)==50) && FlagTick) { 00492 Trig2 = 1; 00493 wait_us(20); 00494 Trig2 = 0; 00495 FlagTick = 0; 00496 } 00497 00498 if (((Tick%150)==100) && FlagTick) { 00499 Trig3 = 1; 00500 wait_us(20); 00501 Trig3 = 0; 00502 FlagTick = 0; 00503 } 00504 00505 if (FlagUS1==1) { 00506 Pc.printf ("\rUS 1 = %04d mm", (int)DistUS1); 00507 FlagUS1 = 0; 00508 } 00509 00510 if (FlagUS2==1) { 00511 Pc.printf ("\r\t\t\tUS 2 = %04d mm", (int)DistUS2); 00512 FlagUS2 = 0; 00513 } 00514 00515 if (FlagUS3==1) { 00516 Pc.printf ("\r\t\t\t\t\t\tUS 3 = %04d mm", (int)DistUS3); 00517 FlagUS3 = 0; 00518 } 00519 } while(!Pc.readable()); 00520 MENU_choix = Pc.getc(); 00521 break; 00522 00523 case 2 : 00524 Pc.printf ("\n\n\rBoussole\n"); 00525 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00526 00527 Pc.printf ("\n\rVerif du bus I2C :"); 00528 I2C_check = Bus_I2C.write (BOUSSOLE_adress,BOUSSOLE_status,1,false); 00529 if (I2C_check==0) { 00530 Pc.printf (" OK\n"); 00531 Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true); 00532 Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4); 00533 Pc.printf ("\rVersion Firmware boussole : %03d\n", I2C_registerValue[0]); 00534 } else { 00535 Pc.printf (" FAIL\n"); 00536 } 00537 00538 BOUSSOLE_periode = PWMB.period(); 00539 Pc.printf ("\rVerif de la PWM :"); 00540 if ((BOUSSOLE_periode > 0.11) || (BOUSSOLE_periode < 0.06)) { 00541 Pc.printf (" FAIL\n\n"); 00542 } else { 00543 Pc.printf (" OK\n\n"); 00544 BOUSSOLE_check = 0; 00545 } 00546 00547 do { 00548 if (FlagTickLed) { 00549 Led1 = !Led1; 00550 FlagTickLed = 0; 00551 } 00552 00553 if (((Tick%150)==0) && FlagTick) { 00554 FlagTick = 0; 00555 if (BOUSSOLE_check==0) { 00556 CAP_PWM = ((PWMB.pulsewidth()*1000)-1)*10; 00557 Pc.printf ("\r PWM = %4.1lf", CAP_PWM); 00558 } 00559 if (I2C_check==0) { 00560 Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true); 00561 Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4); 00562 CAP_I2C = (double)(((unsigned short)I2C_registerValue[2]<<8)+(unsigned short)I2C_registerValue[3])/10.0; 00563 Pc.printf ("\r\t\t I2C = %4.1lf", CAP_I2C); 00564 } 00565 } 00566 } while(!Pc.readable()); 00567 MENU_choix = Pc.getc(); 00568 break; 00569 00570 case 3 : 00571 Pc.printf ("\n\n\rGP2xx\n"); 00572 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00573 do { 00574 if (FlagTickLed) { 00575 Led1 = !Led1; 00576 FlagTickLed = 0; 00577 00578 SD1_val = SD1; 00579 SD2_val = SD2; 00580 LD1_val = LD1; 00581 LD2_val = LD2; 00582 00583 if (SD1_val < 0.06) { 00584 SD1_val = 0; 00585 SD1_dist = 40; 00586 } else { 00587 SD1_dist = 11.611/(SD1_val*3.3-0.0237); 00588 if (SD1_dist > 40) SD1_dist = 40; 00589 } 00590 00591 if (SD2_val < 0.06) { 00592 SD2_val = 0; 00593 SD2_dist = 40; 00594 } else { 00595 SD2_dist = 11.611/(SD2_val*3.3-0.0237); 00596 if (SD2_dist > 40) SD2_dist = 40; 00597 } 00598 00599 if (LD1_val < 0.1) { 00600 LD1_val = 0; 00601 LD1_dist = 150; 00602 } else { 00603 LD1_dist = 59.175/(LD1_val*3.3-0.0275); 00604 if (LD1_dist > 150) LD1_dist = 150; 00605 } 00606 00607 if (LD2_val < 0.1) { 00608 LD2_val = 0; 00609 LD2_dist = 150; 00610 } else { 00611 LD2_dist = 59.175/(LD2_val*3.3-0.0275); 00612 if (LD2_dist > 150) LD2_dist = 150; 00613 } 00614 00615 Pc.printf ("\r SD1 = %3.1lf cm - SD2 = %3.1lf cm - LD1 = %4.1lf cm - LD2 = %4.1lf cm", SD1_dist, SD2_dist, LD1_dist, LD2_dist); 00616 } 00617 } while(!Pc.readable()); 00618 MENU_choix = Pc.getc(); 00619 break; 00620 00621 case 4 : 00622 Pc.printf ("\n\n\rCNY70\n"); 00623 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00624 do { 00625 if (FlagTickLed) { 00626 Led1 = !Led1; 00627 FlagTickLed = 0; 00628 00629 CNY1_val = CNY1; 00630 CNY2_val = CNY2; 00631 CNY3_val = CNY3; 00632 00633 Pc.printf ("\r CNY1 = %3.2lf V\t CNY2 = %3.2lf V\t CNY3 = %3.2lf V", CNY1_val*3.3, CNY2_val*3.3, CNY3_val*3.3); 00634 } 00635 } while(!Pc.readable()); 00636 MENU_choix = Pc.getc(); 00637 break; 00638 00639 case 5 : 00640 Pc.printf ("\n\n\rVbat\n"); 00641 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00642 do { 00643 if (FlagTickLed) { 00644 Led1 = !Led1; 00645 FlagTickLed = 0; 00646 00647 Vbat_val = Vbat; 00648 Pc.printf ("\rVbat = %5.3lf V", Vbat_val*3.3*4.3); 00649 } 00650 } while(!Pc.readable()); 00651 MENU_choix = Pc.getc(); 00652 break; 00653 00654 case 6 : 00655 Pc.printf ("\n\n\rMoteur Gauche\n"); 00656 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00657 do { 00658 if (FlagTickLed) { 00659 Led1 = !Led1; 00660 FlagTickLed = 0; 00661 } 00662 } while(!Pc.readable()); 00663 MENU_choix = Pc.getc(); 00664 break; 00665 00666 case 7 : 00667 Pc.printf ("\n\n\rMoteur Droit\n"); 00668 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00669 do { 00670 if (FlagTickLed) { 00671 Led1 = !Led1; 00672 FlagTickLed = 0; 00673 wait (0.1); 00674 } 00675 } while(!Pc.readable()); 00676 MENU_choix = Pc.getc(); 00677 break; 00678 00679 case 8 : 00680 Pc.printf ("\n\n\rServo Moteur\n"); 00681 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00682 do { 00683 if (FlagTickLed) { 00684 Led1 = !Led1; 00685 FlagTickLed = 0; 00686 } 00687 00688 if (((Tick%250)==0) && FlagTick) { 00689 if (SERVO_dir == S_monte) { 00690 if (SERVO_angle >= SERVO_angleMAX) { 00691 SERVO_dir = S_descente; 00692 SERVO_angle = SERVO_angleMAX; 00693 } else { 00694 SERVO_angle += 5; 00695 } 00696 } else { 00697 if (SERVO_angle <= SERVO_angleMIN) { 00698 SERVO_dir = S_monte; 00699 SERVO_angle = SERVO_angleMIN; 00700 } else { 00701 SERVO_angle -= 5; 00702 } 00703 } 00704 SERVO_pos = (lWord)(SERVO_angle*50/9) + 1000; 00705 Servo.pulsewidth_us(SERVO_pos); 00706 Pc.printf ("\rAngle = %4.1lf", SERVO_angle); 00707 } 00708 } while(!Pc.readable()); 00709 MENU_choix = Pc.getc(); 00710 break; 00711 00712 case 9 : 00713 Pc.printf ("\n\n\rPixy\n"); 00714 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00715 00716 if (Pixy_check == 0) Pc.printf ("\n\rPIXY is ALIVE\n"); 00717 else Pc.printf ("\n\rPIXY don't talk\n"); 00718 00719 do { 00720 if (FlagPixy) { 00721 00722 if (FlagPixyOverflow) { 00723 Pc.printf ("\rSome Data were lost\n"); 00724 FlagPixyOverflow = 0; 00725 } 00726 for (PIXY_objet = 0; PIXY_objet < Pixy_NMObjet; PIXY_objet++) { 00727 Pc.printf ("\rNMobj #%hd/%hd : sig = %hd : X=%5hd, Y=%5hd (W=%5hd, H=%5hd)\n", PIXY_objet+1, Pixy_NMObjet, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.signature, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.x, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.y, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.width, Pixy_NMFIFO[PIXY_rNMObjet].NMbloc.height); 00728 00729 if (PIXY_rNMObjet<19) PIXY_rNMObjet++; 00730 else PIXY_rNMObjet = 0; 00731 } 00732 Pixy_NMObjet = 0; 00733 00734 for (PIXY_objet = 0; PIXY_objet < Pixy_CCObjet; PIXY_objet++) { 00735 Pc.printf ("\rCCobj #%hd/%hd : sig = %hd : X=%5hd, Y=%5hd (W=%5hd, H=%5hd)\n", PIXY_objet+1, Pixy_CCObjet, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.signature, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.x, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.y, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.width, Pixy_CCFIFO[PIXY_rCCObjet].CCbloc.height); 00736 00737 if (PIXY_rCCObjet<19) PIXY_rCCObjet++; 00738 else PIXY_rCCObjet = 0; 00739 } 00740 val_W_balle = 0; 00741 val_H_balle = 0; 00742 val_carre_balle = 0; 00743 max_val_W_balle = 0; 00744 indice_balle = 0; 00745 for(i=0;i<14;i++){ 00746 val_W_balle=Pixy_NMFIFO[i].NMbloc.width; 00747 val_H_balle=Pixy_NMFIFO[i].NMbloc.height; 00748 00749 val_carre_balle = val_H_balle/val_W_balle; 00750 00751 val_x_balle =Pixy_NMFIFO[i].NMbloc.x; 00752 00753 00754 if(val_W_balle>max_val_W_balle){ 00755 max_val_W_balle=val_W_balle; 00756 indice_balle=i; 00757 if(val_x_balle>130 && val_x_balle<170){ 00758 En = 1; 00759 SensG=0; 00760 SensD=1; 00761 Pwm_MG=0.5; 00762 Pwm_MD=0.5; 00763 } 00764 else{ 00765 En = 0; 00766 } 00767 00768 } 00769 } 00770 Pc.printf("\rNMobj sig = %hd : X=%5hd,Y=%5hd (W=%5hd, H=%5hd)\n",Pixy_NMFIFO[indice_balle].NMbloc.signature,Pixy_NMFIFO[indice_balle].NMbloc.x,Pixy_NMFIFO[indice_balle].NMbloc.y,Pixy_NMFIFO[indice_balle].NMbloc.width,Pixy_NMFIFO[indice_balle].NMbloc.height); 00771 00772 Pixy_CCObjet = 0; 00773 Pc.printf("\n\r"); 00774 FlagPixy = 0; 00775 wait_ms(500); 00776 } 00777 00778 if (FlagTickLed) { 00779 Led1 = !Led1; 00780 FlagTickLed = 0; 00781 } 00782 } while(!Pc.readable()); 00783 MENU_choix = Pc.getc(); 00784 break; 00785 00786 } 00787 } 00788 }
Generated on Wed Aug 10 2022 04:58:17 by
1.7.2