Automate
Dependencies: Encoder_Nucleo_16_bits PwmIn mbed
Fork of TestBoard by
Diff: main.cpp
- Revision:
- 3:3dfe26a10796
- Parent:
- 2:1d440e938c44
- Child:
- 4:f65b0905e4ce
diff -r 1d440e938c44 -r 3dfe26a10796 main.cpp --- a/main.cpp Wed May 31 06:39:30 2017 +0000 +++ b/main.cpp Wed May 31 07:57:18 2017 +0000 @@ -55,7 +55,7 @@ Word y; Word width; Word height; -} T_pixyBloc; +} T_pixyNMBloc; typedef union { Byte tab[14]; @@ -64,8 +64,8 @@ typedef union { Byte tab[12]; - T_pixyBloc bloc; -} T_pixyData; + T_pixyNMBloc NMbloc; +} T_pixyNMData; @@ -208,22 +208,21 @@ */ // Structure de temps -lWord Tick = 0; +lWord Tick = 0; // Sémaphore d'interruption -int FlagUS1 = 0, FlagUS2 = 0, FlagUS3 = 0, FlagPixy = 0, FlagPixyOverflow = 0; -int FlagTick = 0, FlagTickLed = 0, BPPressed = 0; -int Pixy_check = -1; +int FlagUS1 = 0, FlagUS2 = 0, FlagUS3 = 0, FlagPixy = 0, FlagPixyOverflow = 0; +int FlagTick = 0, FlagTickLed = 0, BPPressed = 0; +int Pixy_check = -1; // Dialogue avec la Pixy -T_pixyCCData Pixy_CCFIFO[20]; -T_pixyData Pixy_FIFO[20]; -Byte Pixy_saveNbCCObjet; -Byte Pixy_saveNbObjet; +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; +long Echo1Start, Echo2Start, Echo3Start; +double DistUS1, DistUS2, DistUS3; /** Liste des interruptions * @@ -278,17 +277,16 @@ { static T_tmpBuffer tmpBuffer; static T_structBuffer msgBuffer; - static T_pixyState Pixy_state = none; + static T_pixyState PIXY_state = none; static Byte byteCount = 0; - static int Pixy_synced = 0; - static int dummy; + static int PIXY_synced = 0, dummy; int i, somme; - static Byte Pixy_nbCCObjet = 0, Pixy_wCCObjet = 0; - static Byte Pixy_nbObjet = 0, Pixy_wObjet = 0; + 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) + 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 @@ -298,16 +296,16 @@ 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; + PIXY_synced = 1; // On passe en mode synchronisé + PIXY_state = begin; byteCount = 0; } } } - if (Pixy_synced) { + if (PIXY_synced) { - switch (Pixy_state) { + switch (PIXY_state) { case begin : // l'aiguillage est là ! msgBuffer.tab[byteCount%2] = Pixy.getc(); // on stocke les octets reçus @@ -315,19 +313,19 @@ 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 + 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 + 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 + 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 + 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; } @@ -335,19 +333,19 @@ case normal : // Si on a un bloc normal - Pixy_FIFO[Pixy_wObjet].tab[byteCount] = Pixy.getc(); // On stocke les octets un à un dans la structure Bloc + 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 + PIXY_state = begin; // On retourne à l'aiguillage // On calcule la somme de contrôle - somme = Pixy_FIFO[Pixy_wObjet].bloc.signature + Pixy_FIFO[Pixy_wObjet].bloc.x + Pixy_FIFO[Pixy_wObjet].bloc.y + Pixy_FIFO[Pixy_wObjet].bloc.width + Pixy_FIFO[Pixy_wObjet].bloc.height; + 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_FIFO[Pixy_wObjet].bloc.checksum) { // Si le checksum est bon, on valide la réception - if (Pixy_wObjet < 19) Pixy_wObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet - else Pixy_wObjet = 0; - if (Pixy_nbObjet < 19) Pixy_nbObjet++; // On dit que l'on a un objet CC de plus + 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 } } @@ -355,18 +353,18 @@ 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 + 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 + 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; + 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 + 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 } } @@ -374,21 +372,19 @@ case doubleZero : // Si on a reçu le code de début d'une nouvelle trame. - Pixy_saveNbCCObjet = Pixy_nbCCObjet; - Pixy_saveNbObjet = Pixy_nbObjet; - Pixy_nbCCObjet = 0; - Pixy_nbObjet = 0; - FlagPixy = 1; // On valide le traitement de la trame précédente. - 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_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 + PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé. + PIXY_state = none; // Ceinture et bretelle } byteCount = 0; } @@ -403,7 +399,7 @@ int I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/; - Byte PIXY_rCCObjet = 0, PIXY_rObjet = 0; + Byte PIXY_rCCObjet = 0, PIXY_rNMObjet = 0; int PIXY_objet; double SERVO_angle = 0, SERVO_angleMAX = 180, SERVO_angleMIN = 0; @@ -414,11 +410,11 @@ char BOUSSOLE_status[1] = {0}; char I2C_registerValue[4]; + double BOUSSOLE_periode; double CAP_I2C, CAP_PWM; double SD1_val, SD2_val, LD1_val, LD2_val, CNY1_val, CNY2_val, CNY3_val, Vbat_val; double SD1_dist, SD2_dist, LD1_dist, LD2_dist; - double periode; times.reset(); times.start(); @@ -539,9 +535,9 @@ Pc.printf (" FAIL\n"); } - periode = PWMB.period(); + BOUSSOLE_periode = PWMB.period(); Pc.printf ("\rVerif de la PWM :"); - if ((periode > 0.11) || (periode < 0.06)) { + if ((BOUSSOLE_periode > 0.11) || (BOUSSOLE_periode < 0.06)) { Pc.printf (" FAIL\n\n"); } else { Pc.printf (" OK\n\n"); @@ -663,9 +659,6 @@ Led1 = !Led1; FlagTickLed = 0; } - - - } while(!Pc.readable()); MENU_choix = Pc.getc(); break; @@ -725,29 +718,28 @@ do { if (FlagPixy) { - Led2 = 1; if (FlagPixyOverflow) { Pc.printf ("\rSome Data were lost\n"); FlagPixyOverflow = 0; } - for (PIXY_objet = 0; PIXY_objet < Pixy_saveNbObjet; PIXY_objet++) { - Pc.printf ("\robj #%hd/%hd : sig = %hd : X=%5hd, Y=%5hd (W=%5hd, H=%5hd)\n", PIXY_objet+1, Pixy_saveNbObjet, Pixy_FIFO[PIXY_rObjet].bloc.signature, Pixy_FIFO[PIXY_rObjet].bloc.x, Pixy_FIFO[PIXY_rObjet].bloc.y, Pixy_FIFO[PIXY_rObjet].bloc.width, Pixy_FIFO[PIXY_rObjet].bloc.height); + for (PIXY_objet = 0; PIXY_objet < Pixy_NMObjet; PIXY_objet++) { + 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); - if (PIXY_rObjet<19) PIXY_rObjet++; - else PIXY_rObjet = 0; + if (PIXY_rNMObjet<19) PIXY_rNMObjet++; + else PIXY_rNMObjet = 0; } - Pixy_saveNbObjet = 0; + Pixy_NMObjet = 0; - for (PIXY_objet = 0; PIXY_objet < Pixy_saveNbCCObjet; PIXY_objet++) { - Pc.printf ("\rCCobj #%hd/%hd : sig = %hd : X=%5hd, Y=%5hd (W=%5hd, H=%5hd)\n", PIXY_objet+1, Pixy_saveNbCCObjet, 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); + for (PIXY_objet = 0; PIXY_objet < Pixy_CCObjet; PIXY_objet++) { + 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); if (PIXY_rCCObjet<19) PIXY_rCCObjet++; else PIXY_rCCObjet = 0; } - Pixy_saveNbCCObjet = 0; + Pixy_CCObjet = 0; + Pc.printf("\n\r"); FlagPixy = 0; - Led2 = 0; } if (FlagTickLed) {