FRC - Hackathon Pixy
Diff: Pixy.cpp
- Revision:
- 0:6f78c735f07c
- Child:
- 2:90355c600404
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Pixy.cpp Sun May 20 08:18:49 2018 +0000 @@ -0,0 +1,204 @@ +#include "Pixy.h" + +PIXY::PIXY(PinName tx, PinName rx, int debit) +{ + Pixy_CCFrameIsNew = -1; + Pixy_NMFrameIsNew = -1; + FlagPixy = 0; + FlagPixyOverflow = 0; + Pixy_check = -1; + + _cmucam = new Serial (tx, rx, debit); + _cmucam->attach (this,&PIXY::getPixyByte); + +} + + +void PIXY::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] = _cmucam->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] = _cmucam->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] = _cmucam->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 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. + + msgBuffer.tab[byteCount%2] = _cmucam->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; + 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. + } 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 PIXY::detectedObject (int* nbNormal, int* nbCC) +{ + *nbNormal = (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; +} + +int PIXY::setBrightness (Byte brightness) +{ + + Byte PixyMsgBuf[3]; + + PixyMsgBuf[0] = 0x00; //Start + PixyMsgBuf[1] = 0xFE; //Brightness Level + PixyMsgBuf[2] = brightness; + + return _cmucam->write(PixyMsgBuf,3,NULL,0); +} \ No newline at end of file