File content as of revision 9:168226ff8f76:

//Variables

Serial      Pixy    (PA_0, PA_1, 230400);
Serial      Pc      (PA_2, PA_3, 921600);

AnalogIn    CNY1    (PC_4);
AnalogIn    CNY2    (PA_7);
AnalogIn    CNY3    (PA_6);
AnalogIn    LD1     (PC_0);
AnalogIn    LD2     (PC_1);
AnalogIn    SD1     (PB_0);
AnalogIn    SD2     (PA_4);
AnalogIn    Vbat    (PC_5);

DigitalOut  Led1    (PA_5);
DigitalOut  Led2    (PD_2);
DigitalOut  Trig1   (PC_3);
DigitalOut  Trig2   (PC_2);
DigitalOut  Trig3   (PA_9);
DigitalOut  En      (PC_9);
DigitalOut  SensG   (PC_8);
DigitalOut  SensD   (PC_6);
DigitalOut  SS      (PA_12);

InterruptIn Echo1   (PA_10);
InterruptIn Echo2   (PA_11);
InterruptIn Echo3   (PB_12);
InterruptIn BP      (PC_13);
InterruptIn IG      (PC_7);
InterruptIn PWMG    (PB_2);
InterruptIn PWMD    (PB_1);

PwmIn       PWMB    (PA_15);

PwmOut      Pwm_MG  (PB_10);
PwmOut      Pwm_MD  (PB_3);
PwmOut      Servo   (PA_8);

I2C         Bus_I2C (PB_9, PB_8);

SPI         SPIG    (PC_12, PC_11, PC_10);
SPI         SPID    (PB_15, PB_14, PB_13);

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      tick;
Timer       times;

/** Liste des variables globales
 *
 *  Tick        -> (long)   Compte le nombre de MS écoulé et déclenche l'exécution de la boucle en fonction du temps écoulé.
 *  FlagIG      -> (int)    Indication de la présence de fronts sur l'index de l'encodeur de la roue gauche
 *  FlagID      -> (int)    Indication de la présence de fronts sur l'index de l'encodeur de la roue droite
 *  EchoXStart  -> (long)   Donne le temps en µs de début de l'impulsion d'écho de l'US n°X
 *  DistUSX     -> (float)  Donne la distance en mm mesurée par l'US n°X
 */

// Structure de temps
lWord               Tick = 0, Gperiod, Dperiod;

// Sémaphore d'interruption
int                 FlagUS1 = 0, FlagUS2 = 0, FlagUS3 = 0, FlagPixy = 0, FlagPixyOverflow = 0;
int                 FlagTick = 0, FlagTickLed = 0, BPPressed = 0, nbTurnG = 0, nbTurnD = 0;
int                 Pixy_check = -1;

//  Dialogue avec la Pixy
T_pixyCCData        Pixy_CCFIFO[20];
T_pixyNMData        Pixy_NMFIFO[20];
Byte                Pixy_CCObjet, Pixy_NMObjet;

// Gestion des capteurs Ultrason
long                Echo1Start, Echo2Start, Echo3Start;
double              DistUS1, DistUS2, DistUS3;

/** Liste des interruptions
 *
 */

void tickTime() {
    Tick++;
    FlagTick = 1;
    if ((Tick%100)==0)  FlagTickLed = 1;
}

void BPevent () {
    BPPressed = 1;
}

void PWM_motGRise (void) {
    static lWord    oldTime;
    lWord           now;
    now = times.read_us();
    Gperiod = now-oldTime;
    oldTime = now;
    nbTurnG++;
}

void PWM_motDRise (void) {
    static lWord    oldTime;
    lWord           now;
    now = times.read_us();
    Dperiod = now-oldTime;
    oldTime = now;
    nbTurnD++;
}

void echo1Rise () {
    Echo1Start = times.read_us();
}

void echo2Rise () {
    Echo2Start = times.read_us();
}

void echo3Rise () {
    Echo3Start = times.read_us();
}

void echo1Fall () {
    DistUS1 = (double)(times.read_us() - Echo1Start)/5.8;
    FlagUS1 = 1;
}

void echo2Fall () {
    DistUS2 = (double)(times.read_us() - Echo2Start)/5.8;
    FlagUS2 = 1;
}

void echo3Fall () {
    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, 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] = 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;
            }
        }
    }

    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_NMFIFO[PIXY_wNMObjet].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_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 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.

                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
                        Pixy_NMObjet = PIXY_nbNMObjet;                          // On met à jour les variables pour le traitement
                        Pixy_CCObjet = PIXY_nbCCObjet;
                        PIXY_nbCCObjet = 0;
                        PIXY_nbNMObjet = 0;
                        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;
        }
    }
}

void ir_read(double SD2,double LD2,double *SD2_dist,double *LD2_dist) {
    double SD2_val, LD2_val;

    SD2_val = SD2;
    LD2_val = LD2;

    if (SD2_val < 0.06) {
        SD2_val = 0;
        *SD2_dist = 40;
    } else {
        *SD2_dist = 11.611/(SD2_val*3.3-0.0237);
        if (*SD2_dist > 40) *SD2_dist = 40;
    }

    if (LD2_val < 0.1) {
        LD2_val = 0;
        *LD2_dist = 150;
    } else {
        *LD2_dist = 59.175/(LD2_val*3.3-0.0275);
        if (*LD2_dist > 150) *LD2_dist = 150;
    }
}

void motor_command(int mg_command, int md_command, int* mg_pwm, int* md_pwm, int* mg_sens, int* md_sens) {
    if(mg_command > 0) {
        *mg_sens = 1;
        *mg_pwm = mg_command;
    } else {
        *mg_sens = -1;
        *mg_pwm = -mg_command;
    }
    if(md_command > 0) {
        *mg_sens = 1;
        *md_pwm = md_command;
    } else {
        *md_sens = -1;
        *md_pwm = md_command;
    }
}