FRC - Hackathon Pixy
Pixy.cpp
- Committer:
- haarkon
- Date:
- 2018-05-31
- Revision:
- 5:d395fe0fb0e0
- Parent:
- 2:90355c600404
- Child:
- 4:5982d904f7aa
- Child:
- 6:f16f250fe2cb
File content as of revision 5:d395fe0fb0e0:
#include "Pixy.h" PIXY::PIXY(PinName tx, PinName rx, int debit) { Pixy_CCFrameIsNew = -1; Pixy_NMFrameIsNew = -1; FlagPixy = 0; FlagPixyOverflow = 0; Pixy_check = -1; _Pixy = new Serial (tx, rx, debit); _Pixy->attach (callback(this,&PIXY::getPixyByte)); } void PIXY::getPixyByte () { static T_msgBuffer msgBuffer; static T_wordBuffer wordBuffer; static T_pixyState PIXY_state = none; static Byte byteCount = 0; static int PIXY_synced = 0; Word i, somme; Byte dummy; static Byte PIXY_nbCCObjet = 0, PIXY_wCCObjet = 0; static Byte PIXY_nbNMObjet = 0, PIXY_wNMObjet = 0; Pixy_check = 0; dummy = _Pixy->getc(); // On stocke l'octet reçu dans une variable temporaire if (!PIXY_synced) { // Si la camera et la carte ne sont pas synchronisées msgBuffer.octet[byteCount] = dummy; // On stocke l'octet reçu dans la première case dispo du tableau temporaire if (byteCount < 3) { // Tant qu'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 ((msgBuffer.motlong == 0xaa550000)||(msgBuffer.motlong == 0xaa560000)) { // On teste pour voir si on a reçu un entête PIXY_synced = 1; // Si oui : On passe en mode synchronisé PIXY_state = begin; // Et on va à l'aiguillage byteCount = 0; // Et on remet le comptage d'octet à 0 } else { // Si on n'a pas trouvé d'entête for (i=1; i<4; i++) msgBuffer.octet[i-1] = msgBuffer.octet[i]; // On décalle les cases du tableau byteCount = 3; // Et on attend le caractère suivant } } } else { switch (PIXY_state) { case begin : // l'aiguillage est là ! wordBuffer.octet[byteCount%2] = dummy; // on stocke les octets reçus byteCount++; if (byteCount == 2) { // Quand on a 2 octets switch (wordBuffer.mot) { case 0xaa55 : // Si c'est un bloc normal (code = 0xAA55) PIXY_state = normal; // On part vers le traitement spécifique break; case 0xaa56 : // Si c'est un bloc color Code (code = 0xAA56) PIXY_state = colorCode; // On part vers le traitement spécifique break; case 0 : // Si c'est un debut de trame (code = 0x0000) PIXY_state = doubleZero; // On part vers le traitement spécifique break; default : // Si c'est autre chose PIXY_synced = 0; // C'est qu'on est perdu donc plus synchronisé. PIXY_state = none; // Ceinture et bretelle break; } byteCount = 0; } break; case normal : // Si on a un bloc normal Pixy_NMFIFO[PIXY_wNMObjet].tab[byteCount] = dummy; // 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 de plus else FlagPixyOverflow = 1; // Si on a plus de 20 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 (0000). Pixy_NMObjet = PIXY_nbNMObjet; // On met à jour les variables pour le traitement extérieur Pixy_CCObjet = PIXY_nbCCObjet; Pixy_CCFrameIsNew = 1; Pixy_FirstCCObjet = PIXY_wCCObjet; Pixy_NMFrameIsNew = 1; Pixy_FirstNMObjet = PIXY_wNMObjet; FlagPixy = 1; // On valide le traitement de la trame précédente. PIXY_nbCCObjet = 0; // On remet à 0 les variables internes PIXY_nbNMObjet = 0; byteCount = 0; wordBuffer.octet[byteCount%2] = dummy; // on stocke les octets reçus byteCount++; PIXY_state = waitForStart; // On passe en attente de la deuxième partie du code break; case waitForStart : // Si on a reçu le code de début d'une nouvelle trame. wordBuffer.octet[byteCount%2] = dummy; // on stocke les octets reçus byteCount++; if (byteCount == 2) { // Quand on a 2 octets byteCount = 0; if (wordBuffer.mot == 0xaa55) { // On vérifie qu'on a bien reçu l'entête de depart PIXY_state = begin; // Si c'est le cas, alors tout va bien et on va à l'aiguillage } else { // Si ce n'est pas le cas PIXY_synced = 0; // On n'est plus synchronisés PIXY_state = none; // Ceinture et Bretelles } } break; } } } int PIXY::detectedObject (int* nbNM, int* nbCC) { *nbNM = (int)Pixy_NMObjet; *nbCC = (int)Pixy_CCObjet; if (Pixy_check!=0) return -1; if (FlagPixyOverflow) return -2; return 0; } T_pixyCCBloc PIXY::getCCBloc (void) { T_pixyCCBloc dummy; static Byte PIXY_rCCObjet = 0; if (Pixy_CCObjet !=0) { if (Pixy_CCFrameIsNew) { //Si on est au début d'une nouvelle trame, on initialise le pointeur de lecture if (Pixy_FirstCCObjet <= Pixy_CCObjet) PIXY_rCCObjet = 20 - Pixy_CCObjet + Pixy_FirstCCObjet ; else PIXY_rCCObjet = Pixy_FirstCCObjet - Pixy_CCObjet ; Pixy_CCFrameIsNew = 0; } dummy = Pixy_CCFIFO[PIXY_rCCObjet].CCbloc; if (PIXY_rCCObjet < 19) PIXY_rCCObjet++; // On incrémente le pointeur de lecture dans la FIFO CCObjet else PIXY_rCCObjet = 0; Pixy_CCObjet--; // On dit que l'on a un objet CC de moins à traiter } else { dummy.signature = 0xFFFF; } return dummy; } T_pixyNMBloc PIXY::getNMBloc (void) { T_pixyNMBloc dummy; static Byte PIXY_rNMObjet = 0; if (Pixy_NMObjet !=0) { if (Pixy_NMFrameIsNew) { //Si on est au début d'une nouvelle trame, on initialise le pointeur de lecture if (Pixy_FirstNMObjet <= Pixy_NMObjet) PIXY_rNMObjet = 20 - Pixy_NMObjet + Pixy_FirstNMObjet ; else PIXY_rNMObjet = Pixy_FirstNMObjet - Pixy_NMObjet ; Pixy_NMFrameIsNew = 0; } dummy = Pixy_NMFIFO[PIXY_rNMObjet].NMbloc; if (PIXY_rNMObjet < 19) PIXY_rNMObjet++; // On incrémente le pointeur de lecture dans la FIFO CCObjet else PIXY_rNMObjet = 0; Pixy_NMObjet--; // On dit que l'on a un objet CC de moins à traiter } else { dummy.signature = 0xFFFF; } return dummy; } void PIXY::setBrightness (Byte brightness) { _Pixy->putc(0); //Start _Pixy->putc(0xFE); //Brightness Level _Pixy->putc(brightness); } int PIXY::checkNewImage (void) { if (FlagPixy) { FlagPixy = 0; return 1; } else { return 0; } } int PIXY::checkPixy (void) { return Pixy_check; }