Pixy.cpp

00001 #include "Pixy.h"
00002 
00003 PIXY::PIXY(PinName tx, PinName rx, int debit)
00004 {
00005     Pixy_CCFrameIsNew = -1;
00006     Pixy_NMFrameIsNew = -1;
00007     FlagPixy = 0;
00008     FlagPixyOverflow = 0;
00009     Pixy_check = -1;
00010 
00011     _Pixy = new Serial (tx, rx, debit);
00012     _Pixy->attach (callback(this,&PIXY::getPixyByte));
00013 
00014 }
00015 
00016 
00017 void PIXY::getPixyByte ()
00018 {
00019     static T_msgBuffer      msgBuffer;
00020     static T_wordBuffer     wordBuffer;
00021     static T_pixyState      PIXY_state = none;
00022     static Byte             byteCount = 0;
00023     static int              PIXY_synced = 0;
00024     Word                    i, somme;
00025     Byte                    dummy;
00026     static Byte             PIXY_nbCCObjet = 0, PIXY_wCCObjet = 0;
00027     static Byte             PIXY_nbNMObjet = 0, PIXY_wNMObjet = 0;
00028 
00029     Pixy_check = 0;
00030 
00031     dummy = _Pixy->getc();                                                                // On stocke l'octet reçu dans une variable temporaire
00032 
00033     if (!PIXY_synced) {                                                                 // Si la camera et la carte ne sont pas synchronisées
00034         msgBuffer.octet[byteCount] = dummy;                                             // On stocke l'octet reçu dans la première case dispo du tableau temporaire
00035 
00036         if (byteCount < 3) {                                                            // Tant qu'on n'a pas encore reçu les 4 premier octets
00037             byteCount++;                                                                    // On passe à la case suivante du tableau temporaire
00038         } else {                                                                        // Lorsqu'on a 4 octets
00039             if ((msgBuffer.motlong == 0xaa550000)||(msgBuffer.motlong == 0xaa560000)) { // On teste pour voir si on a reçu un entête
00040                 PIXY_synced = 1;                                                            // Si oui : On passe en mode synchronisé
00041                 PIXY_state = begin;                                                         // Et on va à l'aiguillage
00042                 byteCount = 0;                                                              // Et on remet le comptage d'octet à 0
00043             } else {                                                                    // Si on n'a pas trouvé d'entête
00044                 for (i=1; i<4; i++) msgBuffer.octet[i-1] = msgBuffer.octet[i];              // On décalle les cases du tableau
00045                 byteCount = 3;                                                              // Et on attend le caractère suivant
00046             }
00047         }
00048     } else {
00049         switch (PIXY_state) {
00050             case begin :                                                        // l'aiguillage est là !
00051                 wordBuffer.octet[byteCount%2] = dummy;                          // on stocke les octets reçus
00052                 byteCount++;
00053                 if (byteCount == 2) {                                           // Quand on a 2 octets
00054                     switch (wordBuffer.mot) {
00055                         case NM_BLOCCODE :                                      // Si c'est un bloc normal (code = 0xAA55)
00056                             PIXY_state = normal;                                // On part vers le traitement spécifique
00057                             break;
00058 
00059                         case CC_BLOCCODE :                                      // Si c'est un bloc color Code (code = 0xAA56)
00060                             PIXY_state = colorCode;                             // On part vers le traitement spécifique
00061                             break;
00062 
00063                         case 0 :                                                // Si c'est un debut de trame (code = 0x0000)
00064                             PIXY_state = doubleZero;                            // On part vers le traitement spécifique
00065                             break;
00066 
00067                         default :                                               // Si c'est autre chose
00068                             PIXY_synced = 0;                                    // C'est qu'on est perdu donc plus synchronisé.
00069                             PIXY_state = none;                                  // Ceinture et bretelle
00070                             break;
00071                     }
00072                     byteCount = 0;
00073                 }
00074                 break;
00075 
00076             case normal :                                                       // Si on a un bloc normal
00077 
00078                 Pixy_NMFIFO[PIXY_wNMObjet].tab[byteCount] = dummy;              // On stocke les octets un à un dans la structure Bloc
00079                 if (byteCount < (NM_BLOCSIZE-1)) {                              // Tant que la structure n'est pas pleine
00080                     byteCount++;                                                // On passe à l'octet suivant
00081                 } else {                                                        // Quand elle est pleine
00082                     byteCount = 0;                                              // On réinitialise
00083                     PIXY_state = begin;                                         // On retourne à l'aiguillage
00084 
00085                     // On calcule la somme de contrôle
00086                     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;
00087 
00088                     if (somme == Pixy_NMFIFO[PIXY_wNMObjet].NMbloc.checksum) {  // Si le checksum est bon, on valide la réception
00089                         if (PIXY_wNMObjet < (NM_MAXOBJECT-1))   PIXY_wNMObjet++;            // On incrémente le pointeur d'écriture dans la FIFO Objet
00090                         else                                    PIXY_wNMObjet = 0;
00091                         if (PIXY_nbNMObjet < (NM_MAXOBJECT-1))  PIXY_nbNMObjet++;           // On dit que l'on a un objet de plus
00092                         else                                    FlagPixyOverflow = 1;       // Si on a plus de NM_MAXOBJECT objets (en attente) => Overflow
00093                     }
00094                 }
00095                 break;
00096 
00097             case colorCode :                                                    // Si on a un bloc colorCode
00098 
00099                 Pixy_CCFIFO[PIXY_wCCObjet].tab[byteCount] = dummy;              // On stocke les octets un à un dans la structure CCBloc
00100                 if (byteCount < (CC_BLOCSIZE-1)) byteCount++;                   // tant que la structure n'est pas pleine on passe à l'octet suivant
00101                 else {                                                          // Quand elle est pleine
00102                     byteCount = 0;                                              // On réinitialise
00103                     PIXY_state = begin;                                         // On retourne à l'aiguillage
00104 
00105                     // On calcule la somme de contrôle
00106                     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;
00107 
00108                     if (somme == Pixy_CCFIFO[PIXY_wCCObjet].CCbloc.checksum) {  // Si le checksum est bon
00109                         if (PIXY_wCCObjet < (CC_MAXOBJECT-1))   PIXY_wCCObjet++;            // On incrémente le pointeur d'écriture dans la FIFO CCObjet
00110                         else                                    PIXY_wCCObjet = 0;
00111                         if (PIXY_nbCCObjet < (CC_MAXOBJECT-1))  PIXY_nbCCObjet++;           // On dit que l'on a un objet CC de plus à traiter
00112                         else                                    FlagPixyOverflow = 1;       // Si on a plus de CC_MAXOBJECT CC objets (en attente) => Overflow
00113                     }
00114                 }
00115                 break;
00116 
00117             case doubleZero :                                                   // Si on a reçu le code de début d'une nouvelle trame (0000).
00118 
00119                 Pixy_NMObjet = PIXY_nbNMObjet;                                  // On met à jour les variables pour le traitement extérieur
00120                 Pixy_CCObjet = PIXY_nbCCObjet;
00121                 Pixy_CCFrameIsNew = 1;
00122                 Pixy_FirstCCObjet = PIXY_wCCObjet;
00123                 Pixy_NMFrameIsNew = 1;
00124                 Pixy_FirstNMObjet = PIXY_wNMObjet;
00125                 FlagPixy = 1;                                                   // On valide le traitement de la trame précédente.
00126 
00127                 PIXY_nbCCObjet = 0;                                             // On remet à 0 les variables internes
00128                 PIXY_nbNMObjet = 0;
00129                 byteCount = 0;
00130 
00131                 wordBuffer.octet[byteCount%2] = dummy;                          // on stocke les octets reçus
00132                 byteCount++;
00133                 PIXY_state = waitForStart;                                      // On passe en attente de la deuxième partie du code
00134                 break;
00135 
00136             case waitForStart :                                                 // Si on a reçu le code de début d'une nouvelle trame.
00137 
00138                 wordBuffer.octet[byteCount%2] = dummy;                          // on stocke les octets reçus
00139                 byteCount++;
00140                 if (byteCount == 2) {                                           // Quand on a 2 octets
00141                     byteCount = 0;
00142                     if (wordBuffer.mot == NM_BLOCCODE) {                        // On vérifie qu'on a bien reçu l'entête de depart
00143                         PIXY_state = begin;                                     // Si c'est le cas, alors tout va bien et on va à l'aiguillage
00144                     } else {                                                    // Si ce n'est pas le cas
00145                         PIXY_synced = 0;                                        // On n'est plus synchronisés
00146                         PIXY_state = none;                                      // Ceinture et Bretelles
00147                     }
00148                 }
00149                 break;
00150         }
00151     }
00152 }
00153 
00154 int PIXY::detectedObject (int* nbNM, int* nbCC)
00155 {
00156     *nbNM = (int)Pixy_NMObjet;
00157     *nbCC = (int)Pixy_CCObjet;
00158     if (Pixy_check!=0) return -1;
00159     if (FlagPixyOverflow) return -2;
00160     return 0;
00161 }
00162 
00163 T_pixyCCBloc PIXY::getCCBloc (void)
00164 {
00165     T_pixyCCBloc dummy;
00166     static Byte PIXY_rCCObjet = 0;
00167 
00168     if (Pixy_CCObjet !=0) {
00169         if (Pixy_CCFrameIsNew) {                                                //Si on est au début d'une nouvelle trame, on initialise le pointeur de lecture
00170             if (Pixy_FirstCCObjet <= Pixy_CCObjet)  PIXY_rCCObjet = CC_MAXOBJECT - Pixy_CCObjet + Pixy_FirstCCObjet ;
00171             else                                    PIXY_rCCObjet = Pixy_FirstCCObjet - Pixy_CCObjet ;
00172             Pixy_CCFrameIsNew = 0;
00173         }
00174         dummy = Pixy_CCFIFO[PIXY_rCCObjet].CCbloc;
00175         if (PIXY_rCCObjet < (CC_MAXOBJECT-1))   PIXY_rCCObjet++;                // On incrémente le pointeur de lecture dans la FIFO CCObjet
00176         else                                    PIXY_rCCObjet = 0;
00177         Pixy_CCObjet--;                                                         // On dit que l'on a un objet CC de moins à traiter
00178     } else {
00179         dummy.signature = 0xFFFF;
00180     }
00181     return dummy;
00182 }
00183 
00184 T_pixyNMBloc PIXY::getNMBloc (void)
00185 {
00186     T_pixyNMBloc dummy;
00187     static Byte PIXY_rNMObjet = 0;
00188 
00189     if (Pixy_NMObjet !=0) {
00190         if (Pixy_NMFrameIsNew) {                                                //Si on est au début d'une nouvelle trame, on initialise le pointeur de lecture
00191             if (Pixy_FirstNMObjet <= Pixy_NMObjet)  PIXY_rNMObjet = NM_MAXOBJECT - Pixy_NMObjet + Pixy_FirstNMObjet ;
00192             else                                    PIXY_rNMObjet = Pixy_FirstNMObjet - Pixy_NMObjet ;
00193             Pixy_NMFrameIsNew = 0;
00194         }
00195         dummy = Pixy_NMFIFO[PIXY_rNMObjet].NMbloc;
00196         if (PIXY_rNMObjet < (NM_MAXOBJECT-1))   PIXY_rNMObjet++;                // On incrémente le pointeur de lecture dans la FIFO CCObjet
00197         else                                    PIXY_rNMObjet = 0;
00198         Pixy_NMObjet--;                                                         // On dit que l'on a un objet CC de moins à traiter
00199     } else {
00200         dummy.signature = 0xFFFF;
00201     }
00202     return dummy;
00203 }
00204 
00205 void PIXY::setBrightness (Byte brightness)
00206 {
00207     _Pixy->putc(0); //Start
00208     _Pixy->putc(0xFE); //Brightness Level
00209     _Pixy->putc(brightness);
00210 }
00211 
00212 int PIXY::checkNewImage (void)
00213 {
00214     if (FlagPixy) {
00215         FlagPixy = 0;
00216         return 1;
00217     } else {
00218         return 0;
00219     }
00220 }
00221 
00222 int PIXY::checkPixy (void)
00223 {
00224     return Pixy_check;
00225 }