/* * ---------- * Christophe Le Lann * http://www.totofweb.net * ---------- * Robot de coupe 2006 * Programme principal * ---------- */ //****************************** // Paramètres //****************************** #define PARAM_LCD TRUE // Faut-il utiliser le LCD ? #define PARAM_BATT 8 // Modification de la PWM en fonction de l'état des batteries #define PARAM_HSL FALSE // Activation ou non de la détection par conversion RGB=>HSL // Débug //#define PARAM_DEBUG 1 // Capteurs IS471F //#define PARAM_DEBUG 2 // Détection et saisie de balle //#define PARAM_DEBUG 3 // Déambulation avec capteurs de contact //#define PARAM_DEBUG 4 // Mouvements sur timers //#define PARAM_DEBUG 5 // Centrage sur trou //#define PARAM_DEBUG 6 // Capteurs de couleur //#define PARAM_DEBUG 7 // Temporisation 90" //#define PARAM_DEBUG 0 // Match #IFNDEF PARAM_DEBUG #define PARAM_DEBUG 0 #ENDIF // Commenter ou mettre FALSE pour avoir la séquence de start complète (avec jack & lcd) #define DESACTIVER_SEQ_START FALSE // TODO : Prévoir démarrage d'urgence sans jack si PARAM_LCD = FALSE //****************************** // Configuration du compilateur //****************************** #include <18F452.h> // Inclusion du Header correspondant au µC utilisé #fuses HS,NOWDT,NOPROTECT,NOBROWNOUT,NOLVP // Paramétrage des fusibles du microcontrôleur #use delay(clock=20000000) // Quartz de 20MHz #IF PARAM_LCD == TRUE #use rs232(baud=115200, xmit=PIN_C6, rcv=PIN_C7) // Liaison série sur RC6 et RC7 #ENDIF //****************************** // Defines //****************************** // Moteurs #define MOT_G_PWM PIN_C1 // CCP2 #define MOT_G_AV PIN_A0 #define MOT_G_AR PIN_A1 #define MOT_D_PWM PIN_C2 // CCP1 #define MOT_D_AV PIN_A3 #define MOT_D_AR PIN_A2 // Bumpers #define BUMP_CO_G PIN_B1 // Capteur côté gauche #define BUMP_AV_G PIN_B0 // Capteur avant gauche #define BUMP_AV_D PIN_B3 // Capteur avant droit #define BUMP_CO_D PIN_B2 // Capteur côté droit // PWM #IF PARAM_BATT > 10 #error "Indice de charge de batterie invalide !" #ENDIF #define PWM_TMP 750 // durée en nombre de Timer0 de l'accélération (750x1.024ms = 750ms) #define GAUCHE_ACC 0 #define GAUCHE_MOY 1 #define DROITE_ACC 2 #define DROITE_MOY 3 // Nombre de 50ms (TMR1) de délai entre chaque compensation de décharge de batterie #define DELAI_AUTOCOMPENSATION 400 // 20sec = 400*.05 // Sécurité pour éviter les inversions brutales de polarité #define SECURITY_DELAY 400 // délai en ms pour éviter les inversions brutales de polarité // Timers et interruptions // ATTENTION : Timer0 8bits sur 16F877 et 16bits sur 18F452 #define INIT_TMR0 65506 // Timer0 compteur 8bits. ici, 65477 (old:65477-197) fera une interruption toutes les 3ms (prescaler 1:256) #define INIT_TMR1 34286 // Timer1 compteur 16bits. ici, 34286 fera une interruptions toutes les 50ms (prescaler 1:8) #define NBTMR1FIN 1780 // Nombre de Timer1 écoulés pour (90-1)" = 1780*.05 (-1 pour enlever les balles restantes) // Servos #define SRV_PORTE PIN_C4 // Activation du bloquage de balle #define SRV_PINCE PIN_C3 // Activation du rouleau // Capteurs de trou #define TROU_01 PIN_B6 #define TROU_02 PIN_B4 #define TROU_03 PIN_D0 #define TROU_04 PIN_B5 #define TROU_05 PIN_B7 #define TROU_06 PIN_D1 #define TROU_07 PIN_D3 #define TROU_08 PIN_D5 #define TROU_09 PIN_D2 #define TROU_10 PIN_D4 #define TROU_11 PIN_D7 // faisceau direct de détection de balle #define TROU_12 PIN_D6 // faisceau réfléchis de détection de couleur de balle // Capteurs de couleur et balance des blancs #define TCS_G_S2 PIN_C0 #define TCS_G_S3 PIN_A5 #define TCS_G_OUT PIN_A4 #define TCS_D_S2 PIN_E0 #define TCS_D_S3 PIN_E1 #define TCS_D_OUT PIN_E2 #define TCS_NB3MS 50 // Temps d'échantillonnage par couleur en pas de 3ms #define TCS_BAL_D_R 0.7 #define TCS_BAL_D_B 1 #define TCS_BAL_G_R 0.7 #define TCS_BAL_G_B 1 /* TCS230 * * Paramétrage des filtres : * - S2H S3H : Vert * - S2H S3L : Blanc * - S2L S3H : Bleu * - S2L S3L : Rouge * * Facteur de fréquence : * - S0H S1H : 100% * - S0H S1L : 20% * - S0L S1H : 2% * - S0L S1L : 0% * * Tableau de conversion de fréquences * * En mode 2% * Sombre 500Hz => 2ms => 10000inst/period * ... 1KHz => 1ms => 5000inst/period * ... 2KHz => 500µs => 2500inst/period * ... 3KHz => 333µs => 1667inst/period * ... 4KHz => 250µs => 1250inst/period * ... 5Khz => 200µs => 1000inst/period * ... 6KHz => 167µs => 835inst/period * ... 8KHz => 125µs => 625inst/period * ... 10KHz => 100µs => 500inst/period * Lumineux 12KHz => 83µs => 416inst/period * * En mode 20% * Sombre 5KHz => 200µs => 1000inst/period * ... 10KHz => 100µs => 500inst/period * ... 20KHz => 50µs => 250inst/period * ... 30KHz => 33µs => 167inst/period * ... 40KHz => 25µs => 125inst/period * ... 50Khz => 20µs => 100inst/period * ... 60KHz => 17µs => 83inst/period * ... 80KHz => 12µs => 62inst/period * ... 100KHz => 10µs => 50inst/period * Lumineux 120KHz => 8µs => 42inst/period */ //. Communication avec le LCD // Messages envoyés (ascii) #define LCD_MSG_ALLUMAGE 0 // Allumage du robot, reset du pic #define LCD_MSG_DETEJACK 1 // Cordon Jack détecté #define LCD_MSG_COULROUG 2 // Couleur rouge #define LCD_MSG_COULBLEU 3 // Couleur bleue #define LCD_MSG_DEBTESTM 4 // Début de la séquence de tests #define LCD_MSG_OKBUMPER 5 // Aucun bumper bloqué #define LCD_MSG_NOBUMPER 6 // Erreur, l'un des bumpers est bloqué #define LCD_MSG_FINTESTM 7 // Fin de la séquence de tests #define LCD_MSG_DEBMATCH 8 // Début du match, cordon jack retiré #define LCD_MSG_FINMATCH 9 // Fin du match après 90" // Messages reçus (8bits) #define LCD_RCV_DETEJACK 49 // Couleur rouge choisie #define LCD_RCV_COULROUG 50 // Couleur rouge choisie #define LCD_RCV_COULBLEU 51 // Couleur bleue choisie #define LCD_RCV_DEBTESTM 52 // Début de la séquence de tests #define LCD_RCV_DEBMATCH 53 // Début du match // Pseudo-fonctions #define set_pwm_gauche set_pwm2_duty #define set_pwm_droite set_pwm1_duty #define MAX(a, b) (((a) > (b)) ? (a) : (b) ) #define MIN(a, b) (((a) < (b)) ? (a) : (b) ) //****************************** // Variables globales //****************************** int mot_g_acceleration = 0; // nombre de Timer0 restants avant la fin de la période d'accélération int mot_d_acceleration = 0; // nombre de Timer0 restants avant la fin de la période d'accélération int obstacles = 0b0000; // binaire 0b(abcd), a = input(AV_G), b = input(AV_D), c = input(AR_G), d = input(AR_D) qui contient l'état des capteurs d'obstacles (1 = obstacle) int1 debutmatch = 0; // binaire true quand match commence int1 finmatch = 0; // binaire true quand match fini int1 sequence_test = 0; // binaire d'activation de la séquence de tests int16 temps_ecoule = 0; // nombre de Timer0 écoulés depuis le début du match int16 trous = 0b0000000000; // binaire (comme pour les obstacles) qui contient l'état des capteurs de trous (1 = trou) int1 accelerer = 1; // binaire true quand le robot s'est arrêté depuis la dernière accélération int1 jackdetecte = 0; // binaire int comptage_capteur_couleur = 0; // donne une indication de temps par l'intermédiaire du timer0 => nombre de 3ms depuis dernier reset int moyennageobturation = 0; // de 0 à 32, sert de moyennage du capteur de balle, pour éviter les petites erreurs de détection int32 obturations = 0; // buffer 0bxxxxxxx....., faisant défiler de droite à gauche les 1 et 0 correspondant à l'état du capteur de balle int16 tcs_g_comptage; // Pour le double-retour de coul_comptage() int16 tcs_d_comptage; // Pour le double-retour de coul_comptage() int16 trous_tmp; // Pour le déparasitage des capteurs de trous int alternative_evitement = 0; // Sert à définir une "stratégie d'évitement", ce qui permet d'éviter de rester bloqué quand la stratégie par défaut échoue... int16 timesincequelquechose = 0; // Si ça fait trop longtemps que le robot n'a pas bougé, on donne un gros coup de patins... int _param_batt = PARAM_BATT; int balle_etait_presente = 0; // Sert à la stabilisation du capteur de couleur de balle float _tcs_bal_d_r = TCS_BAL_D_R; float _tcs_bal_d_b = TCS_BAL_D_B; float _tcs_bal_g_r = TCS_BAL_G_R; float _tcs_bal_g_b = TCS_BAL_G_B; // pour les tests int1 a, b, c, d, e, f, g, h, i, j, k, l; /* +===>balle(direct) * -- trous = 0bxxxxxxxxxx00 : -- * - - | | +==>ball(réf)- - * - (1) - <===+ +===> - (10) - * - - - - * - (2) - - (9) - * - - - - * - (3) - - (8) - * - - - - * - - - - * - (4) - - (7) - * - ------------ - * - (5) (6) - * - - */ enum coul {ROUGE, VERT, BLEU, INDETERMINE}; enum coulballe {NOIR, BLANC}; coul couleur_robot = INDETERMINE; // couleur donnée au robot coul couleur_trou_d = INDETERMINE; // couleur donnée au trou coul couleur_trou_g = INDETERMINE; // couleur donnée au trou coul couleur_recue = INDETERMINE; // couleur recue de l'IHM coulballe couleur_balle = BLANC; // couleur de la balle enum etatmot {AVANT, ARRIERE, ARRET}; etatmot etat_moteur_gauche = ARRET; etatmot etat_moteur_droit = ARRET; int pwm_moteur[16][4]; //****************************** // Fonctions //****************************** // Gestion des moteurs void mot_init() { pwm_moteur[0][GAUCHE_ACC] = 95; // % de PWM en accélération pour le moteur gauche pwm_moteur[0][DROITE_ACC] = 100; // % de PWM en accélération pour le moteur droit pwm_moteur[0][GAUCHE_MOY] = 80; // % de PWM en vitesse normale pour le moteur gauche pwm_moteur[0][DROITE_MOY] = 85; // % de PWM en vitesse normale pour le moteur droit pwm_moteur[1][GAUCHE_ACC] = 90; pwm_moteur[1][DROITE_ACC] = 100; pwm_moteur[1][GAUCHE_MOY] = 75; pwm_moteur[1][DROITE_MOY] = 80; pwm_moteur[2][GAUCHE_ACC] = 80; pwm_moteur[2][DROITE_ACC] = 90; pwm_moteur[2][GAUCHE_MOY] = 70; pwm_moteur[2][DROITE_MOY] = 85; pwm_moteur[3][GAUCHE_ACC] = 70; pwm_moteur[3][DROITE_ACC] = 75; pwm_moteur[3][GAUCHE_MOY] = 60; pwm_moteur[3][DROITE_MOY] = 65; pwm_moteur[4][GAUCHE_ACC] = 60; pwm_moteur[4][DROITE_ACC] = 65; pwm_moteur[4][GAUCHE_MOY] = 50; pwm_moteur[4][DROITE_MOY] = 55; pwm_moteur[5][GAUCHE_ACC] = 55; pwm_moteur[5][DROITE_ACC] = 60; pwm_moteur[5][GAUCHE_MOY] = 45; pwm_moteur[5][DROITE_MOY] = 50; pwm_moteur[6][GAUCHE_ACC] = 55; pwm_moteur[6][DROITE_ACC] = 60; pwm_moteur[6][GAUCHE_MOY] = 40; pwm_moteur[6][DROITE_MOY] = 45; pwm_moteur[7][GAUCHE_ACC] = 45; pwm_moteur[7][DROITE_ACC] = 50; pwm_moteur[7][GAUCHE_MOY] = 40; pwm_moteur[7][DROITE_MOY] = 45; pwm_moteur[8][GAUCHE_ACC] = 45; pwm_moteur[8][DROITE_ACC] = 45; pwm_moteur[8][GAUCHE_MOY] = 32; pwm_moteur[8][DROITE_MOY] = 35; pwm_moteur[9][GAUCHE_ACC] = 40; pwm_moteur[9][DROITE_ACC] = 45; pwm_moteur[9][GAUCHE_MOY] = 27; pwm_moteur[9][DROITE_MOY] = 30; pwm_moteur[10][GAUCHE_ACC] = 28; pwm_moteur[10][DROITE_ACC] = 32; pwm_moteur[10][GAUCHE_MOY] = 24; pwm_moteur[10][DROITE_MOY] = 27; } void mot_stop() { // Sens de rotation output_low(MOT_G_AV); output_low(MOT_D_AV); output_low(MOT_G_AR); output_low(MOT_D_AR); etat_moteur_gauche = ARRET; etat_moteur_droit = ARRET; // PWM set_pwm_droite(0); set_pwm_gauche(0); // Accélération mot_g_acceleration = 0; mot_d_acceleration = 0; accelerer = 1; } void mot_avancer() { // Sécurité contre les inversions brutales de polarité if (!(etat_moteur_gauche == AVANT && etat_moteur_droit == AVANT) && !(etat_moteur_gauche == ARRET && etat_moteur_droit == ARRET)) { mot_stop(); delay_ms(SECURITY_DELAY); } // Sens de rotation output_low(MOT_G_AR); output_low(MOT_D_AR); output_high(MOT_G_AV); output_high(MOT_D_AV); etat_moteur_gauche = AVANT; etat_moteur_droit = AVANT; // PWM if (accelerer == 1) { set_pwm_droite(pwm_moteur[_param_batt][DROITE_ACC]); set_pwm_gauche(pwm_moteur[_param_batt][GAUCHE_ACC]); } else { set_pwm_droite(pwm_moteur[_param_batt][DROITE_MOY]); set_pwm_gauche(pwm_moteur[_param_batt][GAUCHE_MOY]); } // Accélération mot_g_acceleration = PWM_TMP; mot_d_acceleration = PWM_TMP; accelerer = 0; } void mot_reculer() { // Sécurité contre les inversions brutales de polarité if (!(etat_moteur_gauche == ARRIERE && etat_moteur_droit == ARRIERE) && !(etat_moteur_gauche == ARRET && etat_moteur_droit == ARRET)) { mot_stop(); delay_ms(SECURITY_DELAY); } // Sens de rotation output_low(MOT_G_AV); output_low(MOT_D_AV); output_high(MOT_G_AR); output_high(MOT_D_AR); etat_moteur_gauche = ARRIERE; etat_moteur_droit = ARRIERE; // PWM if (accelerer == 1) { set_pwm_droite(pwm_moteur[_param_batt][DROITE_ACC]); set_pwm_gauche(pwm_moteur[_param_batt][GAUCHE_ACC]); } else { set_pwm_droite(pwm_moteur[_param_batt][DROITE_MOY]); set_pwm_gauche(pwm_moteur[_param_batt][GAUCHE_MOY]); } // Accélération mot_g_acceleration = PWM_TMP; mot_d_acceleration = PWM_TMP; } void mot_virage_droite() { // Sécurité contre les inversions brutales de polarité if (!(etat_moteur_gauche == AVANT && etat_moteur_droit == ARRET) && !(etat_moteur_gauche == ARRET && etat_moteur_droit == ARRET)) { mot_stop(); delay_ms(SECURITY_DELAY); } // Sens de rotation output_low(MOT_G_AR); output_low(MOT_D_AR); output_high(MOT_G_AV); output_low(MOT_D_AV); etat_moteur_gauche = AVANT; etat_moteur_droit = ARRET; // PWM set_pwm_droite(0); if (accelerer == 1) set_pwm_gauche(pwm_moteur[_param_batt][GAUCHE_ACC]); else set_pwm_gauche(pwm_moteur[_param_batt][GAUCHE_MOY]); // Accélération mot_g_acceleration = PWM_TMP; mot_d_acceleration = 0; accelerer = 0; } void mot_virage_gauche() { // Sécurité contre les inversions brutales de polarité if (!(etat_moteur_gauche == ARRET && etat_moteur_droit == AVANT) && !(etat_moteur_gauche == ARRET && etat_moteur_droit == ARRET)) { mot_stop(); delay_ms(SECURITY_DELAY); } // Sens de rotation output_low(MOT_G_AR); output_low(MOT_D_AR); output_low(MOT_G_AV); output_high(MOT_D_AV); etat_moteur_gauche = ARRET; etat_moteur_droit = AVANT; // PWM if (accelerer == 1) set_pwm_droite(pwm_moteur[_param_batt][DROITE_ACC]); else set_pwm_droite(pwm_moteur[_param_batt][DROITE_MOY]); set_pwm_gauche(0); // Accélération mot_g_acceleration = 0; mot_d_acceleration = PWM_TMP; accelerer = 0; } void mot_rotation_droite() { // Sécurité contre les inversions brutales de polarité if (!(etat_moteur_gauche == AVANT && etat_moteur_droit == ARRIERE) && !(etat_moteur_gauche == ARRET && etat_moteur_droit == ARRET)) { mot_stop(); delay_ms(SECURITY_DELAY); } // Sens de rotation output_low(MOT_G_AR); output_low(MOT_D_AV); output_high(MOT_G_AV); output_high(MOT_D_AR); etat_moteur_gauche = AVANT; etat_moteur_droit = ARRIERE; // PWM if (accelerer == 1) { set_pwm_droite(pwm_moteur[_param_batt][DROITE_ACC]); set_pwm_gauche(pwm_moteur[_param_batt][GAUCHE_ACC]); } else { set_pwm_droite(pwm_moteur[_param_batt][DROITE_MOY]); set_pwm_gauche(pwm_moteur[_param_batt][GAUCHE_MOY]); } // Accélération mot_g_acceleration = PWM_TMP; mot_d_acceleration = PWM_TMP; accelerer = 0; } void mot_rotation_gauche() { // Sécurité contre les inversions brutales de polarité if (!(etat_moteur_gauche == ARRIERE && etat_moteur_droit == AVANT) && !(etat_moteur_gauche == ARRET && etat_moteur_droit == ARRET)) { mot_stop(); delay_ms(SECURITY_DELAY); } // Sens de rotation output_low(MOT_G_AV); output_low(MOT_D_AR); output_high(MOT_G_AR); output_high(MOT_D_AV); etat_moteur_gauche = ARRIERE; etat_moteur_droit = AVANT; // PWM if (accelerer == 1) { set_pwm_droite(pwm_moteur[_param_batt][DROITE_ACC]); set_pwm_gauche(pwm_moteur[_param_batt][GAUCHE_ACC]); } else { set_pwm_droite(pwm_moteur[_param_batt][DROITE_MOY]); set_pwm_gauche(pwm_moteur[_param_batt][GAUCHE_MOY]); } // Accélération mot_g_acceleration = PWM_TMP; mot_d_acceleration = PWM_TMP; accelerer = 0; } void mot_reculer_droite() { // Sécurité contre les inversions brutales de polarité if (!(etat_moteur_gauche == ARRET && etat_moteur_droit == ARRIERE) && !(etat_moteur_gauche == ARRET && etat_moteur_droit == ARRET)) { mot_stop(); delay_ms(SECURITY_DELAY); } // Sens de rotation output_low(MOT_G_AV); output_low(MOT_D_AV); output_low(MOT_G_AR); output_high(MOT_D_AR); etat_moteur_gauche = ARRET; etat_moteur_droit = ARRIERE; // PWM if (accelerer == 1) set_pwm_droite(pwm_moteur[_param_batt][DROITE_ACC]); else set_pwm_droite(pwm_moteur[_param_batt][DROITE_MOY]); set_pwm_gauche(0); // Accélération mot_g_acceleration = 0; mot_d_acceleration = PWM_TMP; accelerer = 0; } void mot_reculer_gauche() { // Sécurité contre les inversions brutales de polarité if (!(etat_moteur_gauche == ARRIERE && etat_moteur_droit == ARRET) && !(etat_moteur_gauche == ARRET && etat_moteur_droit == ARRET)) { mot_stop(); delay_ms(SECURITY_DELAY); } // Sens de rotation output_low(MOT_G_AV); output_low(MOT_D_AV); output_high(MOT_G_AR); output_low(MOT_D_AR); etat_moteur_gauche = ARRIERE; etat_moteur_droit = ARRET; // PWM set_pwm_droite(0); if (accelerer == 1) set_pwm_gauche(pwm_moteur[_param_batt][GAUCHE_ACC]); else set_pwm_gauche(pwm_moteur[_param_batt][GAUCHE_MOY]); // Accélération mot_g_acceleration = PWM_TMP; mot_d_acceleration = 0; accelerer = 0; } void mot_deceleration() { if (mot_d_acceleration > 1) mot_d_acceleration--; else if (mot_d_acceleration == 1) { mot_d_acceleration--; set_pwm_droite(pwm_moteur[_param_batt][DROITE_MOY]); } if (mot_g_acceleration > 1) mot_g_acceleration--; else if (mot_g_acceleration == 1) { mot_g_acceleration--; set_pwm_gauche(pwm_moteur[_param_batt][GAUCHE_MOY]); } } // Communication avec le LCD void lcd_msg(int msg) { #if PARAM_LCD == TRUE printf("%u", msg); #endif } // Gestion des servos void srv_porte_on() { output_high(SRV_PORTE); } void srv_porte_off() { output_low(SRV_PORTE); } void srv_pince_on() { output_high(SRV_PINCE); } void srv_pince_off() { output_low(SRV_PINCE); } // Vérification des capteurs void capt_obstacles() { obstacles = 0b0000; if (input(BUMP_CO_G)) obstacles |= 0b1000; if (input(BUMP_AV_G)) obstacles |= 0b0100; if (input(BUMP_AV_D)) obstacles |= 0b0010; if (input(BUMP_CO_D)) obstacles |= 0b0001; #IF PARAM_DEBUG == 3 printf("%u%u%u%u\n\r", (obstacles&0b1000)>>3, (obstacles&0b0100)>>2, (obstacles&0b0010)>>1, (obstacles&0b0001)); #ENDIF } void capt_trous() { trous = 0b000000000000; if (input(TROU_01)) { if (trous_tmp & 0b100000000000) trous |= 0b100000000000; else trous_tmp |= 0b100000000000; } else trous_tmp &= 0b011111111111; // extreme gauche if (input(TROU_02)) { if (trous_tmp & 0b010000000000) trous |= 0b010000000000; else trous_tmp |= 0b010000000000; } else trous_tmp &= 0b101111111111; // ... if (input(TROU_03)) { if (trous_tmp & 0b001000000000) trous |= 0b001000000000; else trous_tmp |= 0b001000000000; } else trous_tmp &= 0b110111111111; // ... if (input(TROU_04)) { if (trous_tmp & 0b000100000000) trous |= 0b000100000000; else trous_tmp |= 0b000100000000; } else trous_tmp &= 0b111011111111; // ... if (input(TROU_05)) { if (trous_tmp & 0b000010000000) trous |= 0b000010000000; else trous_tmp |= 0b000010000000; } else trous_tmp &= 0b111101111111; // ... if (input(TROU_06)) { if (trous_tmp & 0b000001000000) trous |= 0b000001000000; else trous_tmp |= 0b000001000000; } else trous_tmp &= 0b111110111111; // ... if (input(TROU_07)) { if (trous_tmp & 0b000000100000) trous |= 0b000000100000; else trous_tmp |= 0b000000100000; } else trous_tmp &= 0b111111011111; // ... if (input(TROU_08)) { if (trous_tmp & 0b000000010000) trous |= 0b000000010000; else trous_tmp |= 0b000000010000; } else trous_tmp &= 0b111111101111; // ... if (input(TROU_09)) { if (trous_tmp & 0b000000001000) trous |= 0b000000001000; else trous_tmp |= 0b000000001000; } else trous_tmp &= 0b111111110111; // ... if (input(TROU_10)) { if (trous_tmp & 0b000000000100) trous |= 0b000000000100; else trous_tmp |= 0b000000000100; } else trous_tmp &= 0b111111111011; // extreme droite if (input(TROU_11)) { if (trous_tmp & 0b000000000010) trous |= 0b000000000010; else trous_tmp |= 0b000000000010; } else trous_tmp &= 0b111111111101; // presence de balle if (input(TROU_12)) { if (trous_tmp & 0b000000000001) trous |= 0b000000000001; else trous_tmp |= 0b000000000001; } else trous_tmp &= 0b111111111110; // couleur de la balle //#IF PARAM_DEBUG == 5 //printf("%u%u%u%u%u%u%u%u%u%u %u%u\n\r", (trous&0b100000000000)>>11, (trous&0b010000000000)>>10, (trous&0b001000000000)>>9, (trous&0b000100000000)>>8, (trous&0b000010000000)>>7, (trous&0b000001000000)>>6, (trous&0b000000100000)>>5, (trous&0b000000010000)>>4, (trous&0b000000001000)>>3, (trous&0b000000000100)>>2, (trous&0b000000000010)>>1, (trous&0b000000000001)); // printf("%lu\n\r", trous); /* a = trous&0b100000000000; b = trous&0b010000000000; c = trous&0b001000000000; d = trous&0b000100000000; e = trous&0b000010000000; f = trous&0b000001000000; g = trous&0b000000100000; h = trous&0b000000010000; i = trous&0b000000001000; j = trous&0b000000000100; k = trous&0b000000000010; l = trous&0b100000000001; printf("trous:%u%u%u%u%u%u%u%u%u%u | presence:%u | couleur:%u\n\r", a, b, c, d, e, f, g, h, i, j, k, l);*/ //#ENDIF } // Gestion de balle void bal_moyennage() { // Balle détectée mais erreur possible obturations = obturations << 1; // Décalage du buffer if (trous & 0b000000000010) obturations |= 1; // Ajout d'un bit } int bal_presence() { // Erreur presque impossible int i; int32 obt; obt = obturations; moyennageobturation = 0; balle_etait_presente = /*(*/balle_etait_presente << 1/*) & 0b11111*/; for (i = 0; i < 32; i++) { if (obt & 1) moyennageobturation++; obt = obt >> 1; } if (moyennageobturation > 12) { #IF PARAM_DEBUG == 2 printf("detectee = oui (%u/32) couleur = %lu\n\r", moyennageobturation, trous&0b000000000001); #ENDIF balle_etait_presente |= 0b00001; return 1; } else { #IF PARAM_DEBUG == 2 printf("detectee = non (%u/32) couleur = %lu\n\r", moyennageobturation, trous&0b000000000001); #ENDIF return 0; } } void bal_coul_detect() { if (trous & 0b000000000001) couleur_balle = NOIR; else couleur_balle = BLANC; } // Actions basiques du robot int obstacles_evites() { // Retourne 1 si pas d'obstacle, 0 si obstacle a été évité if (obstacles == 0b0000) { // Pas d'obstacle if (timesincequelquechose >= 200) { // Si rien ne se passe depuis 10 secondes // gros coup de patins // Sécurité contre les inversions brutales de polarité output_low(MOT_G_AV); output_low(MOT_D_AV); output_high(MOT_G_AR); output_high(MOT_D_AR); etat_moteur_gauche = AVANT; etat_moteur_droit = AVANT; set_pwm_droite(pwm_moteur[_param_batt-2][DROITE_ACC]); set_pwm_gauche(pwm_moteur[_param_batt-2][GAUCHE_ACC]); delay_ms(1000); pwm_moteur[_param_batt][DROITE_ACC] += 2; pwm_moteur[_param_batt][DROITE_MOY] += 2; pwm_moteur[_param_batt][GAUCHE_ACC] += 2; pwm_moteur[_param_batt][GAUCHE_MOY] += 2; accelerer = 0; mot_virage_droite(); delay_ms(1000); mot_stop(); delay_ms(250); mot_avancer(); timesincequelquechose = 0; } return 1; } else if (obstacles & 0b1000) { // Obstacle angle gauche timesincequelquechose = 0; mot_reculer(); delay_ms(1000); mot_stop(); delay_ms(50); switch(alternative_evitement) { case 0: alternative_evitement++; mot_reculer_droite(); break; case 1: alternative_evitement++; mot_virage_droite(); break; default: alternative_evitement=0; mot_rotation_droite(); break; } delay_ms(1000); return 0; } else if (obstacles & 0b0100) { // Obstacle avant gauche timesincequelquechose = 0; mot_reculer(); delay_ms(1000); mot_stop(); delay_ms(50); switch(alternative_evitement) { case 0: alternative_evitement++; mot_reculer_droite(); break; case 1: alternative_evitement++; mot_virage_droite(); break; default: alternative_evitement=0; mot_rotation_droite(); break; } delay_ms(1500); return 0; } else if (obstacles & 0b0010) { // Obstacle avant droit timesincequelquechose = 0; mot_reculer(); delay_ms(1000); mot_stop(); delay_ms(50); switch(alternative_evitement) { case 0: alternative_evitement++; mot_reculer_gauche(); break; case 1: alternative_evitement++; mot_virage_gauche(); break; default: alternative_evitement=0; mot_rotation_gauche(); break; } delay_ms(1500); return 0; } else { // Obstacle angle droit timesincequelquechose = 0; mot_reculer(); delay_ms(1000); mot_stop(); delay_ms(50); switch(alternative_evitement) { case 0: alternative_evitement++; mot_reculer_gauche(); break; case 1: alternative_evitement++; mot_virage_gauche(); break; default: alternative_evitement=0; mot_rotation_gauche(); break; } delay_ms(1000); return 0; } } int trou_centrage() { // Retourne 1 si on estime être sur un trou, 0 sinon if (!(trous & 0b111111111100)) { // Pas de trou détecté mot_avancer(); return 0; } else if (trous & 0b100000000000) { timesincequelquechose = 0; mot_rotation_gauche(); delay_ms(350); return 0; } else if (trous & 0b011000000000) { timesincequelquechose = 0; mot_rotation_gauche(); delay_ms(250); return 0; } else if (trous & 0b000100000000) { timesincequelquechose = 0; mot_rotation_gauche(); delay_ms(350); return 0; } else if (trous & 0b000011000000) { timesincequelquechose = 0; mot_reculer(); delay_ms(200); mot_stop(); return 1; } else if (trous & 0b000000100000) { timesincequelquechose = 0; mot_rotation_droite(); delay_ms(350); return 0; } else if (trous & 0b000000011000) { timesincequelquechose = 0; mot_rotation_droite(); delay_ms(250); return 0; } else {//(trous & 0b000000000100) { timesincequelquechose = 0; mot_rotation_droite(); delay_ms(350); return 0; } } // Capteurs de couleur /* Paramétrage des filtres : * - S2H S3H : Vert * - S2H S3L : Blanc * - S2L S3H : Bleu * - S2L S3L : Rouge */ void coul_select_blanc() { output_high(TCS_G_S2); output_low(TCS_G_S3); output_high(TCS_D_S2); output_low(TCS_D_S3); } void coul_select_rouge() { output_low(TCS_G_S2); output_low(TCS_G_S3); output_low(TCS_D_S2); output_low(TCS_D_S3); } void coul_select_vert() { output_high(TCS_G_S2); output_high(TCS_G_S3); output_high(TCS_D_S2); output_high(TCS_D_S3); } void coul_select_bleu() { output_low(TCS_G_S2); output_high(TCS_G_S3); output_low(TCS_D_S2); output_high(TCS_D_S3); } void coul_comptage() { // Compte le nombre d'impulsions sur un temps donné int1 previous_state_g = 0; int1 previous_state_d = 0; tcs_g_comptage = 0; tcs_d_comptage = 0; for(comptage_capteur_couleur = 0; comptage_capteur_couleur < TCS_NB3MS; ) { // L'incrémentation est faite par interruption toutes les 3ms if (!input(TCS_G_OUT) && previous_state_g == 1) { tcs_g_comptage++; previous_state_g = 0; } else if (input(TCS_G_OUT) && previous_state_g == 0) { previous_state_g = 1; } if (!input(TCS_D_OUT) && previous_state_d == 1) { tcs_d_comptage++; previous_state_d = 0; } else if (input(TCS_D_OUT) && previous_state_d == 0) { previous_state_d = 1; } } } #IF PARAM_HSL == TRUE void coul_detect() { int16 periodes[6]; // 0,1,2 => R,V,B gauche; 3,4,5 => R,V,B droite, entre 0 et 360*TCS_NB3MS int16 max_g, max_d, min_g, min_d; float hue_g, hue_d; // Comptage des impulsion (=> fréquence) coul_select_rouge(); coul_comptage(); periodes[0] = tcs_g_comptage; periodes[3] = tcs_d_comptage; coul_select_vert(); coul_comptage(); periodes[1] = tcs_g_comptage; periodes[4] = tcs_d_comptage; coul_select_bleu(); coul_comptage(); periodes[2] = tcs_g_comptage; periodes[5] = tcs_d_comptage; // Balance des blancs periodes[0] = periodes[0]*_tcs_bal_g_r; periodes[2] = periodes[2]*_tcs_bal_g_b; periodes[3] = periodes[3]*_tcs_bal_d_r; periodes[5] = periodes[5]*_tcs_bal_d_b; // TODO : vérification de luminosité et de saturation // Conversion RGB => HSL et exploitation des résultats max_g = MAX(periodes[0], periodes[1]); max_g = MAX(max_g, periodes[2]); min_g = MIN(periodes[0], periodes[1]); min_g = MIN(min_g, periodes[2]); max_d = MAX(periodes[3], periodes[4]); max_d = MAX(max_d, periodes[5]); min_d = MIN(periodes[3], periodes[4]); min_d = MIN(min_d, periodes[5]); #IF PARAM_DEBUG == 6 printf("%lu, %lu, %lu, ", periodes[3], periodes[4], periodes[5]); #ENDIF if ((max_g - min_g) == 0) couleur_trou_g = INDETERMINE; else { if (max_g == periodes[0]) hue_g = ((float)periodes[1] - (float)periodes[2]) / ((float)max_g - (float)min_g); else if (max_g == periodes[1]) hue_g = 2 + ((float)periodes[2] - (float)periodes[0]) / ((float)max_g - (float)min_g); else hue_g = 4 + ((float)periodes[0] - (float)periodes[1]) / ((float)max_g - (float)min_g); hue_g = hue_g / 6; if (hue_g < 0) hue_g += 1; if (hue_g >= 0.833) couleur_trou_g = ROUGE; else if (hue_g >= 0.500) couleur_trou_g = BLEU; else if (hue_g >= 0.167) couleur_trou_d = VERT; else couleur_trou_g = ROUGE; } if ((max_d - min_d) == 0) couleur_trou_d = INDETERMINE; else { if (max_d == periodes[3]) hue_d = ((float)periodes[4] - (float)periodes[5]) / ((float)max_d - (float)min_d); else if (max_d == periodes[4]) hue_d = 2 + ((float)periodes[5] - (float)periodes[3]) / ((float)max_d - (float)min_d); else hue_d = 4 + ((float)periodes[3] - (float)periodes[4]) / ((float)max_d - (float)min_d); hue_d = hue_d / 6; if (hue_d < 0) hue_d += 1; #IF PARAM_DEBUG == 6 printf("hue_d : %f ", hue_d); #ENDIF if (hue_d >= 0.833) couleur_trou_d = ROUGE; else if (hue_d >= 0.500) couleur_trou_d = BLEU; else if (hue_d >= 0.167) couleur_trou_d = VERT; else couleur_trou_d = ROUGE; } } #ELSE void coul_detect() { int16 periodes[6]; // 0,1,2 => R,V,B gauche; 3,4,5 => R,V,B droite, entre 0 et 360*TCS_NB3MS couleur_trou_g = INDETERMINE; couleur_trou_d = INDETERMINE; // Comptage des impulsion (=> fréquence) coul_select_rouge(); coul_comptage(); periodes[0] = tcs_g_comptage; periodes[3] = tcs_d_comptage; coul_select_vert(); coul_comptage(); periodes[1] = tcs_g_comptage; periodes[4] = tcs_d_comptage; /*coul_select_bleu(); coul_comptage(); periodes[2] = tcs_g_comptage; periodes[5] = tcs_d_comptage;*/ // Balance des blancs periodes[0] = periodes[0]*_tcs_bal_g_r; //periodes[2] = periodes[2]*_tcs_bal_g_b; periodes[3] = periodes[3]*_tcs_bal_d_r; //periodes[5] = periodes[5]*_tcs_bal_d_b; // TODO : vérification de luminosité et de saturation #IF PARAM_DEBUG == 6 printf("%lu, %lu, %lu, ", periodes[3], periodes[4], periodes[5]); #ENDIF if (periodes[0] > periodes[1]/* && periodes[0] > periodes[2]*/) couleur_trou_g = ROUGE; /*else if (periodes[2] > periodes[0] && periodes[2] > periodes[1]) couleur_trou_g = BLEU; */else couleur_trou_g = VERT; if (periodes[3] > periodes[4]/* && periodes[3] > periodes[5]*/) couleur_trou_d = ROUGE; /*else if (periodes[5] > periodes[3] && periodes[5] > periodes[4]) couleur_trou_d = BLEU; */else couleur_trou_d = VERT; // Peut-être une erreur, on reteste /*if (couleur_trou_g == VERT && couleur_trou_d == VERT) { // Comptage des impulsion (=> fréquence) coul_select_rouge(); coul_comptage(); periodes[0] = tcs_g_comptage; periodes[3] = tcs_d_comptage; coul_select_vert(); coul_comptage(); periodes[1] = tcs_g_comptage; periodes[4] = tcs_d_comptage; coul_select_bleu(); coul_comptage(); periodes[2] = tcs_g_comptage; periodes[5] = tcs_d_comptage; // Balance des blancs periodes[0] = periodes[0]*_tcs_bal_g_r; periodes[2] = periodes[2]*_tcs_bal_g_b; periodes[3] = periodes[3]*_tcs_bal_d_r; periodes[5] = periodes[5]*_tcs_bal_d_b; // TODO : vérification de luminosité et de saturation #IF PARAM_DEBUG == 6 printf("%lu, %lu, %lu, ", periodes[3], periodes[4], periodes[5]); #ENDIF if (periodes[0] > periodes[1] && periodes[0] > periodes[2]) couleur_trou_g = ROUGE; else if (periodes[2] > periodes[0] && periodes[2] > periodes[1]) couleur_trou_g = BLEU; else couleur_trou_g = VERT; if (periodes[3] > periodes[4] && periodes[3] > periodes[5]) couleur_trou_d = ROUGE; else if (periodes[5] > periodes[3] && periodes[5] > periodes[4]) couleur_trou_d = BLEU; else couleur_trou_d = VERT; }*/ } #ENDIF //****************************** // Interruptions //****************************** void config_interruptions() { // RS232 enable_interrupts(INT_RDA); // PWM setup_ccp1(CCP_PWM); // mode PWM pour le CCP1 (RC2) setup_ccp2(CCP_PWM); // mode PWM pour le CCP2 (RC1) setup_timer_2(T2_DIV_BY_16, 127, 1); // pour la fréquence de la PWM : "The cycle time will be (1/clock)*4*t2div*(period+1)", ici 2.4KHz set_pwm_droite(0); // initialisation PWM=0% set_pwm_gauche(0); // initialisation PWM=0% // Timer0 set_timer0(INIT_TMR0); setup_timer_0(RTCC_INTERNAL|RTCC_DIV_256); // Pescaler 1:256 enable_interrupts(INT_TIMER0); // Timer1 set_timer1(INIT_TMR1); setup_timer_1(T1_INTERNAL|T1_DIV_BY_8); // Prescaler 1:8 enable_interrupts(INT_TIMER1); // Activation générale enable_interrupts(GLOBAL); // Servos en position off srv_pince_off(); srv_porte_off(); } #INT_TIMER0 void interruption_timer0() { // Toutes les 3ms // Décélération mot_deceleration(); // Test des bumpers capt_obstacles(); // Test des capteurs de trous capt_trous(); bal_moyennage(); // Fournit une base de temps pour la mesure de fréquence des capteurs de couleur comptage_capteur_couleur++; // Réactivation du timer set_timer0(INIT_TMR0); } #INT_TIMER1 void interruption_timer1() { // Toutes les 50ms // Test de fin de match if (debutmatch == 1 && finmatch == 0) { temps_ecoule++; timesincequelquechose++; } if (finmatch == 0 && temps_ecoule >= NBTMR1FIN) finmatch = 1; // Autocompensation de PWM if (temps_ecoule > 0 && (temps_ecoule % DELAI_AUTOCOMPENSATION) == 0) { pwm_moteur[_param_batt][GAUCHE_ACC]++; pwm_moteur[_param_batt][GAUCHE_MOY]++; pwm_moteur[_param_batt][DROITE_ACC]++; pwm_moteur[_param_batt][DROITE_MOY]++; } // Réactivation du timer set_timer1(INIT_TMR1); } #INT_RDA void interruption_rs232() { int rs232; rs232 = getc(); switch(rs232) { case LCD_RCV_DEBTESTM: sequence_test = 1; break; case LCD_RCV_COULROUG: couleur_recue = ROUGE; break; case LCD_RCV_COULBLEU: couleur_recue = BLEU; break; case LCD_RCV_DEBMATCH: debutmatch = 1; break; case LCD_RCV_DETEJACK: jackdetecte = 1; break; } } //****************************** // Programme principal //****************************** void main () { #IF PARAM_DEBUG == 1 // Séquence de test des capteurs de trou int1 a, b, c, d, e, f, g, h, i, j, k, l; delay_ms(500); config_interruptions(); delay_ms(500); lcd_msg(LCD_MSG_ALLUMAGE); delay_ms(500); while(1) { a = trous & 0b100000000000; b = trous & 0b010000000000; c = trous & 0b001000000000; d = trous & 0b000100000000; e = trous & 0b000010000000; f = trous & 0b000001000000; g = trous & 0b000000100000; h = trous & 0b000000010000; i = trous & 0b000000001000; j = trous & 0b000000000100; k = trous & 0b000000000010; l = trous & 0b100000000001; printf("trous:%u%u%u%u%u%u%u%u%u%u | presence:%u | couleur:%u\n\r", a, b, c, d, e, f, g, h, i, j, k, l); delay_ms(20); } /* int1 a, b, c, d, e, f, g, h, i, j, k, l; delay_ms(500); while(1) { a = input(TROU_01); b = input(TROU_02); c = input(TROU_03); d = input(TROU_04); e = input(TROU_05); f = input(TROU_06); g = input(TROU_07); h = input(TROU_08); i = input(TROU_09); j = input(TROU_10); k = input(TROU_11); l = input(TROU_12); printf("trous:%u%u%u%u%u%u%u%u%u%u | presence:%u | couleur:%u\n\r", a, b, c, d, e, f, g, h, i, j, k, l); delay_ms(20); }*/ #ELIF PARAM_DEBUG == 2 // Séquence de test des capteurs de balle et des servos int1 a; int i; int32 obt; delay_ms(500); config_interruptions(); delay_ms(500); while(1) { //moyennageobturation = 0; //for (i = 0; i < 32; i++) { // if (input(TROU_11)) // balle détectée mais erreur possible // if (moyennageobturation < 32) // moyennageobturation++; // else // if (moyennageobturation > 0) // moyennageobturation--; // delay_ms(5); //} /*for (i = 0; i < 32; i++) { obturations = obturations << 1; // Décalage du buffer if (input(TROU_11)) obturations |= 1; // Ajout d'un bit delay_ms(5); } printf("%lu ", obturations); obt = obturations; moyennageobturation = 0; for (i = 0; i < 32; i++) { if (obt & 1) moyennageobturation++; obt = obt >> 1; } a = input(TROU_12); if ((moyennageobturation > 12) && a) { srv_pince_on(); srv_porte_on(); printf("detectee = oui (%u/32) - couleur = noire\n\r", moyennageobturation); } else if ((moyennageobturation > 12) && !a) { srv_pince_on(); srv_porte_on(); printf("detectee = oui (%u/32) - couleur = blanche\n\r", moyennageobturation); } else if (!(moyennageobturation > 12) && a) { srv_pince_off(); srv_porte_off(); printf("detectee = non (%u/32) - couleur = noire\n\r", moyennageobturation); } else { srv_pince_off(); srv_porte_off(); printf("detectee = non (%u/32) - couleur = blanche\n\r", moyennageobturation); } delay_ms(20);*/ if (bal_presence()) { srv_porte_on(); srv_pince_on(); delay_ms(3000); srv_pince_off(); srv_porte_off(); delay_ms(1000); } else { srv_pince_off(); srv_porte_off(); } delay_ms(20); } #ELIF PARAM_DEBUG == 3 // Séquence de test des capteurs de contact avec les moteurs mot_init(); delay_ms(500); config_interruptions(); delay_ms(500); lcd_msg(LCD_MSG_ALLUMAGE); delay_ms(500); /*// En cas de paranoia while(1) { if (obstacles & 0b1100) { mot_reculer_droite(); delay_ms(750); } else if (obstacles & 0b0011) { mot_reculer_gauche(); delay_ms(750); } else { mot_avancer(); } }*/ while(1) { if (obstacles_evites() == 1) { // Pas d'obstacle mot_avancer(); } } #ELIF PARAM_DEBUG == 4 // Séquence de test des moteurs et des délais mot_init(); delay_ms(500); config_interruptions(); delay_ms(500); lcd_msg(LCD_MSG_ALLUMAGE); delay_ms(500); while(1) { mot_avancer(); delay_ms(500); delay_ms(500); mot_stop(); delay_ms(500); mot_reculer(); delay_ms(500); delay_ms(500); mot_stop(); delay_ms(500); } #ELIF PARAM_DEBUG == 5 // Séquence de test du centrage sur trou mot_init(); delay_ms(500); config_interruptions(); delay_ms(500); lcd_msg(LCD_MSG_ALLUMAGE); delay_ms(500); while(1) { if (obstacles_evites() == 1) { // Pas d'obstacle if (trou_centrage() == 1) { // Centrage sur le trou, 1 quand centré sur trou while (!(trous & 0b111111111100)); } } } #ELIF PARAM_DEBUG == 6 // Séquence de test des capteurs de couleur int16 periodes; int1 previous_state; /* delay_ms(500); config_interruptions(); delay_ms(500); lcd_msg(LCD_MSG_ALLUMAGE); delay_ms(500); while(1) { // On compte le nombre de périodes complétées en 20ms coul_select_blanc(); periodes = 0; previous_state = 0; for(comptage_capteur_couleur = 0; comptage_capteur_couleur < 7; ) { // L'incrémentation est faite par interruption toutes les 3ms if (!input(TCS_G_OUT) && previous_state == 1) { periodes++; previous_state = 0; } else if (input(TCS_G_OUT) && previous_state == 0) { previous_state = 1; } } printf("Nperiodes : Blanc %lu", periodes); periodes = 0; previous_state = 0; for(comptage_capteur_couleur = 0; comptage_capteur_couleur < 7; ) { // L'incrémentation est faite par interruption toutes les 3ms if (!input(TCS_D_OUT) && previous_state == 1) { periodes++; previous_state = 0; } else if (input(TCS_D_OUT) && previous_state == 0) { previous_state = 1; } } printf("/%lu", periodes); coul_select_rouge(); periodes = 0; previous_state = 0; for(comptage_capteur_couleur = 0; comptage_capteur_couleur < 7; ) { // L'incrémentation est faite par interruption toutes les 3ms if (!input(TCS_G_OUT) && previous_state == 1) { periodes++; previous_state = 0; } else if (input(TCS_G_OUT) && previous_state == 0) { previous_state = 1; } } printf(" | Rouge %lu", periodes); periodes = 0; previous_state = 0; for(comptage_capteur_couleur = 0; comptage_capteur_couleur < 7; ) { // L'incrémentation est faite par interruption toutes les 3ms if (!input(TCS_D_OUT) && previous_state == 1) { periodes++; previous_state = 0; } else if (input(TCS_D_OUT) && previous_state == 0) { previous_state = 1; } } printf("/%lu", periodes); coul_select_vert(); periodes = 0; previous_state = 0; for(comptage_capteur_couleur = 0; comptage_capteur_couleur < 7; ) { // L'incrémentation est faite par interruption toutes les 3ms if (!input(TCS_G_OUT) && previous_state == 1) { periodes++; previous_state = 0; } else if (input(TCS_G_OUT) && previous_state == 0) { previous_state = 1; } } printf(" | Vert %lu", periodes); periodes = 0; previous_state = 0; for(comptage_capteur_couleur = 0; comptage_capteur_couleur < 7; ) { // L'incrémentation est faite par interruption toutes les 3ms if (!input(TCS_D_OUT) && previous_state == 1) { periodes++; previous_state = 0; } else if (input(TCS_D_OUT) && previous_state == 0) { previous_state = 1; } } printf("/%lu", periodes); coul_select_bleu(); periodes = 0; previous_state = 0; for(comptage_capteur_couleur = 0; comptage_capteur_couleur < 7; ) { // L'incrémentation est faite par interruption toutes les 3ms if (!input(TCS_G_OUT) && previous_state == 1) { periodes++; previous_state = 0; } else if (input(TCS_G_OUT) && previous_state == 0) { previous_state = 1; } } printf(" | Bleu %lu", periodes); periodes = 0; previous_state = 0; for(comptage_capteur_couleur = 0; comptage_capteur_couleur < 7; ) { // L'incrémentation est faite par interruption toutes les 3ms if (!input(TCS_D_OUT) && previous_state == 1) { periodes++; previous_state = 0; } else if (input(TCS_D_OUT) && previous_state == 0) { previous_state = 1; } } printf("/%lu\n\r", periodes); } */ delay_ms(500); config_interruptions(); delay_ms(500); lcd_msg(LCD_MSG_ALLUMAGE); delay_ms(500); while(1) { coul_detect(); /*printf("TCS : Gauche "); if (couleur_trou_g == ROUGE) printf("ROUGE"); else if (couleur_trou_g == VERT) printf("VERT "); else if (couleur_trou_g == BLEU) printf("BLEU "); else printf("???? ");*/ printf(" Droite "); if (couleur_trou_d == ROUGE) printf("ROUGE\n\r"); else if (couleur_trou_d == VERT) printf("VERT \n\r"); else if (couleur_trou_d == BLEU) printf("BLEU \n\r"); else printf("???? \n\r"); } #ELIF PARAM_DEBUG == 7 // Séquence de test de la temporisation 90" int i = 1; delay_ms(500); config_interruptions(); delay_ms(500); debutmatch = 1; printf("Debut du match\n\r"); while (finmatch == 0) { delay_ms(990); printf("%u", i); i++; } printf("Fin du match\n\r"); #ELSE // Match normal int16 calibration_vert__rouge, calibration_vert__vert, calibration_rouge__rouge; // Initialisation mot_init(); delay_ms(500); config_interruptions(); delay_ms(500); lcd_msg(LCD_MSG_ALLUMAGE); delay_ms(500); #IF DESACTIVER_SEQ_START == FALSE // Prédéfinition de la couleur en fonction du bumper gauche (en cas d'IHM défaillante) if (obstacles & 0b1000) { // enfoncé => rouge couleur_robot = ROUGE; lcd_msg(LCD_MSG_COULROUG); } else { // relâché => bleu couleur_robot = BLEU; lcd_msg(LCD_MSG_COULBLEU); } srv_pince_off(); srv_porte_off(); // Avant le match while (debutmatch == 0) { if (sequence_test == 1) { // Déclenché par interruption RS232 // Début de la séquence de test des moteurs et des bumpers lcd_msg(LCD_MSG_DEBTESTM); // Autocalibration du TCS droit /* coul_select_rouge(); coul_comptage(); calibration_vert__rouge = tcs_d_comptage; coul_select_vert(); coul_comptage(); calibration_vert__vert = tcs_d_comptage; delay_ms(5000); coul_select_rouge(); coul_comptage(); calibration_rouge__rouge = tcs_d_comptage; //printf("\n\r%lu %lu %lu\n\r", calibration_vert__rouge, calibration_vert__vert, calibration_rouge__rouge); _tcs_bal_d_r = ((float)calibration_rouge__rouge - (float)calibration_vert__rouge)/2; //printf("%f\n\r", _tcs_bal_d_r); _tcs_bal_d_r += (float)calibration_vert__vert; //printf("%f\n\r", _tcs_bal_d_r); _tcs_bal_d_r /= (float)calibration_rouge__rouge; //printf("%f\n\r", _tcs_bal_d_r); */ // Test de l'état des capteurs de contact if (obstacles == 0b0000) lcd_msg(LCD_MSG_OKBUMPER); else lcd_msg(LCD_MSG_NOBUMPER); delay_ms(500); // Test des moteurs mot_avancer(); //srv_rouleau_on(); delay_ms(2000); mot_stop(); //srv_rouleau_off(); lcd_msg(LCD_MSG_FINTESTM); // Fin de la séquence de tests sequence_test = 0; } // Confirme la présence du jack sur le LCD if (jackdetecte == 1) { // Déclenché par interruption RS232 jackdetecte = 0; lcd_msg(LCD_MSG_DETEJACK); } // Confirme la réception de la couleur du robot sur le LCD if (couleur_recue == ROUGE) { // Déclenché par interruption RS232 couleur_recue = INDETERMINE; couleur_robot = ROUGE; lcd_msg(LCD_MSG_COULROUG); } else if (couleur_recue == BLEU) { couleur_recue = INDETERMINE; couleur_robot = BLEU; lcd_msg(LCD_MSG_COULBLEU); } } #ELSE debutmatch = 1; #ENDIF // Jack retiré => début du match signalé par RS232 lcd_msg(LCD_MSG_DEBMATCH); //srv_rouleau_on(); // TODO : que faire si l'IHM ne marche pas ou que la communication RS232 ne marche pas ? // Pendant le match while (finmatch == 0) { // Une interruption compte les 90 secondes // Tests de présence d'obstacles (comprends l'évitement si besoin) if (obstacles_evites() == 1) { // Pas d'obstacle // Les 3 premières secondes, on ne s'occupe pas des trous, car ce sont les trous de notre couleur (inintéressants), et on s'assure de ne pas être forfait if (temps_ecoule < 60) { mot_avancer(); // Attendre une balle, sans faire attention au trou if (bal_presence() == 0) { srv_pince_off(); srv_porte_off(); // Au cas où... } else { // Blocage de la balle srv_porte_on(); srv_pince_on(); if ((balle_etait_presente & 0b11110) == 0) { // On attend pour que le capteur se stabilise delay_ms(100); mot_stop(); delay_ms(600); // Détection de la couleur de la balle bal_coul_detect(); // On ne s'intéresse pas aux balles noires if (couleur_balle == NOIR) { srv_pince_off(); srv_porte_off(); mot_reculer(); delay_ms(700); mot_stop(); delay_ms(100); mot_rotation_droite(); delay_ms(800); mot_stop(); delay_ms(100); mot_avancer(); } } } } // A partir de t = 3 sec else { // Attendre une balle if (bal_presence() == 0) { mot_avancer(); srv_pince_off(); srv_porte_off(); // Au cas où... } // Balle détectée else { // Blocage de la balle srv_porte_on(); srv_pince_on(); if ((balle_etait_presente & 0b11110) == 0) { // On attend pour que le capteur se stabilise delay_ms(100); mot_stop(); delay_ms(600); // Détection de la couleur de la balle bal_coul_detect(); // On ne s'intéresse pas aux balles noires if (couleur_balle == NOIR) { srv_pince_off(); srv_porte_off(); mot_reculer(); delay_ms(700); mot_stop(); delay_ms(100); mot_rotation_droite(); delay_ms(800); mot_stop(); delay_ms(100); mot_avancer(); } } // Balle blanche toujours détectée ? toujours pas d'obstacle en vue ? if (bal_presence()) { // Recherche d'un trou if (trou_centrage() == 1) { // Attente d'un trou et centrage sur le trou, 1 quand centré sur trou printf("centre sur trou\n\r"); // Détection de la couleur de la balle bal_coul_detect(); if (couleur_balle == BLANC) printf("balle blanche\n\r"); else if (couleur_balle == NOIR) printf("balle noire\n\r"); else printf("balle ???? \n\r"); // Détection de couleur coul_detect(); // Gestion arbitraire de la couleur indéterminée //if (couleur_trou_g == INDETERMINE) // couleur_trou_g = VERT; if (couleur_trou_d == INDETERMINE) couleur_trou_d = VERT; if (couleur_trou_d == ROUGE) printf("ROUGE\n\r"); else if (couleur_trou_d == VERT) printf("VERT \n\r"); else if (couleur_trou_d == BLEU) printf("BLEU \n\r"); else printf("???? \n\r"); // Bonne => on fais tomber la balle, si besoin en s'agitant un peu //le capteur de gauche ne fonctionne pas //if (((couleur_trou_g == couleur_robot || couleur_trou_d == couleur_robot) && couleur_balle == BLANC) || ((couleur_trou_g != couleur_robot && couleur_trou_d != couleur_robot) && couleur_balle == NOIR)) { //if ((couleur_trou_d == couleur_robot && couleur_balle == BLANC) || (couleur_trou_d != couleur_robot && couleur_balle == NOIR)) { // // gestion arbitraire : si c'est rouge c'est bon, sinon le vert est considéré comme bleu :( if ((((couleur_robot == ROUGE && couleur_trou_d == ROUGE) || (couleur_robot != ROUGE && couleur_trou_d != ROUGE)) && couleur_balle == BLANC) || (((couleur_robot == ROUGE && couleur_trou_d != ROUGE) || (couleur_robot != ROUGE && couleur_trou_d == ROUGE)) && couleur_balle == NOIR)) { printf("bonne couleur\n\r"); srv_pince_off(); delay_ms(500); // Est-ce que la balle n'est pas tombée ? if (bal_presence()) { // On s'agite sur un côté ou l'autre si les deux capteurs ne renvoient pas la même info mot_avancer(); delay_ms(150); mot_stop(); delay_ms(100); // On avait détecté le trou plutôt du côté gauche // Commenté car le capteur gauche ne fonctionne pas /*if ((couleur_trou_g == couleur_robot && couleur_balle == BLANC) || (couleur_trou_g != couleur_robot && couleur_balle == NOIR)) { mot_rotation_droite(); delay_ms(150); mot_stop(); delay_ms(100); mot_reculer(); delay_ms(250); mot_stop(); delay_ms(100); mot_rotation_gauche(); } // Plutôt du côté droit else {*/ mot_rotation_gauche(); delay_ms(200); mot_stop(); delay_ms(100); mot_reculer(); delay_ms(300); mot_stop(); delay_ms(100); mot_rotation_droite(); //} delay_ms(250); mot_stop(); delay_ms(500); } // Balle encore là => on reprend et on repart (trou plein ou erreur de positionnement, impossible de savoir) if (bal_presence()) { srv_pince_on(); mot_avancer(); delay_ms(1000); } // Sinon on ouvre et on repart else { srv_porte_off(); } } // Pas bonne => on repart else { printf("mauvaise couleur\n\r"); mot_stop(); delay_ms(500); mot_avancer(); delay_ms(1000); // On s'assure d'avoir passé le trou avant de retester } } } // Non => déblocage else { srv_pince_off(); srv_porte_off(); } } } } } // On évacue les balles juste avant la fin mot_stop(); delay_ms(300); srv_pince_off(); srv_porte_off(); mot_reculer(); delay_ms(500); // Après le match lcd_msg(LCD_MSG_FINMATCH); mot_stop(); //srv_rouleau_off(); #ENDIF }