File content as of revision 10:b7c012987c5d:

#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 NM_BLOCCODE :                                      // Si c'est un bloc normal (code = 0xAA55)
                            PIXY_state = normal;                                // On part vers le traitement spécifique
                            break;

                        case CC_BLOCCODE :                                      // 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 < (NM_BLOCSIZE-1)) {                              // 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 < (NM_MAXOBJECT-1))   PIXY_wNMObjet++;            // On incrémente le pointeur d'écriture dans la FIFO Objet
                        else                                    PIXY_wNMObjet = 0;
                        if (PIXY_nbNMObjet < (NM_MAXOBJECT-1))  PIXY_nbNMObjet++;           // On dit que l'on a un objet de plus
                        else                                    FlagPixyOverflow = 1;       // Si on a plus de NM_MAXOBJECT 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 < (CC_BLOCSIZE-1)) 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 < (CC_MAXOBJECT-1))   PIXY_wCCObjet++;            // On incrémente le pointeur d'écriture dans la FIFO CCObjet
                        else                                    PIXY_wCCObjet = 0;
                        if (PIXY_nbCCObjet < (CC_MAXOBJECT-1))  PIXY_nbCCObjet++;           // On dit que l'on a un objet CC de plus à traiter
                        else                                    FlagPixyOverflow = 1;       // Si on a plus de CC_MAXOBJECT 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 == NM_BLOCCODE) {                        // 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 = CC_MAXOBJECT - Pixy_CCObjet + Pixy_FirstCCObjet ;
            else                                    PIXY_rCCObjet = Pixy_FirstCCObjet - Pixy_CCObjet ;
            Pixy_CCFrameIsNew = 0;
        }
        dummy = Pixy_CCFIFO[PIXY_rCCObjet].CCbloc;
        if (PIXY_rCCObjet < (CC_MAXOBJECT-1))   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 = NM_MAXOBJECT - Pixy_NMObjet + Pixy_FirstNMObjet ;
            else                                    PIXY_rNMObjet = Pixy_FirstNMObjet - Pixy_NMObjet ;
            Pixy_NMFrameIsNew = 0;
        }
        dummy = Pixy_NMFIFO[PIXY_rNMObjet].NMbloc;
        if (PIXY_rNMObjet < (NM_MAXOBJECT-1))   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;
}