FRC - Hackathon
Programme de test à priori fonctionnel avec la version 2018 de la carte
Dependencies: PwmIn mbed Encoder_Nucleo_16_bits
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 * 00119 * PA_0 -> Pixy RX (Serial) -> OK 00120 * PA_1 -> Pixy TX (Serial) -> OK 00121 * PA_2 -> PC TX (Serial) -> OK 00122 * PA_3 -> PX RX (Serial) -> OK 00123 * PA_4 -> GP2 SD #2 (Analog In) -> OK 00124 * PA_5 -> LED1 (Digital Out) -> OK 00125 * PA_6 -> US D Echo (IRQ In) -> OK 00126 * PA_7 -> CNY #1 (Analog In) -> OK 00127 * PA_8 -> MotD PWM (PWM Out) -> OK 00128 * PA_9 -> MotG PWM (PWM Out) -> OK 00129 * PA_10 -> MotD speed (IRQ In) -> OK 00130 * PA_11 -> MotG speed (IRQ In) -> OK 00131 * PA_12 -> 00132 * PA_13 00133 * PA_14 00134 * PA_15 -> Servomoteur (PWM Out) -> OK 00135 * 00136 * PB_0 -> GP2 SD #1 (Analog In) -> OK 00137 * PB_1 -> Vbat (Analog In) -> OK 00138 * PB_2 -> US G Echo (IRQ In) -> OK 00139 * PB_3 -> SS G (Digital Out) -> OK 00140 * PB_4 -> Enocdeur Droit A (QE) -> OK 00141 * PB_5 -> Enocdeur Droit B (QE) -> OK 00142 * PB_6 -> Enocdeur Gauche A (QE) -> OK 00143 * PB_7 -> Enocdeur Gauche B (QE) -> OK 00144 * PB_8 -> SCL (I2C) -> OK 00145 * PB_9 -> SDA (I2C) -> OK 00146 * PB_10 -> 00147 * PB_11 00148 * PB_12 -> SS D (Digital Out) -> OK 00149 * PB_13 -> US G Trig (Digital Out) -> OK 00150 * PB_14 -> US F Trig (Digital Out) -> OK 00151 * PB_15 -> US D Trig (Digital Out) -> OK 00152 * 00153 * PC_0 -> GP2 LD #1 (Analog In) -> OK 00154 * PC_1 -> GP2 LD #2 (Analog In) -> OK 00155 * PC_2 -> CNY #2 (Analog In) -> OK 00156 * PC_3 -> CNY #3 (Analog In) -> OK 00157 * PC_4 -> Boussole (Pwm In) -> OK 00158 * PC_5 -> MotD IN2 (Digital Out) -> OK 00159 * PC_6 -> MotD IN1 (Digital Out) -> OK 00160 * PC_7 -> US F Echo (IRQ In) -> OK 00161 * PC_8 -> MotG IN2 (Digital Out) -> OK 00162 * PC_9 -> MotG IN1 (Digital Out) -> OK 00163 * PC_10 -> SCK (SPI) -> OK 00164 * PC_11 -> MISO (SPI) -> OK 00165 * PC_12 -> MOSI (SPI) -> OK 00166 * PC_13 -> User BP (IRQ In) -> OK 00167 * PC_14 00168 * PC_15 00169 * 00170 * PD_1 00171 * PD_2 -> Led2 (Digital Out) -> OK 00172 */ 00173 00174 Serial Pixy (PA_0, PA_1, 230400); 00175 Serial Pc (PA_2, PA_3, 115200); 00176 00177 AnalogIn SD2 (PA_4); 00178 AnalogIn CNY1 (PA_7); 00179 AnalogIn SD1 (PB_0); 00180 AnalogIn Vbat (PB_1); 00181 AnalogIn LD1 (PC_0); 00182 AnalogIn LD2 (PC_1); 00183 AnalogIn CNY2 (PC_2); 00184 AnalogIn CNY3 (PC_3); 00185 00186 DigitalOut Led1 (PA_5); 00187 DigitalOut SSG (PB_3); 00188 DigitalOut SSD (PB_12); 00189 DigitalOut Trig1 (PB_13); 00190 DigitalOut Trig2 (PB_14); 00191 DigitalOut Trig3 (PB_15); 00192 DigitalOut Led2 (PD_2); 00193 DigitalOut IN1G (PC_9); 00194 DigitalOut IN2G (PC_8); 00195 DigitalOut IN1D (PC_6); 00196 DigitalOut IN2D (PC_5); 00197 00198 InterruptIn Echo3 (PA_6); 00199 InterruptIn PWMD (PA_10); 00200 InterruptIn PWMG (PA_11); 00201 InterruptIn Echo1 (PB_2); 00202 InterruptIn Echo2 (PC_7); 00203 InterruptIn BP (PC_13); 00204 00205 PwmIn PWMB (PC_4); 00206 00207 PwmOut Pwm_MG (PA_9); 00208 PwmOut Pwm_MD (PA_8); 00209 PwmOut Servo (PA_15); 00210 00211 I2C Bus_I2C (PB_9, PB_8); 00212 00213 SPI mySPI (PC_12, PC_11, PC_10); 00214 00215 Nucleo_Encoder_16_bits Gauche (TIM4); // A = PB_6, B = PB_7 00216 Nucleo_Encoder_16_bits Droite (TIM3); // A = PB_4, B = PB_5 00217 00218 Ticker tick; 00219 Timer times; 00220 00221 /** Liste des variables globales 00222 * 00223 * Tick -> (long) Compte le nombre de MS écoulé et déclenche l'exécution de la boucle en fonction du temps écoulé. 00224 * FlagIG -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue gauche 00225 * FlagID -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue droite 00226 * EchoXStart -> (long) Donne le temps en µs de début de l'impulsion d'écho de l'US n°X 00227 * DistUSX -> (float) Donne la distance en mm mesurée par l'US n°X 00228 */ 00229 00230 // Structure de temps 00231 lWord Tick = 0, Gperiod, Dperiod; 00232 00233 // Sémaphore d'interruption 00234 int FlagUS1 = 0, FlagUS2 = 0, FlagUS3 = 0, FlagPixy = 0, FlagPixyOverflow = 0; 00235 int FlagTick = 0, FlagTickLed = 0, BPPressed = 0, nbTurnG = 0, nbTurnD = 0; 00236 int Pixy_check = -1; 00237 00238 // Dialogue avec la Pixy 00239 T_pixyCCData Pixy_CCFIFO[20]; 00240 T_pixyNMData Pixy_NMFIFO[20]; 00241 Byte Pixy_CCObjet, Pixy_NMObjet; 00242 00243 // Gestion des capteurs Ultrason 00244 long Echo1Start, Echo2Start, Echo3Start; 00245 double DistUS1, DistUS2, DistUS3; 00246 00247 /** Liste des interruptions 00248 * 00249 */ 00250 00251 void tickTime() 00252 { 00253 Tick++; 00254 FlagTick = 1; 00255 if ((Tick%100)==0) FlagTickLed = 1; 00256 } 00257 00258 void BPevent () 00259 { 00260 BPPressed = 1; 00261 } 00262 00263 void PWM_motGRise (void) 00264 { 00265 static lWord oldTime; 00266 lWord now; 00267 now = times.read_us(); 00268 Gperiod = now-oldTime; 00269 oldTime = now; 00270 nbTurnG++; 00271 } 00272 00273 void PWM_motDRise (void) 00274 { 00275 static lWord oldTime; 00276 lWord now; 00277 now = times.read_us(); 00278 Dperiod = now-oldTime; 00279 oldTime = now; 00280 nbTurnD++; 00281 } 00282 00283 void echo1Rise () 00284 { 00285 Echo1Start = times.read_us(); 00286 } 00287 00288 void echo2Rise () 00289 { 00290 Echo2Start = times.read_us(); 00291 } 00292 00293 void echo3Rise () 00294 { 00295 Echo3Start = times.read_us(); 00296 } 00297 00298 void echo1Fall () 00299 { 00300 DistUS1 = (double)(times.read_us() - Echo1Start)/5.8; 00301 FlagUS1 = 1; 00302 } 00303 00304 void echo2Fall () 00305 { 00306 DistUS2 = (double)(times.read_us() - Echo2Start)/5.8; 00307 FlagUS2 = 1; 00308 } 00309 00310 void echo3Fall () 00311 { 00312 DistUS3 = (double)(times.read_us() - Echo3Start)/5.8; 00313 FlagUS3 = 1; 00314 } 00315 00316 void getPixyByte () 00317 { 00318 static T_tmpBuffer tmpBuffer; 00319 static T_structBuffer msgBuffer; 00320 static T_pixyState PIXY_state = none; 00321 static Byte byteCount = 0; 00322 static int PIXY_synced = 0, dummy; 00323 int i, somme; 00324 static Byte PIXY_nbCCObjet = 0, PIXY_wCCObjet = 0; 00325 static Byte PIXY_nbNMObjet = 0, PIXY_wNMObjet = 0; 00326 00327 Pixy_check = 0; 00328 00329 if (!PIXY_synced) { // On n'a pas trouvé le START (0x55aa0000) 00330 tmpBuffer.tab[byteCount] = Pixy.getc(); // On stocke l'octet reçu dans la première case dispo du tableau temporaire 00331 00332 if (byteCount < 3) { // Si on n'a pas encore reçu les 4 premier octets 00333 byteCount++; // On passe à la case suivante du tableau temporaire 00334 } else { // Lorsqu'on a 4 octets 00335 if (tmpBuffer.mot != 0xaa550000) { // Si le code n'est pas le code de START 00336 for (i=1; i<4; i++) tmpBuffer.tab[i-1] = tmpBuffer.tab[i]; // On décalle les cases du tableau 00337 byteCount = 3; // Et on attend le caractère suivant 00338 } else { // Si on a trouvé le code de START 00339 PIXY_synced = 1; // On passe en mode synchronisé 00340 PIXY_state = begin; 00341 byteCount = 0; 00342 } 00343 } 00344 } 00345 00346 if (PIXY_synced) { 00347 00348 switch (PIXY_state) { 00349 case begin : // l'aiguillage est là ! 00350 00351 msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus 00352 byteCount++; 00353 if (byteCount == 2) { // Quand on a 2 octets 00354 00355 if (msgBuffer.mot == 0xaa55) { // Si c'est un bloc normal (code 0xAA55) 00356 PIXY_state = normal; // On part vers le traitement spécifique 00357 } 00358 00359 if (msgBuffer.mot == 0xaa56) { // Si c'est un bloc Color Code (code 0xAA56) 00360 PIXY_state = colorCode; // On part vers le traitement spécifique 00361 } 00362 00363 if (msgBuffer.mot == 0) { // Si on a un debut de trame (code 0000) 00364 PIXY_state = doubleZero; // On part vers le traitement spécifique 00365 } 00366 if ((PIXY_state == begin) || (PIXY_state == none)) { // Si c'est autre chose 00367 PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé. 00368 PIXY_state = none; // Ceinture et bretelle 00369 } 00370 byteCount = 0; 00371 } 00372 break; 00373 00374 case normal : // Si on a un bloc normal 00375 00376 Pixy_NMFIFO[PIXY_wNMObjet].tab[byteCount] = Pixy.getc(); // On stocke les octets un à un dans la structure Bloc 00377 if (byteCount < 11) { // Tant que la structure n'est pas pleine 00378 byteCount++; // On passe à l'octet suivant 00379 } else { // Quand elle est pleine 00380 byteCount = 0; // On réinitialise 00381 PIXY_state = begin; // On retourne à l'aiguillage 00382 // On calcule la somme de contrôle 00383 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; 00384 00385 if (somme == Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.checksum) { // Si le checksum est bon, on valide la réception 00386 if (PIXY_wNMObjet < 19) PIXY_wNMObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet 00387 else PIXY_wNMObjet = 0; 00388 if (PIXY_nbNMObjet < 19) PIXY_nbNMObjet++; // On dit que l'on a un objet CC de plus 00389 else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow 00390 } 00391 } 00392 break; 00393 00394 case colorCode : // Si on a un bloc colorCode 00395 00396 Pixy_CCFIFO[PIXY_wCCObjet].tab[byteCount] = dummy; // On stocke les octets un à un dans la structure CCBloc 00397 if (byteCount < 13) byteCount++; // tant que la structure n'est pas pleine on passe à l'octet suivant 00398 else { // Quand elle est pleine 00399 byteCount = 0; // On réinitialise 00400 PIXY_state = begin; // On retourne à l'aiguillage 00401 // On calcule la somme de contrôle 00402 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; 00403 00404 if (somme == Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.checksum) { // Si le checksum est bon 00405 if (PIXY_wCCObjet < 19) PIXY_wCCObjet++; // On incrémente le pointeur d'écriture dans la FIFO CCObjet 00406 else PIXY_wCCObjet = 0; 00407 if (PIXY_nbCCObjet < 19) PIXY_nbCCObjet++; // On dit que l'on a un objet CC de plus à traiter 00408 else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow 00409 } 00410 } 00411 break; 00412 00413 case doubleZero : // Si on a reçu le code de début d'une nouvelle trame. 00414 00415 msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus 00416 byteCount++; 00417 if (byteCount == 2) { // Quand on a 2 octets 00418 if (msgBuffer.mot == 0xaa55) { // On doit impérativement trouver le code 0xAA55 00419 PIXY_state = begin; // Si c'est le cas, alors tout va bien et on va à l'aiguillage 00420 Pixy_NMObjet = PIXY_nbNMObjet; // On met à jour les variables pour le traitement 00421 Pixy_CCObjet = PIXY_nbCCObjet; 00422 PIXY_nbCCObjet = 0; 00423 PIXY_nbNMObjet = 0; 00424 FlagPixy = 1; // On valide le traitement de la trame précédente. 00425 } else { // Si on trouve autre chose 00426 PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé. 00427 PIXY_state = none; // Ceinture et bretelle 00428 } 00429 byteCount = 0; 00430 } 00431 break; 00432 } 00433 } 00434 } 00435 00436 00437 int main() 00438 { 00439 00440 int I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/; 00441 00442 Byte PIXY_rCCObjet = 0, PIXY_rNMObjet = 0; 00443 int PIXY_objet; 00444 00445 int SERVO_pulseWidth = 400, SERVO_max = 1400, SERVO_min = 400; 00446 T_SERVODIR SERVO_dir = S_monte; 00447 00448 char MENU_choix = 0; 00449 00450 char BOUSSOLE_status[1] = {0}; 00451 char I2C_registerValue[4]; 00452 double BOUSSOLE_periode; 00453 00454 double CAP_I2C, CAP_PWM; 00455 double SD1_val, SD2_val, LD1_val, LD2_val, CNY1_val, CNY2_val, CNY3_val, Vbat_val; 00456 double SD1_dist, SD2_dist, LD1_dist, LD2_dist; 00457 00458 int MOTG_evol = 1, MOTD_evol = 1; 00459 double MOTG_duty = 0.5, MOTD_duty = 0.5; 00460 int SensG = 1, SensD = 1; 00461 00462 times.reset(); 00463 times.start(); 00464 00465 // Initialisation des interruptions 00466 tick.attach(&tickTime, 0.001); 00467 00468 BP.rise (&BPevent); 00469 00470 Echo1.rise (&echo1Rise); 00471 Echo2.rise (&echo2Rise); 00472 Echo3.rise (&echo3Rise); 00473 Echo1.fall (&echo1Fall); 00474 Echo2.fall (&echo2Fall); 00475 Echo3.fall (&echo3Fall); 00476 00477 PWMG.rise (&PWM_motGRise); 00478 PWMD.rise (&PWM_motDRise); 00479 00480 Pixy.attach (&getPixyByte); 00481 00482 BP.enable_irq(); 00483 Echo1.enable_irq(); 00484 Echo2.enable_irq(); 00485 Echo3.enable_irq(); 00486 PWMG.enable_irq(); 00487 PWMD.enable_irq(); 00488 00489 // Initialisation des périphériques 00490 // Bus I2C 00491 Bus_I2C.frequency (100000); 00492 00493 // PWM des moteurs 00494 Pwm_MG.period_us(50); 00495 Pwm_MD.period_us(50); 00496 IN1G = 0; 00497 IN2G = 0; 00498 IN1D = 0; 00499 IN2D = 0; 00500 00501 // Bus SPI 00502 mySPI.format (16,1); 00503 mySPI.frequency (1000000); 00504 00505 SSG = 1; 00506 SSD = 1; 00507 00508 // Led 00509 Led2 = 0; 00510 00511 Servo.period_ms (20); 00512 Servo.pulsewidth_us(200); 00513 00514 while(1) { 00515 00516 do { 00517 Led1 = 0; 00518 Pc.printf ("\n\n\n\n\rTest program \n\n\r Choos the component to test :\n\n"); 00519 Pc.printf ("\r1- Ultrasonic sensors (3)\n"); 00520 Pc.printf ("\r2- Compass et I2C\n"); 00521 Pc.printf ("\r3- GP2 sensors \n"); 00522 Pc.printf ("\r4- CNY70 (3)\n"); 00523 Pc.printf ("\r5- VBAT \t! value error!\n"); 00524 Pc.printf ("\r6- Left motor\n"); 00525 Pc.printf ("\r7- Right motor\n"); 00526 Pc.printf ("\r8- Servomotor\n"); 00527 Pc.printf ("\r9- PIXY (CMUCAM5)\n"); 00528 MENU_choix = Pc.getc (); 00529 } while (((MENU_choix-'0')<1) || ((MENU_choix-'0')>9)); 00530 00531 switch (MENU_choix-'0') { 00532 00533 case 1 : 00534 Pc.printf ("\n\n\rUltrasonic sensors check: \n"); 00535 Pc.printf ("\rHit Enter to quit \n"); 00536 do { 00537 00538 if (FlagTickLed) { 00539 Led1 = !Led1; 00540 FlagTickLed = 0; 00541 } 00542 00543 // Gestion des US 00544 if (((Tick%150)==0) && FlagTick) { 00545 Trig1 = 1; 00546 wait_us(20); 00547 Trig1 = 0; 00548 FlagTick = 0; 00549 } 00550 00551 if (((Tick%150)==50) && FlagTick) { 00552 Trig2 = 1; 00553 wait_us(20); 00554 Trig2 = 0; 00555 FlagTick = 0; 00556 } 00557 00558 if (((Tick%150)==100) && FlagTick) { 00559 Trig3 = 1; 00560 wait_us(20); 00561 Trig3 = 0; 00562 FlagTick = 0; 00563 } 00564 00565 if (FlagUS1==1) { 00566 Pc.printf ("\rUS 1 = %04d mm", (int)DistUS1); 00567 FlagUS1 = 0; 00568 } 00569 00570 if (FlagUS2==1) { 00571 Pc.printf ("\r\t\t\tUS 2 = %04d mm", (int)DistUS2); 00572 FlagUS2 = 0; 00573 } 00574 00575 if (FlagUS3==1) { 00576 Pc.printf ("\r\t\t\t\t\t\tUS 3 = %04d mm", (int)DistUS3); 00577 FlagUS3 = 0; 00578 } 00579 } while(!Pc.readable()); 00580 MENU_choix = Pc.getc(); 00581 break; 00582 00583 case 2 : 00584 Pc.printf ("\n\n\rBoussole\n"); 00585 Pc.printf ("\rHit Enter to quit \n"); 00586 00587 Pc.printf ("\n\r I2C check:"); 00588 I2C_check = Bus_I2C.write (BOUSSOLE_adress,BOUSSOLE_status,1,false); 00589 if (I2C_check==0) { 00590 Pc.printf (" OK\n"); 00591 Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true); 00592 Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4); 00593 Pc.printf ("\rCOMPASS Firmware version: %03d\n", I2C_registerValue[0]); 00594 } else { 00595 Pc.printf (" FAIL\n"); 00596 } 00597 00598 BOUSSOLE_periode = PWMB.period(); 00599 Pc.printf ("\rPWM check:"); 00600 if ((BOUSSOLE_periode > 0.11) || (BOUSSOLE_periode < 0.06)) { 00601 Pc.printf (" FAIL\n\n"); 00602 } else { 00603 Pc.printf (" OK\n\n"); 00604 BOUSSOLE_check = 0; 00605 } 00606 00607 do { 00608 if (FlagTickLed) { 00609 Led1 = !Led1; 00610 FlagTickLed = 0; 00611 } 00612 00613 if (((Tick%150)==0) && FlagTick) { 00614 FlagTick = 0; 00615 if (BOUSSOLE_check==0) { 00616 CAP_PWM = ((PWMB.pulsewidth()*1000)-1)*10; 00617 Pc.printf ("\r PWM = %4.1lf", CAP_PWM); 00618 } 00619 if (I2C_check==0) { 00620 Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true); 00621 Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4); 00622 CAP_I2C = (double)(((unsigned short)I2C_registerValue[2]<<8)+(unsigned short)I2C_registerValue[3])/10.0; 00623 Pc.printf ("\r\t\t I2C = %4.1lf", CAP_I2C); 00624 } 00625 } 00626 } while(!Pc.readable()); 00627 MENU_choix = Pc.getc(); 00628 break; 00629 00630 case 3 : 00631 Pc.printf ("\n\n\rGP2xx\n"); 00632 Pc.printf ("\rHit Enter to quit \n"); 00633 do { 00634 if (FlagTickLed) { 00635 Led1 = !Led1; 00636 FlagTickLed = 0; 00637 00638 SD1_val = SD1; 00639 SD2_val = SD2; 00640 LD1_val = LD1; 00641 LD2_val = LD2; 00642 00643 if (SD1_val < 0.06) { 00644 SD1_val = 0; 00645 SD1_dist = 40; 00646 } else { 00647 SD1_dist = 11.611/(SD1_val*3.3-0.0237); 00648 if (SD1_dist > 40) SD1_dist = 40; 00649 } 00650 00651 if (SD2_val < 0.06) { 00652 SD2_val = 0; 00653 SD2_dist = 40; 00654 } else { 00655 SD2_dist = 11.611/(SD2_val*3.3-0.0237); 00656 if (SD2_dist > 40) SD2_dist = 40; 00657 } 00658 00659 if (LD1_val < 0.1) { 00660 LD1_val = 0; 00661 LD1_dist = 150; 00662 } else { 00663 LD1_dist = 59.175/(LD1_val*3.3-0.0275); 00664 if (LD1_dist > 150) LD1_dist = 150; 00665 } 00666 00667 if (LD2_val < 0.1) { 00668 LD2_val = 0; 00669 LD2_dist = 150; 00670 } else { 00671 LD2_dist = 59.175/(LD2_val*3.3-0.0275); 00672 if (LD2_dist > 150) LD2_dist = 150; 00673 } 00674 00675 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); 00676 } 00677 } while(!Pc.readable()); 00678 MENU_choix = Pc.getc(); 00679 break; 00680 00681 case 4 : 00682 Pc.printf ("\n\n\rCNY70\n"); 00683 Pc.printf ("\rHit Enter to quit \n"); 00684 do { 00685 if (FlagTickLed) { 00686 Led1 = !Led1; 00687 FlagTickLed = 0; 00688 00689 CNY1_val = CNY1; 00690 CNY2_val = CNY2; 00691 CNY3_val = CNY3; 00692 00693 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); 00694 } 00695 } while(!Pc.readable()); 00696 MENU_choix = Pc.getc(); 00697 break; 00698 00699 case 5 : 00700 Pc.printf ("\n\n\rVbat \n"); 00701 Pc.printf ("\rHit Enter to quit \n"); 00702 do { 00703 if (FlagTickLed) { 00704 Led1 = !Led1; 00705 FlagTickLed = 0; 00706 00707 Vbat_val = Vbat; 00708 Pc.printf ("\rVbat = %5.3lf V", Vbat_val*3.3*4.3); 00709 } 00710 } while(!Pc.readable()); 00711 MENU_choix = Pc.getc(); 00712 break; 00713 00714 case 6 : 00715 Pc.printf ("\n\n\rLeft motor\n"); 00716 Pc.printf ("\rHit Enter to quit \n"); 00717 Pc.printf ("\rPWM should change regularly and left motor should move, Tick should increase, Period should be 50 us \n", MOTG_duty, Gauche.GetCounter(), Gperiod); 00718 00719 Led2 = 1; 00720 00721 do { 00722 if (SensG) { 00723 IN1G = 1; 00724 IN2G = 0; 00725 } else { 00726 IN1G = 0; 00727 IN2G = 1; 00728 } 00729 00730 if (FlagTickLed) { 00731 Led1 = !Led1; 00732 FlagTickLed = 0; 00733 } 00734 if (BPPressed) { 00735 BPPressed = 0; 00736 SensG = !SensG; 00737 Led2 = !Led2; 00738 } 00739 00740 if (((Tick%1000)==0) && FlagTick) { 00741 FlagTick = 0; 00742 if (MOTG_evol) { 00743 if (MOTG_duty < 0.9) MOTG_duty += 0.1; 00744 else { 00745 MOTG_evol = 0; 00746 MOTG_duty = 0.9; 00747 } 00748 } else { 00749 if (MOTG_duty > 0.1) MOTG_duty -= 0.1; 00750 else { 00751 MOTG_evol = 1; 00752 MOTG_duty = 0.1; 00753 } 00754 } 00755 } 00756 00757 Pwm_MG = MOTG_duty; 00758 00759 Pc.printf ("\rPWM = %2.1lf => Pos = %10ld Tick, Period = %ldus", MOTG_duty, Gauche.GetCounter(), Gperiod); 00760 00761 00762 } while(!Pc.readable()); 00763 MENU_choix = Pc.getc(); 00764 00765 IN1G = 0; 00766 IN2G = 0; 00767 Pwm_MG = 0; 00768 Led2 = 0; 00769 00770 break; 00771 00772 case 7 : 00773 Pc.printf ("\n\n\rLeft motor\n"); 00774 Pc.printf ("\rHit Enter to quit \n"); 00775 Pc.printf ("\rPWM should change regularly and left motor should move, Tick should increase, Period should be 50 us \n", MOTG_duty, Gauche.GetCounter(), Gperiod); 00776 00777 00778 Led2 = 1; 00779 00780 do { 00781 if (SensD) { 00782 IN1D = 1; 00783 IN2D = 0; 00784 } else { 00785 IN1D = 0; 00786 IN2D = 1; 00787 } 00788 if (FlagTickLed) { 00789 Led1 = !Led1; 00790 FlagTickLed = 0; 00791 } 00792 if (BPPressed) { 00793 BPPressed = 0; 00794 SensD = !SensD; 00795 Led2 = !Led2; 00796 } 00797 00798 if (((Tick%1000)==0) && FlagTick) { 00799 FlagTick = 0; 00800 if (MOTD_evol) { 00801 if (MOTD_duty < 0.9) MOTD_duty += 0.1; 00802 else { 00803 MOTD_evol = 0; 00804 MOTD_duty = 0.9; 00805 } 00806 } else { 00807 if (MOTD_duty > 0.1) MOTD_duty -= 0.1; 00808 else { 00809 MOTD_evol = 1; 00810 MOTD_duty = 0.1; 00811 } 00812 } 00813 } 00814 Pwm_MD = MOTD_duty; 00815 Pc.printf ("\rPWM = %2.1lf => Pos = %10ld Tick, Period = %ldus", MOTD_duty, Droite.GetCounter(), Dperiod); 00816 00817 } while(!Pc.readable()); 00818 MENU_choix = Pc.getc(); 00819 00820 IN1D = 0; 00821 IN2D = 0; 00822 Pwm_MD = 0; 00823 Led2 = 0; 00824 00825 break; 00826 00827 case 8 : 00828 Pc.printf ("\n\n\rServo Moteur\n"); 00829 Pc.printf ("\rHit Enter to quit \n"); 00830 00831 do { 00832 if (FlagTickLed) { 00833 Led1 = !Led1; 00834 FlagTickLed = 0; 00835 } 00836 00837 if (((Tick%250)==0) && FlagTick) { 00838 FlagTick = 0; 00839 if (SERVO_dir == S_monte) { 00840 if (SERVO_pulseWidth < (SERVO_max - 100)) SERVO_pulseWidth +=100; 00841 else { 00842 SERVO_dir = S_descente; 00843 SERVO_pulseWidth = SERVO_max; 00844 } 00845 } else { 00846 if (SERVO_pulseWidth > (SERVO_min + 100)) SERVO_pulseWidth -=100; 00847 else { 00848 SERVO_dir = S_monte; 00849 SERVO_pulseWidth = SERVO_min; 00850 } 00851 } 00852 00853 Servo.pulsewidth_us (SERVO_pulseWidth); 00854 Pc.printf ("\rPulse = %d",SERVO_pulseWidth); 00855 } 00856 00857 } while(!Pc.readable()); 00858 MENU_choix = Pc.getc(); 00859 break; 00860 00861 case 9 : 00862 Pc.printf ("\n\n\rPixy\n"); 00863 Pc.printf ("\rHit Enter to quit \n"); 00864 00865 if (Pixy_check == 0) Pc.printf ("\n\rPIXY is ALIVE\n"); 00866 else Pc.printf ("\n\rPIXY don't talk\n"); 00867 00868 do { 00869 if (FlagPixy) { 00870 00871 if (FlagPixyOverflow) { 00872 Pc.printf ("\rSome Data were lost\n"); 00873 FlagPixyOverflow = 0; 00874 } 00875 for (PIXY_objet = 0; PIXY_objet < Pixy_NMObjet; PIXY_objet++) { 00876 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); 00877 00878 if (PIXY_rNMObjet<19) PIXY_rNMObjet++; 00879 else PIXY_rNMObjet = 0; 00880 } 00881 Pixy_NMObjet = 0; 00882 00883 for (PIXY_objet = 0; PIXY_objet < Pixy_CCObjet; PIXY_objet++) { 00884 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); 00885 00886 if (PIXY_rCCObjet<19) PIXY_rCCObjet++; 00887 else PIXY_rCCObjet = 0; 00888 } 00889 Pixy_CCObjet = 0; 00890 Pc.printf("\n\r"); 00891 FlagPixy = 0; 00892 } 00893 00894 if (FlagTickLed) { 00895 Led1 = !Led1; 00896 FlagTickLed = 0; 00897 } 00898 } while(!Pc.readable()); 00899 MENU_choix = Pc.getc(); 00900 break; 00901 00902 } 00903 } 00904 }
Generated on Fri Jul 22 2022 03:42:37 by
1.7.2