![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Pixi cachan
Dependencies: Encoder_Nucleo_32_bits PwmIn mbed
Fork of v2 by
Diff: main.cpp
- Revision:
- 2:1d440e938c44
- Parent:
- 1:d95546f84105
- Child:
- 3:3dfe26a10796
--- a/main.cpp Mon May 29 16:23:47 2017 +0000 +++ b/main.cpp Wed May 31 06:39:30 2017 +0000 @@ -11,7 +11,14 @@ #include "PwmIn.h" #include "Nucleo_Encoder_16_bits.h" -#define BOUSSOLE_adress 0xC0 +#define BOUSSOLE_adress 0xC0 +#define PIXY_adress 0x54 + +#define CC_BLOCSIZE 14 +#define N_BLOCSIZE 12 + +#define N_BLOCCODE 0xAA55 +#define CC_BLOCCODE 0xAA56 typedef unsigned char Byte; typedef unsigned short Word; @@ -19,10 +26,17 @@ typedef enum {S_monte = 1, S_descente = 0} T_SERVODIR; +typedef enum {none, begin, normal, colorCode, doubleZero} T_pixyState; + typedef union { - lWord header; + lWord mot; Byte tab[4]; -} T_tmpbuffer; +} T_tmpBuffer; + +typedef union { + Word mot; + Byte tab[2]; +} T_structBuffer; typedef struct { Word checksum; @@ -31,16 +45,30 @@ Word y; Word width; Word height; - Word angle; // angle is only available for color coded blocks + Word angle; } T_pixyCCBloc; +typedef struct { + Word checksum; + Word signature; + Word x; + Word y; + Word width; + Word height; +} T_pixyBloc; typedef union { Byte tab[14]; T_pixyCCBloc CCbloc; +} T_pixyCCData; + +typedef union { + Byte tab[12]; + T_pixyBloc bloc; } T_pixyData; + /** Liste des objets * * Serial #4 Pixy @@ -167,8 +195,8 @@ 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 timer; -Timer temps; +Ticker tick; +Timer times; /** Liste des variables globales * @@ -180,15 +208,18 @@ */ // 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 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_pixyData Pixy_FIFO[20]; -Byte Pixy_nbObjet = 0, Pixy_wObjet = 0, Pixy_rObjet = 0; +T_pixyCCData Pixy_CCFIFO[20]; +T_pixyData Pixy_FIFO[20]; +Byte Pixy_saveNbCCObjet; +Byte Pixy_saveNbObjet; // Gestion des capteurs Ultrason long Echo1Start, Echo2Start, Echo3Start; @@ -212,87 +243,176 @@ void echo1Rise () { - Echo1Start = temps.read_us(); + Echo1Start = times.read_us(); } void echo2Rise () { - Echo2Start = temps.read_us(); + Echo2Start = times.read_us(); } void echo3Rise () { - Echo3Start = temps.read_us(); + Echo3Start = times.read_us(); } void echo1Fall () { - DistUS1 = (double)(temps.read_us() - Echo1Start)/5.8; + DistUS1 = (double)(times.read_us() - Echo1Start)/5.8; FlagUS1 = 1; } void echo2Fall () { - DistUS2 = (double)(temps.read_us() - Echo2Start)/5.8; + DistUS2 = (double)(times.read_us() - Echo2Start)/5.8; FlagUS2 = 1; } void echo3Fall () { - DistUS3 = (double)(temps.read_us() - Echo3Start)/5.8; + 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; + static int dummy; + int i, somme; + static Byte Pixy_nbCCObjet = 0, Pixy_wCCObjet = 0; + static Byte Pixy_nbObjet = 0, Pixy_wObjet = 0; - static T_tmpbuffer tmpBuffer; - static Byte bytecount = 0; - static int Pixy_synced = 0; - int i; + Pixy_check = 0; - Led2 = ! Led2; - if (!Pixy_synced) { // On n'a pas trouvé le départ (0x55aa55aa ou 0x55aa56aa) - 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 { // Si on a 4 octets de données - if (tmpBuffer.header == 0x55aa56aa) { // Si on a un Color Code Bloc - if (Pixy_wObjet < 19) Pixy_wObjet++; // On incrémente le pointeur d'écriture dans la FIFO Objet - else Pixy_wObjet = 0; - Pixy_nbObjet++; // On dit que l'on a un objet de plus - if (Pixy_nbObjet >= 20) FlagPixyOverflow = 1; // Si on a plus de 20 objets (en attente) => Overflow - Pixy_synced = 1; // Et on peut dire que l'on a synchronisé la Pixy - bytecount = 0; - } else { // Si on n'a pas vu le header d'un Color Code Bloc - for (i=1; i<4; i++) tmpBuffer.tab[i-1] = tmpBuffer.tab[i]; // On décalle les cases du tableau + 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; } } - } else { // La Pixy est synchronisée - Pixy_FIFO[Pixy_wObjet].tab[bytecount] = Pixy.getc(); // On stocke les octets un à un dans la structure CCBloc - if (bytecount < 14) bytecount++; // tant que la structure n'est pas pleine - else { // Quand elle est pleine - bytecount = 0; // On réinitialise - Pixy_synced = 0; - FlagPixy = 1; // Et on signale au main qu'un objet est pret à être analysé. + } + + 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_FIFO[Pixy_wObjet].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_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; + + 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 + 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. + + 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 + + } 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; } } } + int main() { - int I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, PIXY_check = -1, MOTG_check = -1, MOTD_check = -1*/; - int phase = 0; + 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; + int PIXY_objet; double SERVO_angle = 0, SERVO_angleMAX = 180, SERVO_angleMIN = 0; Word SERVO_pos; T_SERVODIR SERVO_dir = S_monte; - Byte PIXY_red = 0, PIXY_green = 0, PIXY_blue = 0; char MENU_choix = 0; - char BOUSSOLE_status[1] = {0}/*, BOUSSOLE_bearingWord[1] = {2}*/; + char BOUSSOLE_status[1] = {0}; char I2C_registerValue[4]; double CAP_I2C, CAP_PWM; @@ -300,11 +420,10 @@ double SD1_dist, SD2_dist, LD1_dist, LD2_dist; double periode; - temps.reset(); - temps.start(); + times.reset(); + times.start(); - - timer.attach(&tickTime, 0.001); + tick.attach(&tickTime, 0.001); Bus_I2C.frequency (100000); @@ -441,12 +560,12 @@ CAP_PWM = ((PWMB.pulsewidth()*1000)-1)*10; Pc.printf ("\r PWM = %4.1lf", CAP_PWM); } - //if (I2C_check==0) { + if (I2C_check==0) { Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true); Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4); CAP_I2C = (double)(((unsigned short)I2C_registerValue[2]<<8)+(unsigned short)I2C_registerValue[3])/10.0; Pc.printf ("\r\t\t I2C = %4.1lf", CAP_I2C); - //} + } } } while(!Pc.readable()); MENU_choix = Pc.getc(); @@ -544,8 +663,8 @@ Led1 = !Led1; FlagTickLed = 0; } - - + + } while(!Pc.readable()); MENU_choix = Pc.getc(); @@ -600,86 +719,35 @@ case 9 : Pc.printf ("\n\n\rPixy\n"); Pc.printf ("\rAppuyez sur Entree pour quitter\n"); + + if (Pixy_check == 0) Pc.printf ("\n\rPIXY is ALIVE\n"); + else Pc.printf ("\n\rPIXY don't talk\n"); + do { - if (((Tick%50)==0) && FlagTick) { - FlagTick = 0; - switch (phase) { - case 0 : - PIXY_red += 5; - if (PIXY_red == 255) { - phase = 1; - //Pc.printf ("\rPhase 1"); - } - break; - case 1 : - PIXY_green += 5; - if (PIXY_green == 255) { - phase = 2; - //Pc.printf ("\rPhase 2"); - } - break; - case 2 : - PIXY_red -= 5; - if (PIXY_red == 0) { - phase = 3; - //Pc.printf ("\rPhase 3"); - } - break; - case 3 : - PIXY_blue += 5; - if (PIXY_blue == 255) { - phase = 4; - //Pc.printf ("\rPhase 4"); - } - break; - case 4 : - PIXY_green -= 5; - if (PIXY_green == 0) { - phase = 5; - //Pc.printf ("\rPhase 5"); - } - break; - case 5 : - PIXY_red += 5; - if (PIXY_red == 255) { - phase = 6; - //Pc.printf ("\rPhase 6"); - } - break; - case 6 : - PIXY_green += 5; - if (PIXY_green == 255) { - phase = 7; - //Pc.printf ("\rPhase 7"); - } - break; - case 7 : - PIXY_red -= 5; - PIXY_green -= 5; - PIXY_blue -=5; - if (PIXY_red == 0) { - phase = 0; - //Pc.printf ("\rPhase 0"); - } - break; + if (FlagPixy) { + Led2 = 1; + + if (FlagPixyOverflow) { + Pc.printf ("\rSome Data were lost\n"); + FlagPixyOverflow = 0; } - Pixy.putc(0); - Pixy.putc(0xFD); - Pixy.putc(PIXY_red); - Pixy.putc(PIXY_green); - Pixy.putc(PIXY_blue); + 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); + + if (PIXY_rObjet<19) PIXY_rObjet++; + else PIXY_rObjet = 0; + } + Pixy_saveNbObjet = 0; - while (Pixy_nbObjet) { - Pc.printf ("\r%5hd = %5hd,%5hd : %5hdx%5hd", Pixy_FIFO[Pixy_rObjet].CCbloc.signature, Pixy_FIFO[Pixy_rObjet].CCbloc.x, Pixy_FIFO[Pixy_rObjet].CCbloc.y, Pixy_FIFO[Pixy_rObjet].CCbloc.width, Pixy_FIFO[Pixy_rObjet].CCbloc.height); - - if (Pixy_nbObjet>1) Pc.printf("\n"); - - if (Pixy_rObjet<19) Pixy_rObjet++; - else Pixy_rObjet = 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); - Pixy_nbObjet--; - } + if (PIXY_rCCObjet<19) PIXY_rCCObjet++; + else PIXY_rCCObjet = 0; + } + Pixy_saveNbCCObjet = 0; FlagPixy = 0; + Led2 = 0; } if (FlagTickLed) {