![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Moteurs
Dependencies: Encoder_Nucleo_16_bits PwmIn mbed
Fork of Automate by
function_library.h
- Committer:
- DOREL
- Date:
- 2017-06-09
- Revision:
- 8:ad8b64ca548d
- Child:
- 9:168226ff8f76
File content as of revision 8:ad8b64ca548d:
//Variables Serial Pixy (PA_0, PA_1, 230400); Serial Pc (PA_2, PA_3, 921600); AnalogIn CNY1 (PC_4); AnalogIn CNY2 (PA_7); AnalogIn CNY3 (PA_6); AnalogIn LD1 (PC_0); AnalogIn LD2 (PC_1); AnalogIn SD1 (PB_0); AnalogIn SD2 (PA_4); AnalogIn Vbat (PC_5); DigitalOut Led1 (PA_5); DigitalOut Led2 (PD_2); DigitalOut Trig1 (PC_3); DigitalOut Trig2 (PC_2); DigitalOut Trig3 (PA_9); DigitalOut En (PC_9); DigitalOut SensG (PC_8); DigitalOut SensD (PC_6); DigitalOut SS (PA_12); InterruptIn Echo1 (PA_10); InterruptIn Echo2 (PA_11); InterruptIn Echo3 (PB_12); InterruptIn BP (PC_13); InterruptIn IG (PC_7); InterruptIn PWMG (PB_2); InterruptIn PWMD (PB_1); PwmIn PWMB (PA_15); PwmOut Pwm_MG (PB_10); PwmOut Pwm_MD (PB_3); PwmOut Servo (PA_8); I2C Bus_I2C (PB_9, PB_8); SPI SPIG (PC_12, PC_11, PC_10); SPI SPID (PB_15, PB_14, PB_13); Nucleo_Encoder_16_bits Gauche (TIM4); // A = PB_6, B = PB_7 Nucleo_Encoder_16_bits Droite (TIM3); // A = PB_4, B = PB_5 Ticker tick; Timer times; /** Liste des variables globales * * Tick -> (long) Compte le nombre de MS écoulé et déclenche l'exécution de la boucle en fonction du temps écoulé. * FlagIG -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue gauche * FlagID -> (int) Indication de la présence de fronts sur l'index de l'encodeur de la roue droite * EchoXStart -> (long) Donne le temps en µs de début de l'impulsion d'écho de l'US n°X * DistUSX -> (float) Donne la distance en mm mesurée par l'US n°X */ // Structure de temps lWord Tick = 0, Gperiod, Dperiod; // Sémaphore d'interruption int FlagUS1 = 0, FlagUS2 = 0, FlagUS3 = 0, FlagPixy = 0, FlagPixyOverflow = 0; int FlagTick = 0, FlagTickLed = 0, BPPressed = 0, nbTurnG = 0, nbTurnD = 0; int Pixy_check = -1; // Dialogue avec la Pixy T_pixyCCData Pixy_CCFIFO[20]; T_pixyNMData Pixy_NMFIFO[20]; Byte Pixy_CCObjet, Pixy_NMObjet; // Gestion des capteurs Ultrason long Echo1Start, Echo2Start, Echo3Start; double DistUS1, DistUS2, DistUS3; /** Liste des interruptions * */ void tickTime() { Tick++; FlagTick = 1; if ((Tick%100)==0) FlagTickLed = 1; } void BPevent () { BPPressed = 1; } void PWM_motGRise (void) { static lWord oldTime; lWord now; now = times.read_us(); Gperiod = now-oldTime; oldTime = now; nbTurnG++; } void PWM_motDRise (void) { static lWord oldTime; lWord now; now = times.read_us(); Dperiod = now-oldTime; oldTime = now; nbTurnD++; } void echo1Rise () { Echo1Start = times.read_us(); } void echo2Rise () { Echo2Start = times.read_us(); } void echo3Rise () { Echo3Start = times.read_us(); } void echo1Fall () { DistUS1 = (double)(times.read_us() - Echo1Start)/5.8; FlagUS1 = 1; } void echo2Fall () { DistUS2 = (double)(times.read_us() - Echo2Start)/5.8; FlagUS2 = 1; } void echo3Fall () { DistUS3 = (double)(times.read_us() - Echo3Start)/5.8; FlagUS3 = 1; } void getPixyByte () { static T_tmpBuffer tmpBuffer; static T_structBuffer msgBuffer; static T_pixyState PIXY_state = none; static Byte byteCount = 0; static int PIXY_synced = 0, dummy; int i, somme; static Byte PIXY_nbCCObjet = 0, PIXY_wCCObjet = 0; static Byte PIXY_nbNMObjet = 0, PIXY_wNMObjet = 0; Pixy_check = 0; if (!PIXY_synced) { // On n'a pas trouvé le START (0x55aa0000) tmpBuffer.tab[byteCount] = Pixy.getc(); // On stocke l'octet reçu dans la première case dispo du tableau temporaire if (byteCount < 3) { // Si on n'a pas encore reçu les 4 premier octets byteCount++; // On passe à la case suivante du tableau temporaire } else { // Lorsqu'on a 4 octets if (tmpBuffer.mot != 0xaa550000) { // Si le code n'est pas le code de START for (i=1; i<4; i++) tmpBuffer.tab[i-1] = tmpBuffer.tab[i]; // On décalle les cases du tableau byteCount = 3; // Et on attend le caractère suivant } else { // Si on a trouvé le code de START PIXY_synced = 1; // On passe en mode synchronisé PIXY_state = begin; byteCount = 0; } } } if (PIXY_synced) { switch (PIXY_state) { case begin : // l'aiguillage est là ! msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus byteCount++; if (byteCount == 2) { // Quand on a 2 octets if (msgBuffer.mot == 0xaa55) { // Si c'est un bloc normal (code 0xAA55) PIXY_state = normal; // On part vers le traitement spécifique } if (msgBuffer.mot == 0xaa56) { // Si c'est un bloc Color Code (code 0xAA56) PIXY_state = colorCode; // On part vers le traitement spécifique } if (msgBuffer.mot == 0) { // Si on a un debut de trame (code 0000) PIXY_state = doubleZero; // On part vers le traitement spécifique } if ((PIXY_state == begin) || (PIXY_state == none)) { // Si c'est autre chose PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé. PIXY_state = none; // Ceinture et bretelle } byteCount = 0; } break; case normal : // Si on a un bloc normal Pixy_NMFIFO[PIXY_wNMObjet].tab[byteCount] = Pixy.getc(); // On stocke les octets un à un dans la structure Bloc if (byteCount < 11) { // Tant que la structure n'est pas pleine byteCount++; // On passe à l'octet suivant } else { // Quand elle est pleine byteCount = 0; // On réinitialise PIXY_state = begin; // On retourne à l'aiguillage // On calcule la somme de contrôle 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; if (somme == Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.checksum) { // Si le checksum est bon, on valide la réception if (PIXY_wNMObjet < 19) PIXY_wNMObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet else PIXY_wNMObjet = 0; if (PIXY_nbNMObjet < 19) PIXY_nbNMObjet++; // On dit que l'on a un objet CC de plus else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow } } break; case colorCode : // Si on a un bloc colorCode Pixy_CCFIFO[PIXY_wCCObjet].tab[byteCount] = dummy; // On stocke les octets un à un dans la structure CCBloc if (byteCount < 13) byteCount++; // tant que la structure n'est pas pleine on passe à l'octet suivant else { // Quand elle est pleine byteCount = 0; // On réinitialise PIXY_state = begin; // On retourne à l'aiguillage // On calcule la somme de contrôle 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; if (somme == Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.checksum) { // Si le checksum est bon if (PIXY_wCCObjet < 19) PIXY_wCCObjet++; // On incrémente le pointeur d'écriture dans la FIFO CCObjet else PIXY_wCCObjet = 0; if (PIXY_nbCCObjet < 19) PIXY_nbCCObjet++; // On dit que l'on a un objet CC de plus à traiter else FlagPixyOverflow = 1; // Si on a plus de 20 CC objets (en attente) => Overflow } } break; case doubleZero : // Si on a reçu le code de début d'une nouvelle trame. msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus byteCount++; if (byteCount == 2) { // Quand on a 2 octets if (msgBuffer.mot == 0xaa55) { // On doit impérativement trouver le code 0xAA55 PIXY_state = begin; // Si c'est le cas, alors tout va bien et on va à l'aiguillage Pixy_NMObjet = PIXY_nbNMObjet; // On met à jour les variables pour le traitement Pixy_CCObjet = PIXY_nbCCObjet; PIXY_nbCCObjet = 0; PIXY_nbNMObjet = 0; FlagPixy = 1; // On valide le traitement de la trame précédente. } else { // Si on trouve autre chose PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé. PIXY_state = none; // Ceinture et bretelle } byteCount = 0; } break; } } } void ir_read(double SD2,double LD2,double *SD2_dist,double *LD2_dist) { double SD2_val, LD2_val; SD2_val = SD2; LD2_val = LD2; if (SD2_val < 0.06) { SD2_val = 0; *SD2_dist = 40; } else { *SD2_dist = 11.611/(SD2_val*3.3-0.0237); if (*SD2_dist > 40) *SD2_dist = 40; } if (LD2_val < 0.1) { LD2_val = 0; *LD2_dist = 150; } else { *LD2_dist = 59.175/(LD2_val*3.3-0.0275); if (*LD2_dist > 150) *LD2_dist = 150; } }