ilan Sandoz
dfgqf
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 ; 00456 double mesure_angle_pixy, val_carre_balle=0,val_x_balle=0; 00457 00458 Byte PIXY_rCCObjet = 0, PIXY_rNMObjet = 0; 00459 int PIXY_objet; 00460 00461 00462 00463 char MENU_choix = 0; 00464 00465 00466 double BOUSSOLE_periode; 00467 00468 double Vbat_val; 00469 00470 00471 00472 00473 int MOTG_evol = 1, MOTD_evol = 1; 00474 00475 00476 times.reset(); 00477 times.start(); 00478 00479 // Initialisation des interruptions 00480 tick.attach(&tickTime, 0.001); 00481 00482 BP.rise (&BPevent); 00483 00484 Echo1.rise (&echo1Rise); 00485 Echo2.rise (&echo2Rise); 00486 Echo3.rise (&echo3Rise); 00487 Echo1.fall (&echo1Fall); 00488 Echo2.fall (&echo2Fall); 00489 Echo3.fall (&echo3Fall); 00490 00491 PWMG.rise (&PWM_motGRise); 00492 PWMD.rise (&PWM_motDRise); 00493 00494 Pixy.attach (&getPixyByte); 00495 00496 BP.enable_irq(); 00497 IG.enable_irq(); 00498 Echo1.enable_irq(); 00499 Echo2.enable_irq(); 00500 Echo3.enable_irq(); 00501 PWMG.enable_irq(); 00502 PWMD.enable_irq(); 00503 00504 // Initialisation des périphériques 00505 // Bus I2C 00506 Bus_I2C.frequency (100000); 00507 00508 // PWM des moteurs 00509 Pwm_MG.period_us(50); 00510 Pwm_MD.period_us(50); 00511 En = 0; 00512 00513 // Bus SPI 00514 SPIG.format (16,1); 00515 SPIG.frequency (1000000); 00516 00517 SPID.format (16,1); 00518 SPID.frequency (1000000); 00519 00520 SS = 1; 00521 00522 // Led 00523 Led2 = 0; 00524 00525 Servo.period_ms (20); 00526 Servo.pulsewidth_us(200); 00527 00528 while(1) { 00529 00530 do { 00531 Led1 = 0; 00532 Pc.printf ("\n\n\n\n\rProgramme de test\n\n\rEntrez le code du test a effectuer :\n\n"); 00533 Pc.printf ("\r1- Capteurs Ultra Son (les 3)\n"); 00534 Pc.printf ("\r2- Boussole et I2C\n"); 00535 Pc.printf ("\r3- Capteurs GP2 (les 4)\n"); 00536 Pc.printf ("\r4- Capteurs CNY70 (les 3)\n"); 00537 Pc.printf ("\r5- VBAT \t! erreur de composant la mesure est fausse !\n"); 00538 Pc.printf ("\r6- Moteur Gauche\n"); 00539 Pc.printf ("\r7- Moteur Droit\n"); 00540 Pc.printf ("\r8- Servomoteur\n"); 00541 Pc.printf ("\r9- PIXY (CMUCAM5)\n"); 00542 MENU_choix = Pc.getc (); 00543 } while (((MENU_choix-'0')<1) || ((MENU_choix-'0')>9)); 00544 00545 switch (MENU_choix-'0') { 00546 00547 case 1 : 00548 Pc.printf ("\n\n\rTest des captreurs Ultrason\n"); 00549 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00550 do 00551 { 00552 capteur_ultrason(); 00553 } 00554 while(!Pc.readable()); 00555 MENU_choix = Pc.getc(); 00556 break; 00557 00558 case 2 : 00559 Pc.printf ("\n\n\rBoussole\n"); 00560 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00561 00562 Pc.printf ("\n\rVerif du bus I2C :"); 00563 I2C_check = Bus_I2C.write (BOUSSOLE_adress,BOUSSOLE_status,1,false); 00564 if (I2C_check==0) { 00565 Pc.printf (" OK\n"); 00566 Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true); 00567 Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4); 00568 Pc.printf ("\rVersion Firmware boussole : %03d\n", I2C_registerValue[0]); 00569 } else { 00570 Pc.printf (" FAIL\n"); 00571 } 00572 00573 BOUSSOLE_periode = PWMB.period(); 00574 Pc.printf ("\rVerif de la PWM :"); 00575 if ((BOUSSOLE_periode > 0.11) || (BOUSSOLE_periode < 0.06)) { 00576 Pc.printf (" FAIL\n\n"); 00577 } else { 00578 Pc.printf (" OK\n\n"); 00579 BOUSSOLE_check = 0; 00580 } 00581 00582 do { 00583 boussole(); 00584 if(CAP_PWM<160) 00585 { 00586 tour_droit(); 00587 } 00588 else if(CAP_PWM>200) 00589 { 00590 tour_gauche(); 00591 } 00592 else 00593 { 00594 avancer(); 00595 } 00596 } while(!Pc.readable()); 00597 MENU_choix = Pc.getc(); 00598 En=0; 00599 break; 00600 00601 case 3 : 00602 Pc.printf ("\n\n\rGP2xx\n"); 00603 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00604 do { 00605 capteur_infrarouge(); 00606 00607 } while(!Pc.readable()); 00608 MENU_choix = Pc.getc(); 00609 break; 00610 00611 case 4 : 00612 Pc.printf ("\n\n\rCNY70\n"); 00613 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00614 do { 00615 00616 if (FlagPixy) { 00617 00618 if (FlagPixyOverflow) { 00619 Pc.printf ("\rSome Data were lost\n"); 00620 FlagPixyOverflow = 0; 00621 } 00622 for (PIXY_objet = 0; PIXY_objet < Pixy_NMObjet; PIXY_objet++) { 00623 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); 00624 00625 if (PIXY_rNMObjet<19) PIXY_rNMObjet++; 00626 else PIXY_rNMObjet = 0; 00627 } 00628 Pixy_NMObjet = 0; 00629 00630 for (PIXY_objet = 0; PIXY_objet < Pixy_CCObjet; PIXY_objet++) { 00631 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); 00632 00633 if (PIXY_rCCObjet<19) PIXY_rCCObjet++; 00634 else PIXY_rCCObjet = 0; 00635 } 00636 00637 i=0; 00638 do{ 00639 //test carre +/-5% 00640 val_carre_balle = Pixy_NMFIFO[i].NMbloc.height/Pixy_NMFIFO[i].NMbloc.width; 00641 00642 if ((0.95<val_carre_balle)&&(val_carre_balle<1.05)) 00643 { 00644 val_x_balle =Pixy_NMFIFO[i].NMbloc.x; 00645 Pc.printf("\n balle0"); 00646 } 00647 i++; 00648 00649 mesure_angle_pixy=(val_x_balle-160)*0.234375; 00650 Pc.printf("\r x_balle : %lf ° ",val_x_balle); 00651 }while((val_x_balle==0)||(i<20)); 00652 00653 Pixy_CCObjet = 0; 00654 Pc.printf("\n\r"); 00655 FlagPixy = 0; 00656 wait_ms(200); 00657 00658 00659 } 00660 00661 /*if(val_x_balle>190)tour_gauche(); 00662 00663 else if(val_x_balle<130) tour_droit(); 00664 00665 else avancer();*/ 00666 00667 00668 } while(!Pc.readable()); 00669 MENU_choix = Pc.getc(); 00670 En=0; 00671 break; 00672 00673 case 5 : 00674 Pc.printf ("\n\n\rVbat\n"); 00675 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00676 do { 00677 if (FlagTickLed) { 00678 Led1 = !Led1; 00679 FlagTickLed = 0; 00680 00681 Vbat_val = Vbat; 00682 Pc.printf ("\rVbat = %5.3lf V", Vbat_val*3.3*4.3); 00683 } 00684 } while(!Pc.readable()); 00685 MENU_choix = Pc.getc(); 00686 break; 00687 00688 case 6 : 00689 Pc.printf ("\n\n\rMoteur Gauche\n"); 00690 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00691 00692 En = 1; 00693 SensG = 1; 00694 Led2 = 1; 00695 MOTG_duty = 0.5; 00696 MOTG_evol = 1; 00697 Pwm_MG = 0.5; 00698 00699 do { 00700 if (FlagTickLed) { 00701 Led1 = !Led1; 00702 FlagTickLed = 0; 00703 } 00704 if (BPPressed) { 00705 BPPressed = 0; 00706 SensG = !SensG; 00707 Led2 = !Led2; 00708 } 00709 00710 if (((Tick%1000)==0) && FlagTick) { 00711 FlagTick = 0; 00712 if (MOTG_evol) { 00713 if (MOTG_duty < 0.9) MOTG_duty += 0.1; 00714 else { 00715 MOTG_evol = 0; 00716 MOTG_duty = 0.9; 00717 } 00718 } else { 00719 if (MOTG_duty > 0.1) MOTG_duty -= 0.1; 00720 else { 00721 MOTG_evol = 1; 00722 MOTG_duty = 0.1; 00723 } 00724 } 00725 } 00726 00727 Pwm_MG = MOTG_duty; 00728 Pc.printf ("\rPWM = %2.1lf => Pos = %10ld Tick, Period = %ldus", MOTG_duty, Gauche.GetCounter(), Gperiod); 00729 00730 00731 } while(!Pc.readable()); 00732 MENU_choix = Pc.getc(); 00733 00734 En = 0; 00735 if (SensG == 0) Pwm_MG = 0; 00736 else Pwm_MG = 1; 00737 00738 break; 00739 00740 case 7 : 00741 Pc.printf ("\n\n\rMoteur Droit\n"); 00742 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00743 00744 En = 1; 00745 SensD = 1; 00746 Led2 = 1; 00747 MOTD_duty = 0.5; 00748 MOTD_evol = 1; 00749 Pwm_MD = 0.5; 00750 00751 do { 00752 if (FlagTickLed) { 00753 Led1 = !Led1; 00754 FlagTickLed = 0; 00755 } 00756 if (BPPressed) { 00757 BPPressed = 0; 00758 SensD = !SensD; 00759 Led2 = !Led2; 00760 } 00761 00762 if (((Tick%1000)==0) && FlagTick) { 00763 FlagTick = 0; 00764 if (MOTD_evol) { 00765 if (MOTD_duty < 0.9) MOTD_duty += 0.1; 00766 else { 00767 MOTD_evol = 0; 00768 MOTD_duty = 0.9; 00769 } 00770 } else { 00771 if (MOTD_duty > 0.1) MOTD_duty -= 0.1; 00772 else { 00773 MOTD_evol = 1; 00774 MOTD_duty = 0.1; 00775 } 00776 } 00777 } 00778 Pwm_MD = MOTD_duty; 00779 Pc.printf ("\rPWM = %2.1lf => Pos = %10ld Tick, Period = %ldus", MOTD_duty, Droite.GetCounter(), Dperiod); 00780 00781 } while(!Pc.readable()); 00782 MENU_choix = Pc.getc(); 00783 00784 En = 0; 00785 if (SensD == 0) Pwm_MD = 0; 00786 else Pwm_MD = 1; 00787 00788 break; 00789 00790 case 8 : 00791 Pc.printf ("\n\n\rServo Moteur\n"); 00792 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00793 00794 do { 00795 servomoteur(); 00796 } while(!Pc.readable()); 00797 MENU_choix = Pc.getc(); 00798 00799 En = 0; 00800 00801 break; 00802 00803 case 9 : 00804 Pc.printf ("\n\n\rPixy\n"); 00805 Pc.printf ("\rAppuyez sur Entree pour quitter\n"); 00806 00807 if (Pixy_check == 0) Pc.printf ("\n\rPIXY is ALIVE\n"); 00808 else Pc.printf ("\n\rPIXY don't talk\n"); 00809 00810 do { 00811 if (FlagPixy) { 00812 00813 if (FlagPixyOverflow) { 00814 Pc.printf ("\rSome Data were lost\n"); 00815 FlagPixyOverflow = 0; 00816 } 00817 for (PIXY_objet = 0; PIXY_objet < Pixy_NMObjet; PIXY_objet++) { 00818 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); 00819 00820 if (PIXY_rNMObjet<19) PIXY_rNMObjet++; 00821 else PIXY_rNMObjet = 0; 00822 } 00823 Pixy_NMObjet = 0; 00824 00825 for (PIXY_objet = 0; PIXY_objet < Pixy_CCObjet; PIXY_objet++) { 00826 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); 00827 00828 if (PIXY_rCCObjet<19) PIXY_rCCObjet++; 00829 else PIXY_rCCObjet = 0; 00830 } 00831 val_W_balle = 0; 00832 val_H_balle = 0; 00833 val_carre_balle = 0; 00834 max_val_W_balle = 0; 00835 indice_balle = 0; 00836 for(i=0;i<14;i++){ 00837 val_W_balle=Pixy_NMFIFO[i].NMbloc.width; 00838 val_H_balle=Pixy_NMFIFO[i].NMbloc.height; 00839 00840 val_carre_balle = val_H_balle/val_W_balle; 00841 00842 val_x_balle =Pixy_NMFIFO[i].NMbloc.x; 00843 00844 00845 if(val_W_balle>max_val_W_balle){ 00846 max_val_W_balle=val_W_balle; 00847 indice_balle=i; 00848 if(val_x_balle>130 && val_x_balle<170){ 00849 avancer(); 00850 } 00851 else{ 00852 En = 0; 00853 } 00854 00855 } 00856 } 00857 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); 00858 00859 Pixy_CCObjet = 0; 00860 Pc.printf("\n\r"); 00861 FlagPixy = 0; 00862 wait_ms(500); 00863 } 00864 00865 if (FlagTickLed) { 00866 Led1 = !Led1; 00867 FlagTickLed = 0; 00868 } 00869 } while(!Pc.readable()); 00870 MENU_choix = Pc.getc(); 00871 break; 00872 00873 } 00874 } 00875 } 00876 00877 void capteur_ultrason(void) 00878 { 00879 if (FlagTickLed) { 00880 Led1 = !Led1; 00881 FlagTickLed = 0; 00882 } 00883 00884 // Gestion des US 00885 if (((Tick%150)==0) && FlagTick) { 00886 Trig1 = 1; 00887 wait_us(20); 00888 Trig1 = 0; 00889 FlagTick = 0; 00890 } 00891 00892 if (((Tick%150)==50) && FlagTick) { 00893 Trig2 = 1; 00894 wait_us(20); 00895 Trig2 = 0; 00896 FlagTick = 0; 00897 } 00898 00899 if (((Tick%150)==100) && FlagTick) { 00900 Trig3 = 1; 00901 wait_us(20); 00902 Trig3 = 0; 00903 FlagTick = 0; 00904 } 00905 00906 if (FlagUS1==1) { 00907 Pc.printf ("\rUS 1 = %04d mm", (int)DistUS1); 00908 FlagUS1 = 0; 00909 } 00910 00911 if (FlagUS2==1) { 00912 Pc.printf ("\r\t\t\tUS 2 = %04d mm", (int)DistUS2); 00913 FlagUS2 = 0; 00914 } 00915 00916 if (FlagUS3==1) { 00917 Pc.printf ("\r\t\t\t\t\t\tUS 3 = %04d mm", (int)DistUS3); 00918 FlagUS3 = 0; 00919 } 00920 } 00921 00922 void boussole(void) 00923 { 00924 00925 00926 00927 if (FlagTickLed) { 00928 Led1 = !Led1; 00929 FlagTickLed = 0; 00930 } 00931 00932 if (((Tick%150)==0) && FlagTick) { 00933 FlagTick = 0; 00934 if (BOUSSOLE_check==0) { 00935 CAP_PWM = ((PWMB.pulsewidth()*1000)-1)*10; 00936 Pc.printf ("\r PWM = %4.1lf", CAP_PWM); 00937 } 00938 if (I2C_check==0) { 00939 Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true); 00940 Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4); 00941 CAP_I2C = (double)(((unsigned short)I2C_registerValue[2]<<8)+(unsigned short)I2C_registerValue[3])/10.0; 00942 Pc.printf ("\r\t\t I2C = %4.1lf", CAP_I2C); 00943 } 00944 } 00945 } 00946 00947 void capteur_infrarouge(void) 00948 { 00949 double SD1_val, SD2_val, LD1_val, LD2_val; 00950 double SD1_dist, SD2_dist, LD1_dist, LD2_dist; 00951 00952 if (FlagTickLed) { 00953 Led1 = !Led1; 00954 FlagTickLed = 0; 00955 00956 SD1_val = SD1; 00957 SD2_val = SD2; 00958 LD1_val = LD1; 00959 LD2_val = LD2; 00960 00961 if (SD1_val < 0.06) { 00962 SD1_val = 0; 00963 SD1_dist = 40; 00964 } else { 00965 SD1_dist = 11.611/(SD1_val*3.3-0.0237); 00966 if (SD1_dist > 40) SD1_dist = 40; 00967 } 00968 00969 if (SD2_val < 0.06) { 00970 SD2_val = 0; 00971 SD2_dist = 40; 00972 } else { 00973 SD2_dist = 11.611/(SD2_val*3.3-0.0237); 00974 if (SD2_dist > 40) SD2_dist = 40; 00975 } 00976 00977 if (LD1_val < 0.1) { 00978 LD1_val = 0; 00979 LD1_dist = 150; 00980 } else { 00981 LD1_dist = 59.175/(LD1_val*3.3-0.0275); 00982 if (LD1_dist > 150) LD1_dist = 150; 00983 } 00984 00985 if (LD2_val < 0.1) { 00986 LD2_val = 0; 00987 LD2_dist = 150; 00988 } else { 00989 LD2_dist = 59.175/(LD2_val*3.3-0.0275); 00990 if (LD2_dist > 150) LD2_dist = 150; 00991 } 00992 00993 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); 00994 } 00995 } 00996 00997 void CNY(void) 00998 { 00999 double CNY1_val, CNY2_val, CNY3_val; 01000 01001 if (FlagTickLed) { 01002 Led1 = !Led1; 01003 FlagTickLed = 0; 01004 01005 CNY1_val = CNY1; 01006 CNY2_val = CNY2; 01007 CNY3_val = CNY3; 01008 01009 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); 01010 } 01011 } 01012 01013 01014 void servomoteur(void) 01015 { 01016 01017 01018 if (FlagTickLed) { 01019 Led1 = !Led1; 01020 FlagTickLed = 0; 01021 } 01022 01023 if (((Tick%250)==0) && FlagTick) { 01024 FlagTick = 0; 01025 if (SERVO_dir == S_monte) { 01026 if (SERVO_pulseWidth < (SERVO_max - 100)) SERVO_pulseWidth +=100; 01027 else { 01028 SERVO_dir = S_descente; 01029 SERVO_pulseWidth = SERVO_max; 01030 } 01031 } else { 01032 if (SERVO_pulseWidth > (SERVO_min + 100)) SERVO_pulseWidth -=100; 01033 else { 01034 SERVO_dir = S_monte; 01035 SERVO_pulseWidth = SERVO_min; 01036 } 01037 } 01038 01039 Servo.pulsewidth_us (SERVO_pulseWidth); 01040 Pc.printf ("\rPulse = %d",SERVO_pulseWidth); 01041 } 01042 } 01043 01044 void avancer(void) 01045 { 01046 En = 1; 01047 SensD = 1; 01048 Led2 = 1; 01049 MOTD_duty = 0.48; 01050 Pwm_MD = 0.5; 01051 SensG = 0; 01052 MOTG_duty = 0.5; 01053 Pwm_MG = 0.5; 01054 01055 Pwm_MG = MOTG_duty; 01056 Pwm_MD = MOTD_duty; 01057 } 01058 void tour_droit(void) 01059 { 01060 En = 1; 01061 SensD = 0; 01062 Led2 = 1; 01063 MOTD_duty = 0.5; 01064 Pwm_MD = 0.5; 01065 SensG = 0; 01066 MOTG_duty = 0.5; 01067 Pwm_MG = 0.5; 01068 01069 Pwm_MG = MOTG_duty; 01070 Pwm_MD = MOTD_duty; 01071 } 01072 void tour_gauche(void) 01073 { 01074 En = 1; 01075 SensD = 1; 01076 Led2 = 1; 01077 MOTD_duty = 0.5; 01078 Pwm_MD = 0.5; 01079 SensG = 1; 01080 MOTG_duty = 0.5; 01081 Pwm_MG = 0.5; 01082 01083 Pwm_MG = MOTG_duty; 01084 Pwm_MD = MOTD_duty; 01085 }
Generated on Sun Jul 24 2022 21:42:16 by
1.7.2