Chafik ZERROUKI
hkb
Dependencies: Encoder_Nucleo_16_bits PwmIn mbed
Fork of
TestBoardv2_boussole_pixi
by 
ilan Sandoz
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 00024 // Comme la nucleo est Little Endian et que l'AS5047D est Big Endian, les codes sont inversés 00025 // Registre Valeur (BE) Valeur (LE) 00026 // NOP 0xC000 0x0003 00027 // ANGLE 0xFFFF 0xFFFF 00028 // ERROR 0x4001 0x8002 00029 // SETTING1 (R) 0xC018 0x1803 00030 // SETTING2 (R) 0x4019 0x8902 00031 // SETTING1 (W) 0x0018 0x1800 00032 // SETTING2 (W) 0x8019 0x9801 00033 00034 #define SPI_READ_NOP 0x0003 00035 #define SPI_READ_ANGLE 0xFFFF 00036 #define SPI_READ_ERROR 0x8002 00037 #define SPI_READ_SETTINGS1 0x1803 00038 #define SPI_READ_SETTINGS2 0x8902 00039 #define SPI_WRTIE_SETTINGS1 0x1800 00040 #define SPI_WRITE_SETTINGS2 0x9801 00041 00042 typedef unsigned char Byte; 00043 typedef unsigned short Word; 00044 typedef unsigned long lWord; 00045 00046 typedef enum {S_monte = 1, S_descente = 0} T_SERVODIR; 00047 00048 typedef enum {none, begin, normal, colorCode, doubleZero} T_pixyState; 00049 00050 typedef union { 00051 lWord mot; 00052 Byte tab[4]; 00053 } T_tmpBuffer; 00054 00055 typedef union { 00056 Word mot; 00057 Byte tab[2]; 00058 } T_structBuffer; 00059 00060 typedef struct { 00061 Word checksum; 00062 Word signature; 00063 Word x; 00064 Word y; 00065 Word width; 00066 Word height; 00067 Word angle; 00068 } T_pixyCCBloc; 00069 00070 typedef struct { 00071 Word checksum; 00072 Word signature; 00073 Word x; 00074 Word y; 00075 Word width; 00076 Word height; 00077 } T_pixyNMBloc; 00078 00079 typedef union { 00080 Byte tab[14]; 00081 T_pixyCCBloc CCbloc; 00082 } T_pixyCCData; 00083 00084 typedef union { 00085 Byte tab[12]; 00086 T_pixyNMBloc NMbloc; 00087 } T_pixyNMData; 00088 00089 00090 00091 /** Liste des objets 00092 * 00093 * Serial #4 Pixy 00094 * Serial #2 Pc 00095 * 00096 * AnalogIn C1, C2, C3, LD1, LD2, SD1, SD2, Vbat 00097 * 00098 * DigitalOut Led1, Led2, Trig1, Trig2, Trig3, En, SensG, SensD 00099 * 00100 * InterruptIn IndexG, IndexD, Echo1, Echo2, Echo3, BP 00101 * 00102 * PwmOut Pwm_MG, Pwm_MD, Servo 00103 * 00104 * PwmIn PWMG, PWMD, PWMB 00105 * 00106 * I2C Bus_I2C 00107 * 00108 * SPI MotG, MotD 00109 * 00110 * Nucleo_Encoder_16_bits Gauche, Droite 00111 * 00112 * Ticker timer 00113 */ 00114 00115 /** Liste des PINs 00116 * 00117 * PIN MAP (ordre alphabetique) des PINs de la Nucléo 64 utilisée 00118 * PA_0 -> Pixy RX (Serial) 00119 * PA_1 -> Pixy TX (Serial) 00120 * PA_2 -> PC TX (Serial) 00121 * PA_3 -> PX RX (Serial) 00122 * PA_4 -> GP2 SD #2 (Analog In) 00123 * PA_5 -> LED1 (Digital Out) 00124 * PA_6 -> CNY3 (Analog In) 00125 * PA_7 -> CNY2 (Analog In) 00126 * PA_8 -> Servomoteur (PWM Out) 00127 * PA_9 -> US Trigger #3 (Digital Out) 00128 * PA_10 -> US Echo #1 (Pwm In) 00129 * PA_11 -> US Echo #2 (Pwm In) 00130 * PA_12 -> SS (SPI Slave Select) (Digital Out) 00131 * PA_13 00132 * PA_14 00133 * PA_15 -> Boussole (Pwm In) 00134 * 00135 * PB_0 -> GP2 SD #1 (Analog In) 00136 * PB_1 -> Position D (Pwm In) 00137 * PB_2 -> Position G (Pwm In) 00138 * PB_3 -> PWM Mot D (PWM Out) 00139 * PB_4 -> Enocdeur Droit A (QE) 00140 * PB_5 -> Enocdeur Droit B (QE) 00141 * PB_6 -> Enocdeur Gauche A (QE) 00142 * PB_7 -> Enocdeur Gauche B (QE) 00143 * PB_8 -> SCL (I2C) 00144 * PB_9 -> SDA (I2C) 00145 * PB_10 -> PWM Mot G (PWM Out) 00146 * PB_11 00147 * PB_12 -> US Echo #3 (Pwm In) 00148 * PB_13 -> SCK Encodeur D (SPI) 00149 * PB_14 -> MISO Encodeur D (SPI) 00150 * PB_15 -> MOSI Encodeur D (SPI) 00151 * 00152 * PC_0 -> GP2 LD #1 (Analog In) 00153 * PC_1 -> GP2 LD #2 (Analog In) 00154 * PC_2 -> US Trigger #2 (Digital Out) 00155 * PC_3 -> US Trigger #1 (Digital Out) 00156 * PC_4 -> CNY1 (Analog In) 00157 * PC_5 -> Vbat (Analog In) 00158 * PC_6 -> Dir Mot Droit (Digital Out) 00159 * PC_7 -> I (Encodeur Gauche) (IRQ In) 00160 * PC_8 -> Dir Mot Gauche (Digital Out) 00161 * PC_9 -> Enable Moteurs (Digital Out) 00162 * PC_10 -> SCK Encodeur G (SPI) 00163 * PC_11 -> MISO Encodeur G (SPI) 00164 * PC_12 -> MOSI Encodeur G (SPI) 00165 * PC_13 -> User BP (IRQ In) 00166 * PC_14 00167 * PC_15 00168 * 00169 * PD_1 00170 * PD_2 -> Led2 (Digital Out) 00171 */ 00172 00173 Serial Pixy (PA_0, PA_1, 230400); 00174 Serial Pc (PA_2, PA_3, 921600); 00175 00176 AnalogIn CNY1 (PC_4); 00177 AnalogIn CNY2 (PA_7); 00178 AnalogIn CNY3 (PA_6); 00179 AnalogIn LD1 (PC_0); 00180 AnalogIn LD2 (PC_1); 00181 AnalogIn SD1 (PB_0); 00182 AnalogIn SD2 (PA_4); 00183 AnalogIn Vbat (PC_5); 00184 00185 DigitalOut Led1 (PA_5); 00186 DigitalOut Led2 (PD_2); 00187 DigitalOut Trig1 (PC_3); 00188 DigitalOut Trig2 (PC_2); 00189 DigitalOut Trig3 (PA_9); 00190 DigitalOut En (PC_9); 00191 DigitalOut SensG (PC_8); 00192 DigitalOut SensD (PC_6); 00193 DigitalOut SS (PA_12); 00194 00195 InterruptIn Echo1 (PA_10); 00196 InterruptIn Echo2 (PA_11); 00197 InterruptIn Echo3 (PB_12); 00198 InterruptIn BP (PC_13); 00199 InterruptIn IG (PC_7); 00200 InterruptIn PWMG (PB_2); 00201 InterruptIn PWMD (PB_1); 00202 00203 PwmIn PWMB (PA_15); 00204 00205 PwmOut Pwm_MG (PB_10); 00206 PwmOut Pwm_MD (PB_3); 00207 PwmOut Servo (PA_8); 00208 00209 I2C Bus_I2C (PB_9, PB_8); 00210 00211 SPI SPIG (PC_12, PC_11, PC_10); 00212 SPI SPID (PB_15, PB_14, PB_13); 00213 00214 Nucleo_Encoder_16_bits Gauche (TIM4); // A = PB_6, B = PB_7 00215 Nucleo_Encoder_16_bits Droite (TIM3); // A = PB_4, B = PB_5 00216 00217 Ticker tick; 00218 Timer times; 00219 00220 /** Liste des variables globales 00221 * 00222 * Tick -> (long) Compte le nombre de MS écoulé et déclenche l'exécution de la boucle en fonction du temps écoulé. 00223 * FlagIG -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue gauche 00224 * FlagID -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue droite 00225 * EchoXStart -> (long) Donne le temps en µs de début de l'impulsion d'écho de l'US n°X 00226 * DistUSX -> (float) Donne la distance en mm mesurée par l'US n°X 00227 */ 00228 00229 // Structure de temps 00230 lWord Tick = 0, Gperiod, Dperiod; 00231 00232 // Sémaphore d'interruption 00233 int FlagUS1 = 0, FlagUS2 = 0, FlagUS3 = 0, FlagPixy = 0, FlagPixyOverflow = 0; 00234 int FlagTick = 0, FlagTickLed = 0, BPPressed = 0, nbTurnG = 0, nbTurnD = 0; 00235 int Pixy_check = -1; 00236 char BOUSSOLE_status[1] = {0}; 00237 char I2C_registerValue[4]; 00238 00239 // Dialogue avec la Pixy 00240 T_pixyCCData Pixy_CCFIFO[20]; 00241 T_pixyNMData Pixy_NMFIFO[20]; 00242 Byte Pixy_CCObjet, Pixy_NMObjet; 00243 00244 // Gestion des capteurs Ultrason 00245 long Echo1Start, Echo2Start, Echo3Start; 00246 double DistUS1, DistUS2, DistUS3; 00247 int I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/; 00248 int SERVO_pulseWidth = 400, SERVO_max = 1400, SERVO_min = 400; 00249 T_SERVODIR SERVO_dir = S_monte; 00250 double MOTG_duty = 0.5, MOTD_duty = 0.5; 00251 double CAP_I2C, CAP_PWM; 00252 00253 /** Liste des interruptions 00254 * 00255 */ 00256 00257 void capteur_ultrason(void); 00258 void boussole(void); 00259 void capteur_infrarouge(void); 00260 void CNY(void); 00261 void servomoteur(void); 00262 void avancer(void); 00263 void tour_droit(void); 00264 void tour_gauche(void); 00265 00266 void tickTime() 00267 { 00268 Tick++; 00269 FlagTick = 1; 00270 if ((Tick%100)==0) FlagTickLed = 1; 00271 } 00272 00273 void BPevent () 00274 { 00275 BPPressed = 1; 00276 } 00277 00278 void PWM_motGRise (void) 00279 { 00280 static lWord oldTime; 00281 lWord now; 00282 now = times.read_us(); 00283 Gperiod = now-oldTime; 00284 oldTime = now; 00285 nbTurnG++; 00286 } 00287 00288 void PWM_motDRise (void) 00289 { 00290 static lWord oldTime; 00291 lWord now; 00292 now = times.read_us(); 00293 Dperiod = now-oldTime; 00294 oldTime = now; 00295 nbTurnD++; 00296 } 00297 00298 void echo1Rise () 00299 { 00300 Echo1Start = times.read_us(); 00301 } 00302 00303 void echo2Rise () 00304 { 00305 Echo2Start = times.read_us(); 00306 } 00307 00308 void echo3Rise () 00309 { 00310 Echo3Start = times.read_us(); 00311 } 00312 00313 void echo1Fall () 00314 { 00315 DistUS1 = (double)(times.read_us() - Echo1Start)/5.8; 00316 FlagUS1 = 1; 00317 } 00318 00319 void echo2Fall () 00320 { 00321 DistUS2 = (double)(times.read_us() - Echo2Start)/5.8; 00322 FlagUS2 = 1; 00323 } 00324 00325 void echo3Fall () 00326 { 00327 DistUS3 = (double)(times.read_us() - Echo3Start)/5.8; 00328 FlagUS3 = 1; 00329 } 00330 00331 void getPixyByte () 00332 { 00333 static T_tmpBuffer tmpBuffer; 00334 static T_structBuffer msgBuffer; 00335 static T_pixyState PIXY_state = none; 00336 static Byte byteCount = 0; 00337 static int PIXY_synced = 0, dummy; 00338 int i, somme; 00339 static Byte PIXY_nbCCObjet = 0, PIXY_wCCObjet = 0; 00340 static Byte PIXY_nbNMObjet = 0, PIXY_wNMObjet = 0; 00341 00342 Pixy_check = 0; 00343 00344 if (!PIXY_synced) { // On n'a pas trouvé le START (0x55aa0000) 00345 tmpBuffer.tab[byteCount] = Pixy.getc(); // On stocke l'octet reçu dans la première case dispo du tableau temporaire 00346 00347 if (byteCount < 3) { // Si on n'a pas encore reçu les 4 premier octets 00348 byteCount++; // On passe à la case suivante du tableau temporaire 00349 } else { // Lorsqu'on a 4 octets 00350 if (tmpBuffer.mot != 0xaa550000) { // Si le code n'est pas le code de START 00351 for (i=1; i<4; i++) tmpBuffer.tab[i-1] = tmpBuffer.tab[i]; // On décalle les cases du tableau 00352 byteCount = 3; // Et on attend le caractère suivant 00353 } else { // Si on a trouvé le code de START 00354 PIXY_synced = 1; // On passe en mode synchronisé 00355 PIXY_state = begin; 00356 byteCount = 0; 00357 } 00358 } 00359 } 00360 00361 if (PIXY_synced) { 00362 00363 switch (PIXY_state) { 00364 case begin : // l'aiguillage est là ! 00365 00366 msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus 00367 byteCount++; 00368 if (byteCount == 2) { // Quand on a 2 octets 00369 00370 if (msgBuffer.mot == 0xaa55) { // Si c'est un bloc normal (code 0xAA55) 00371 PIXY_state = normal; // On part vers le traitement spécifique 00372 } 00373 00374 if (msgBuffer.mot == 0xaa56) { // Si c'est un bloc Color Code (code 0xAA56) 00375 PIXY_state = colorCode; // On part vers le traitement spécifique 00376 } 00377 00378 if (msgBuffer.mot == 0) { // Si on a un debut de trame (code 0000) 00379 PIXY_state = doubleZero; // On part vers le traitement spécifique 00380 } 00381 if ((PIXY_state == begin) || (PIXY_state == none)) { // Si c'est autre chose 00382 PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé. 00383 PIXY_state = none; // Ceinture et bretelle 00384 } 00385 byteCount = 0; 00386 } 00387 break; 00388 00389 case normal : // Si on a un bloc normal 00390 00391 Pixy_NMFIFO[PIXY_wNMObjet].tab[byteCount] = Pixy.getc(); // On stocke les octets un à un dans la structure Bloc 00392 if (byteCount < 11) { // Tant que la structure n'est pas pleine 00393 byteCount++; // On passe à l'octet suivant 00394 } else { // Quand elle est pleine 00395 byteCount = 0; // On réinitialise 00396 PIXY_state = begin; // On retourne à l'aiguillage 00397 // On calcule la somme de contrôle 00398 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; 00399 00400 if (somme == Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.checksum) { // Si le checksum est bon, on valide la réception 00401 if (PIXY_wNMObjet < 19) PIXY_wNMObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet 00402 else PIXY_wNMObjet = 0; 00403 if (PIXY_nbNMObjet < 19) PIXY_nbNMObjet++; // On dit que l'on a un objet CC de plus 00404 else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow 00405 } 00406 } 00407 break; 00408 00409 case colorCode : // Si on a un bloc colorCode 00410 00411 Pixy_CCFIFO[PIXY_wCCObjet].tab[byteCount] = dummy; // On stocke les octets un à un dans la structure CCBloc 00412 if (byteCount < 13) byteCount++; // tant que la structure n'est pas pleine on passe à l'octet suivant 00413 else { // Quand elle est pleine 00414 byteCount = 0; // On réinitialise 00415 PIXY_state = begin; // On retourne à l'aiguillage 00416 // On calcule la somme de contrôle 00417 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; 00418 00419 if (somme == Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.checksum) { // Si le checksum est bon 00420 if (PIXY_wCCObjet < 19) PIXY_wCCObjet++; // On incrémente le pointeur d'écriture dans la FIFO CCObjet 00421 else PIXY_wCCObjet = 0; 00422 if (PIXY_nbCCObjet < 19) PIXY_nbCCObjet++; // On dit que l'on a un objet CC de plus à traiter 00423 else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow 00424 } 00425 } 00426 break; 00427 00428 case doubleZero : // Si on a reçu le code de début d'une nouvelle trame. 00429 00430 msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus 00431 byteCount++; 00432 if (byteCount == 2) { // Quand on a 2 octets 00433 if (msgBuffer.mot == 0xaa55) { // On doit impérativement trouver le code 0xAA55 00434 PIXY_state = begin; // Si c'est le cas, alors tout va bien et on va à l'aiguillage 00435 Pixy_NMObjet = PIXY_nbNMObjet; // On met à jour les variables pour le traitement 00436 Pixy_CCObjet = PIXY_nbCCObjet; 00437 PIXY_nbCCObjet = 0; 00438 PIXY_nbNMObjet = 0; 00439 FlagPixy = 1; // On valide le traitement de la trame précédente. 00440 } else { // Si on trouve autre chose 00441 PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé. 00442 PIXY_state = none; // Ceinture et bretelle 00443 } 00444 byteCount = 0; 00445 } 00446 break; 00447 } 00448 } 00449 } 00450 00451 00452 int main() 00453 { 00454 00455 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, A=1; 00456 00457 Byte PIXY_rCCObjet = 0, PIXY_rNMObjet = 0; 00458 int PIXY_objet; 00459 00460 00461 00462 char MENU_choix = 0; 00463 00464 00465 double BOUSSOLE_periode; 00466 00467 double Vbat_val; 00468 00469 00470 00471 00472 int MOTG_evol = 1, MOTD_evol = 1; 00473 00474 00475 times.reset(); 00476 times.start(); 00477 00478 // Initialisation des interruptions 00479 tick.attach(&tickTime, 0.001); 00480 00481 BP.rise (&BPevent); 00482 00483 Echo1.rise (&echo1Rise); 00484 Echo2.rise (&echo2Rise); 00485 Echo3.rise (&echo3Rise); 00486 Echo1.fall (&echo1Fall); 00487 Echo2.fall (&echo2Fall); 00488 Echo3.fall (&echo3Fall); 00489 00490 PWMG.rise (&PWM_motGRise); 00491 PWMD.rise (&PWM_motDRise); 00492 00493 Pixy.attach (&getPixyByte); 00494 00495 BP.enable_irq(); 00496 IG.enable_irq(); 00497 Echo1.enable_irq(); 00498 Echo2.enable_irq(); 00499 Echo3.enable_irq(); 00500 PWMG.enable_irq(); 00501 PWMD.enable_irq(); 00502 00503 // Initialisation des périphériques 00504 // Bus I2C 00505 Bus_I2C.frequency (100000); 00506 00507 // PWM des moteurs 00508 Pwm_MG.period_us(50); 00509 Pwm_MD.period_us(50); 00510 En = 0; 00511 00512 // Bus SPI 00513 SPIG.format (16,1); 00514 SPIG.frequency (1000000); 00515 00516 SPID.format (16,1); 00517 SPID.frequency (1000000); 00518 00519 SS = 1; 00520 00521 // Led 00522 Led2 = 0; 00523 00524 Servo.period_ms (20); 00525 Servo.pulsewidth_us(200); 00526 00527 while(1) { 00528 00529 do { 00530 Led1 = 0; 00531 Pc.printf ("\n\n\n\n\rProgramme de test\n\n\rEntrez le code du test a effectuer :\n\n"); 00532 Pc.printf ("\r1- Capteurs Ultra Son (les 3)\n"); 00533 Pc.printf ("\r2- Boussole et I2C\n"); 00534 Pc.printf ("\r3- Capteurs GP2 (les 4)\n"); 00535 Pc.printf ("\r4- Capteurs CNY70 (les 3)\n"); 00536 Pc.printf ("\r5- VBAT \t! erreur de composant la mesure est fausse !\n"); 00537 Pc.printf ("\r6- Moteur Gauche\n"); 00538 Pc.printf ("\r7- Moteur Droit\n"); 00539 Pc.printf ("\r8- Servomoteur\n"); 00540 Pc.printf ("\r9- PIXY (CMUCAM5)\n"); 00541 MENU_choix = Pc.getc (); 00542 } while (((MENU_choix-'0')<1) || ((MENU_choix-'0')>9)); 00543 00544 switch (MENU_choix-'0') { 00545 00546 case 1 : 00547 Pc.printf ("\n\n\rTest des captreurs Ultrason\n"); 00548 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00549 do 00550 { 00551 if(CNY1>0.5 && A==1) { 00552 avancer(); 00553 } 00554 else 00555 { 00556 En = 0; 00557 A=0; 00558 } 00559 } 00560 while(!Pc.readable()); 00561 MENU_choix = Pc.getc(); 00562 break; 00563 00564 case 2 : 00565 Pc.printf ("\n\n\rBoussole\n"); 00566 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00567 00568 Pc.printf ("\n\rVerif du bus I2C :"); 00569 I2C_check = Bus_I2C.write (BOUSSOLE_adress,BOUSSOLE_status,1,false); 00570 if (I2C_check==0) { 00571 Pc.printf (" OK\n"); 00572 Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true); 00573 Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4); 00574 Pc.printf ("\rVersion Firmware boussole : %03d\n", I2C_registerValue[0]); 00575 } else { 00576 Pc.printf (" FAIL\n"); 00577 } 00578 00579 BOUSSOLE_periode = PWMB.period(); 00580 Pc.printf ("\rVerif de la PWM :"); 00581 if ((BOUSSOLE_periode > 0.11) || (BOUSSOLE_periode < 0.06)) { 00582 Pc.printf (" FAIL\n\n"); 00583 } else { 00584 Pc.printf (" OK\n\n"); 00585 BOUSSOLE_check = 0; 00586 } 00587 00588 do { 00589 boussole(); 00590 if(CAP_PWM<160) 00591 { 00592 tour_droit(); 00593 } 00594 else if(CAP_PWM>230) 00595 { 00596 tour_gauche(); 00597 } 00598 else 00599 { 00600 avancer(); 00601 } 00602 } while(!Pc.readable()); 00603 MENU_choix = Pc.getc(); 00604 En=0; 00605 break; 00606 00607 case 3 : 00608 Pc.printf ("\n\n\rGP2xx\n"); 00609 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00610 do { 00611 capteur_infrarouge(); 00612 00613 } while(!Pc.readable()); 00614 MENU_choix = Pc.getc(); 00615 break; 00616 00617 case 4 : 00618 Pc.printf ("\n\n\rCNY70\n"); 00619 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00620 do { 00621 CNY(); 00622 } while(!Pc.readable()); 00623 MENU_choix = Pc.getc(); 00624 break; 00625 00626 case 5 : 00627 Pc.printf ("\n\n\rVbat\n"); 00628 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00629 do { 00630 if (FlagTickLed) { 00631 Led1 = !Led1; 00632 FlagTickLed = 0; 00633 00634 Vbat_val = Vbat; 00635 Pc.printf ("\rVbat = %5.3lf V", Vbat_val*3.3*4.3); 00636 } 00637 } while(!Pc.readable()); 00638 MENU_choix = Pc.getc(); 00639 break; 00640 00641 case 6 : 00642 Pc.printf ("\n\n\rMoteur Gauche\n"); 00643 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00644 00645 En = 1; 00646 SensG = 1; 00647 Led2 = 1; 00648 MOTG_duty = 0.5; 00649 MOTG_evol = 1; 00650 Pwm_MG = 0.5; 00651 00652 do { 00653 if (FlagTickLed) { 00654 Led1 = !Led1; 00655 FlagTickLed = 0; 00656 } 00657 if (BPPressed) { 00658 BPPressed = 0; 00659 SensG = !SensG; 00660 Led2 = !Led2; 00661 } 00662 00663 if (((Tick%1000)==0) && FlagTick) { 00664 FlagTick = 0; 00665 if (MOTG_evol) { 00666 if (MOTG_duty < 0.9) MOTG_duty += 0.1; 00667 else { 00668 MOTG_evol = 0; 00669 MOTG_duty = 0.9; 00670 } 00671 } else { 00672 if (MOTG_duty > 0.1) MOTG_duty -= 0.1; 00673 else { 00674 MOTG_evol = 1; 00675 MOTG_duty = 0.1; 00676 } 00677 } 00678 } 00679 00680 Pwm_MG = MOTG_duty; 00681 Pc.printf ("\rPWM = %2.1lf => Pos = %10ld Tick, Period = %ldus", MOTG_duty, Gauche.GetCounter(), Gperiod); 00682 00683 00684 } while(!Pc.readable()); 00685 MENU_choix = Pc.getc(); 00686 00687 En = 0; 00688 if (SensG == 0) Pwm_MG = 0; 00689 else Pwm_MG = 1; 00690 00691 break; 00692 00693 case 7 : 00694 Pc.printf ("\n\n\rMoteur Droit\n"); 00695 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00696 00697 En = 1; 00698 SensD = 1; 00699 Led2 = 1; 00700 MOTD_duty = 0.5; 00701 MOTD_evol = 1; 00702 Pwm_MD = 0.5; 00703 00704 do { 00705 if (FlagTickLed) { 00706 Led1 = !Led1; 00707 FlagTickLed = 0; 00708 } 00709 if (BPPressed) { 00710 BPPressed = 0; 00711 SensD = !SensD; 00712 Led2 = !Led2; 00713 } 00714 00715 if (((Tick%1000)==0) && FlagTick) { 00716 FlagTick = 0; 00717 if (MOTD_evol) { 00718 if (MOTD_duty < 0.9) MOTD_duty += 0.1; 00719 else { 00720 MOTD_evol = 0; 00721 MOTD_duty = 0.9; 00722 } 00723 } else { 00724 if (MOTD_duty > 0.1) MOTD_duty -= 0.1; 00725 else { 00726 MOTD_evol = 1; 00727 MOTD_duty = 0.1; 00728 } 00729 } 00730 } 00731 Pwm_MD = MOTD_duty; 00732 Pc.printf ("\rPWM = %2.1lf => Pos = %10ld Tick, Period = %ldus", MOTD_duty, Droite.GetCounter(), Dperiod); 00733 00734 } while(!Pc.readable()); 00735 MENU_choix = Pc.getc(); 00736 00737 En = 0; 00738 if (SensD == 0) Pwm_MD = 0; 00739 else Pwm_MD = 1; 00740 00741 break; 00742 00743 case 8 : 00744 Pc.printf ("\n\n\rServo Moteur\n"); 00745 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00746 00747 do { 00748 servomoteur(); 00749 } while(!Pc.readable()); 00750 MENU_choix = Pc.getc(); 00751 00752 En = 0; 00753 00754 break; 00755 00756 case 9 : 00757 Pc.printf ("\n\n\rPixy\n"); 00758 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00759 00760 if (Pixy_check == 0) Pc.printf ("\n\rPIXY is ALIVE\n"); 00761 else Pc.printf ("\n\rPIXY don't talk\n"); 00762 00763 do { 00764 if (FlagPixy) { 00765 00766 if (FlagPixyOverflow) { 00767 Pc.printf ("\rSome Data were lost\n"); 00768 FlagPixyOverflow = 0; 00769 } 00770 for (PIXY_objet = 0; PIXY_objet < Pixy_NMObjet; PIXY_objet++) { 00771 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); 00772 00773 if (PIXY_rNMObjet<19) PIXY_rNMObjet++; 00774 else PIXY_rNMObjet = 0; 00775 } 00776 Pixy_NMObjet = 0; 00777 00778 for (PIXY_objet = 0; PIXY_objet < Pixy_CCObjet; PIXY_objet++) { 00779 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); 00780 00781 if (PIXY_rCCObjet<19) PIXY_rCCObjet++; 00782 else PIXY_rCCObjet = 0; 00783 } 00784 val_W_balle = 0; 00785 val_H_balle = 0; 00786 val_carre_balle = 0; 00787 max_val_W_balle = 0; 00788 indice_balle = 0; 00789 for(i=0;i<14;i++){ 00790 val_W_balle=Pixy_NMFIFO[i].NMbloc.width; 00791 val_H_balle=Pixy_NMFIFO[i].NMbloc.height; 00792 00793 val_carre_balle = val_H_balle/val_W_balle; 00794 00795 val_x_balle =Pixy_NMFIFO[i].NMbloc.x; 00796 00797 00798 if(val_W_balle>max_val_W_balle){ 00799 max_val_W_balle=val_W_balle; 00800 indice_balle=i; 00801 } 00802 if(val_x_balle>130 && val_x_balle<170){ 00803 avancer(); 00804 } 00805 else{ 00806 En = 0; 00807 } 00808 00809 00810 } 00811 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); 00812 00813 Pixy_CCObjet = 0; 00814 Pc.printf("\n\r"); 00815 FlagPixy = 0; 00816 wait_ms(500); 00817 } 00818 00819 if (FlagTickLed) { 00820 Led1 = !Led1; 00821 FlagTickLed = 0; 00822 } 00823 } while(!Pc.readable()); 00824 MENU_choix = Pc.getc(); 00825 break; 00826 00827 } 00828 } 00829 } 00830 00831 void capteur_ultrason(void) 00832 { 00833 if (FlagTickLed) { 00834 Led1 = !Led1; 00835 FlagTickLed = 0; 00836 } 00837 00838 // Gestion des US 00839 if (((Tick%150)==0) && FlagTick) { 00840 Trig1 = 1; 00841 wait_us(20); 00842 Trig1 = 0; 00843 FlagTick = 0; 00844 } 00845 00846 if (((Tick%150)==50) && FlagTick) { 00847 Trig2 = 1; 00848 wait_us(20); 00849 Trig2 = 0; 00850 FlagTick = 0; 00851 } 00852 00853 if (((Tick%150)==100) && FlagTick) { 00854 Trig3 = 1; 00855 wait_us(20); 00856 Trig3 = 0; 00857 FlagTick = 0; 00858 } 00859 00860 if (FlagUS1==1) { 00861 Pc.printf ("\rUS 1 = %04d mm", (int)DistUS1); 00862 FlagUS1 = 0; 00863 } 00864 00865 if (FlagUS2==1) { 00866 Pc.printf ("\r\t\t\tUS 2 = %04d mm", (int)DistUS2); 00867 FlagUS2 = 0; 00868 } 00869 00870 if (FlagUS3==1) { 00871 Pc.printf ("\r\t\t\t\t\t\tUS 3 = %04d mm", (int)DistUS3); 00872 FlagUS3 = 0; 00873 } 00874 } 00875 00876 void boussole(void) 00877 { 00878 00879 00880 00881 if (FlagTickLed) { 00882 Led1 = !Led1; 00883 FlagTickLed = 0; 00884 } 00885 00886 if (((Tick%150)==0) && FlagTick) { 00887 FlagTick = 0; 00888 if (BOUSSOLE_check==0) { 00889 CAP_PWM = ((PWMB.pulsewidth()*1000)-1)*10; 00890 Pc.printf ("\r PWM = %4.1lf", CAP_PWM); 00891 } 00892 if (I2C_check==0) { 00893 Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true); 00894 Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4); 00895 CAP_I2C = (double)(((unsigned short)I2C_registerValue[2]<<8)+(unsigned short)I2C_registerValue[3])/10.0; 00896 Pc.printf ("\r\t\t I2C = %4.1lf", CAP_I2C); 00897 } 00898 } 00899 } 00900 00901 void capteur_infrarouge(void) 00902 { 00903 double SD1_val, SD2_val, LD1_val, LD2_val; 00904 double SD1_dist, SD2_dist, LD1_dist, LD2_dist; 00905 00906 if (FlagTickLed) { 00907 Led1 = !Led1; 00908 FlagTickLed = 0; 00909 00910 SD1_val = SD1; 00911 SD2_val = SD2; 00912 LD1_val = LD1; 00913 LD2_val = LD2; 00914 00915 if (SD1_val < 0.06) { 00916 SD1_val = 0; 00917 SD1_dist = 40; 00918 } else { 00919 SD1_dist = 11.611/(SD1_val*3.3-0.0237); 00920 if (SD1_dist > 40) SD1_dist = 40; 00921 } 00922 00923 if (SD2_val < 0.06) { 00924 SD2_val = 0; 00925 SD2_dist = 40; 00926 } else { 00927 SD2_dist = 11.611/(SD2_val*3.3-0.0237); 00928 if (SD2_dist > 40) SD2_dist = 40; 00929 } 00930 00931 if (LD1_val < 0.1) { 00932 LD1_val = 0; 00933 LD1_dist = 150; 00934 } else { 00935 LD1_dist = 59.175/(LD1_val*3.3-0.0275); 00936 if (LD1_dist > 150) LD1_dist = 150; 00937 } 00938 00939 if (LD2_val < 0.1) { 00940 LD2_val = 0; 00941 LD2_dist = 150; 00942 } else { 00943 LD2_dist = 59.175/(LD2_val*3.3-0.0275); 00944 if (LD2_dist > 150) LD2_dist = 150; 00945 } 00946 00947 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); 00948 } 00949 } 00950 00951 void CNY(void) 00952 { 00953 double CNY1_val, CNY2_val, CNY3_val; 00954 00955 if (FlagTickLed) { 00956 Led1 = !Led1; 00957 FlagTickLed = 0; 00958 00959 CNY1_val = CNY1; 00960 CNY2_val = CNY2; 00961 CNY3_val = CNY3; 00962 00963 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); 00964 } 00965 } 00966 00967 00968 void servomoteur(void) 00969 { 00970 00971 00972 if (FlagTickLed) { 00973 Led1 = !Led1; 00974 FlagTickLed = 0; 00975 } 00976 00977 if (((Tick%250)==0) && FlagTick) { 00978 FlagTick = 0; 00979 if (SERVO_dir == S_monte) { 00980 if (SERVO_pulseWidth < (SERVO_max - 100)) SERVO_pulseWidth +=100; 00981 else { 00982 SERVO_dir = S_descente; 00983 SERVO_pulseWidth = SERVO_max; 00984 } 00985 } else { 00986 if (SERVO_pulseWidth > (SERVO_min + 100)) SERVO_pulseWidth -=100; 00987 else { 00988 SERVO_dir = S_monte; 00989 SERVO_pulseWidth = SERVO_min; 00990 } 00991 } 00992 00993 Servo.pulsewidth_us (SERVO_pulseWidth); 00994 Pc.printf ("\rPulse = %d",SERVO_pulseWidth); 00995 } 00996 } 00997 00998 void avancer(void) 00999 { 01000 En = 1; 01001 SensD = 1; 01002 Led2 = 1; 01003 MOTD_duty = 0.2; 01004 SensG = 0; 01005 MOTG_duty = 0.87; 01006 01007 Pwm_MG = MOTG_duty; 01008 Pwm_MD = MOTD_duty; 01009 } 01010 void tour_droit(void) 01011 { 01012 En = 1; 01013 SensD = 0; 01014 Led2 = 1; 01015 MOTD_duty = 0.5; 01016 Pwm_MD = 0.3; 01017 SensG = 0; 01018 MOTG_duty = 0.5; 01019 Pwm_MG = 0.3; 01020 01021 Pwm_MG = MOTG_duty; 01022 Pwm_MD = MOTD_duty; 01023 } 01024 void tour_gauche(void) 01025 { 01026 En = 1; 01027 SensD = 1; 01028 Led2 = 1; 01029 MOTD_duty = 0.5; 01030 Pwm_MD = 0.5; 01031 SensG = 1; 01032 MOTG_duty = 0.5; 01033 Pwm_MG = 0.5; 01034 01035 Pwm_MG = MOTG_duty; 01036 Pwm_MD = MOTD_duty; 01037 }
Generated on Fri Jul 15 2022 18:13:03 by
1.7.2