Automate
Dependencies: Encoder_Nucleo_16_bits PwmIn mbed
Fork of TestBoard by
Diff: function_library.h
- Revision:
- 8:ad8b64ca548d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/function_library.h Fri Jun 09 17:06:05 2017 +0000 @@ -0,0 +1,275 @@ +//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; + } +} \ No newline at end of file