File content as of revision 10:4e1c8b3edee7:

/** Main Test Board
 *
 *   \brief Programme de tests pour le robot NCR 2017
 *   \author H. Angelis
 *   \version alpha_1
 *   \date 15/05/17
 *
 */

#include "mbed.h"
#include "PwmIn.h"
#include "Nucleo_Encoder_16_bits.h"

#define BOUSSOLE_adress     0xC0
#define PIXY_adress         0x54

#define CC_BLOCSIZE         14
#define N_BLOCSIZE          12

#define N_BLOCCODE          0xAA55
#define CC_BLOCCODE         0xAA56


// Comme la nucleo est Little Endian et que l'AS5047D est Big Endian, les codes sont inversés
//  Registre        Valeur (BE) Valeur (LE)
//  NOP             0xC000      0x0003
//  ANGLE           0xFFFF      0xFFFF
//  ERROR           0x4001      0x8002
//  SETTING1 (R)    0xC018      0x1803
//  SETTING2 (R)    0x4019      0x8902
//  SETTING1 (W)    0x0018      0x1800
//  SETTING2 (W)    0x8019      0x9801

#define SPI_READ_NOP        0x0003
#define SPI_READ_ANGLE      0xFFFF
#define SPI_READ_ERROR      0x8002
#define SPI_READ_SETTINGS1  0x1803
#define SPI_READ_SETTINGS2  0x8902
#define SPI_WRTIE_SETTINGS1 0x1800
#define SPI_WRITE_SETTINGS2 0x9801

typedef unsigned char   Byte;
typedef unsigned short  Word;
typedef unsigned long   lWord;

typedef enum {S_monte = 1, S_descente = 0} T_SERVODIR;

typedef enum {none, begin, normal, colorCode, doubleZero} T_pixyState;

typedef union {
    lWord   mot;
    Byte    tab[4];
} T_tmpBuffer;

typedef union {
    Word    mot;
    Byte    tab[2];
} T_structBuffer;

typedef struct {
    Word    checksum;
    Word    signature;
    Word    x;
    Word    y;
    Word    width;
    Word    height;
    Word    angle;
} T_pixyCCBloc;

typedef struct {
    Word    checksum;
    Word    signature;
    Word    x;
    Word    y;
    Word    width;
    Word    height;
} T_pixyNMBloc;

typedef union {
    Byte            tab[14];
    T_pixyCCBloc    CCbloc;
} T_pixyCCData;

typedef union {
    Byte            tab[12];
    T_pixyNMBloc    NMbloc;
} T_pixyNMData;



/** Liste des objets
 *
 *  Serial #4   Pixy
 *  Serial #2   Pc
 *
 *  AnalogIn    C1, C2, C3, LD1, LD2, SD1, SD2, Vbat
 *
 *  DigitalOut  Led1, Led2, Trig1, Trig2, Trig3, En, SensG, SensD
 *
 *  InterruptIn IndexG, IndexD, Echo1, Echo2, Echo3, BP
 *
 *  PwmOut      Pwm_MG, Pwm_MD, Servo
 *
 *  PwmIn       PWMG, PWMD, PWMB
 *
 *  I2C         Bus_I2C
 *
 *  SPI         MotG, MotD
 *
 *  Nucleo_Encoder_16_bits  Gauche, Droite
 *
 *  Ticker      timer
 */

/** Liste des PINs
 *
 * PIN MAP (ordre alphabetique) des PINs de la Nucléo 64 utilisée
 * PA_0    -> Pixy RX (Serial)
 * PA_1    -> Pixy TX (Serial)
 * PA_2    -> PC TX (Serial)
 * PA_3    -> PX RX (Serial)
 * PA_4    -> GP2 SD #2 (Analog In)
 * PA_5    -> LED1 (Digital Out)
 * PA_6    -> CNY3 (Analog In)
 * PA_7    -> CNY2 (Analog In)
 * PA_8    -> Servomoteur (PWM Out)
 * PA_9    -> US Trigger #3 (Digital Out)
 * PA_10   -> US Echo #1 (Pwm In)
 * PA_11   -> US Echo #2 (Pwm In)
 * PA_12   -> SS (SPI Slave Select) (Digital Out)
 * PA_13
 * PA_14
 * PA_15   -> Boussole (Pwm In)
 *
 * PB_0    -> GP2 SD #1 (Analog In)
 * PB_1    -> Position D (Pwm In)
 * PB_2    -> Position G (Pwm In)
 * PB_3    -> PWM Mot D (PWM Out)
 * PB_4    -> Enocdeur Droit A (QE)
 * PB_5    -> Enocdeur Droit B (QE)
 * PB_6    -> Enocdeur Gauche A (QE)
 * PB_7    -> Enocdeur Gauche B (QE)
 * PB_8    -> SCL (I2C)
 * PB_9    -> SDA (I2C)
 * PB_10   -> PWM Mot G (PWM Out)
 * PB_11
 * PB_12   -> US Echo #3 (Pwm In)
 * PB_13   -> SCK Encodeur D (SPI)
 * PB_14   -> MISO Encodeur D (SPI)
 * PB_15   -> MOSI Encodeur D (SPI)
 *
 * PC_0    -> GP2 LD #1 (Analog In)
 * PC_1    -> GP2 LD #2 (Analog In)
 * PC_2    -> US Trigger #2 (Digital Out)
 * PC_3    -> US Trigger #1 (Digital Out)
 * PC_4    -> CNY1 (Analog In)
 * PC_5    -> Vbat (Analog In)
 * PC_6    -> Dir Mot Droit (Digital Out)
 * PC_7    -> I (Encodeur Gauche) (IRQ In)
 * PC_8    -> Dir Mot Gauche (Digital Out)
 * PC_9    -> Enable Moteurs (Digital Out)
 * PC_10   -> SCK Encodeur G (SPI)
 * PC_11   -> MISO Encodeur G (SPI)
 * PC_12   -> MOSI Encodeur G (SPI)
 * PC_13   -> User BP (IRQ In)
 * PC_14
 * PC_15
 *
 * PD_1
 * PD_2    -> Led2 (Digital Out)
 */

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 motor_command(int mg_command, int md_command, float* mg_pwm, float* md_pwm, int* mg_sens, int* md_sens) {
    if(mg_command >= 0) {
        *mg_sens = 1;
        *mg_pwm = 1-((float)(mg_command%101)/100);
    } else {
        *mg_sens = 0;
        *mg_pwm = ((float)(-mg_command%101)/100);
    }
    if(md_command >= 0) {
        *md_sens = 0;
        *md_pwm = ((float)(md_command%101)/100);
    } else {
        *md_sens = 1;
        *md_pwm = 1-((float)(-md_command%101)/100);
    }
}

int main()
{

    int         I2C_check = -1, BOUSSOLE_check = -1 /*, SPI2_check = -1, SPI3_check = -1, MOTG_check = -1, MOTD_check = -1*/;

    Byte        PIXY_rCCObjet = 0, PIXY_rNMObjet = 0;
    int         PIXY_objet;

    int         SERVO_pulseWidth = 400, SERVO_max = 1400, SERVO_min = 400;
    T_SERVODIR  SERVO_dir = S_monte;

    char        MENU_choix = 0;

    char        BOUSSOLE_status[1] = {0};
    char        I2C_registerValue[4];
    double      BOUSSOLE_periode;

    double      CAP_I2C, CAP_PWM;
    double      SD1_val, SD2_val, LD1_val, LD2_val, CNY1_val, CNY2_val, CNY3_val, Vbat_val;
    double      SD1_dist, SD2_dist, LD1_dist, LD2_dist;

    float md_pwm = 0;
    float mg_pwm = 0;
    int md_sens = 1;
    int mg_sens = 1;
    
    int i = 0;
    float val_carre_balle=0, val_x_balle=0;
    
    float erreur_asservissement, commande_moteur = 30, mesure_angle_pixy;

    times.reset();
    times.start();

    // Initialisation des interruptions
    tick.attach(&tickTime, 0.001);

    BP.rise     (&BPevent);

    Echo1.rise  (&echo1Rise);
    Echo2.rise  (&echo2Rise);
    Echo3.rise  (&echo3Rise);
    Echo1.fall  (&echo1Fall);
    Echo2.fall  (&echo2Fall);
    Echo3.fall  (&echo3Fall);

    PWMG.rise   (&PWM_motGRise);
    PWMD.rise   (&PWM_motDRise);

    Pixy.attach (&getPixyByte);

    BP.enable_irq();
    IG.enable_irq();
    Echo1.enable_irq();
    Echo2.enable_irq();
    Echo3.enable_irq();
    PWMG.enable_irq();
    PWMD.enable_irq();

    // Initialisation des périphériques
    // Bus I2C
    Bus_I2C.frequency (100000);

    // PWM des moteurs
    Pwm_MG.period_us(50);
    Pwm_MD.period_us(50);
    En = 0;

    // Bus SPI
    SPIG.format (16,1);
    SPIG.frequency (1000000);

    SPID.format (16,1);
    SPID.frequency (1000000);

    SS = 1;

    // Led
    Led2 = 0;

    En = 1;

    Servo.period_ms (20);
    Servo.pulsewidth_us(200);    
    
    Pc.printf("test1\n\r");
    
    motor_command(60, -60, &mg_pwm, &md_pwm, &mg_sens, &md_sens);
    Pwm_MD = md_pwm;
    Pwm_MG = mg_pwm;
    SensD = md_sens;
    SensG = mg_sens;
    wait_ms(3000);
    
    Pc.printf("Pwm_MG = %lf, Pwm_MD = %lf, SensG = %d, SensD = %d\r\n", mg_pwm, md_pwm, mg_sens, md_sens);

    Pwm_MD = 0;
    Pwm_MG = 0;
    SensD = 0;
    SensG = 0;
    
    while(1) {
                /*
                val_x_balle = 0;
                i = 0;
                while ((val_x_balle==0) && (i<19)) {
                    //test carre +/-5%
                    val_carre_balle = (float)Pixy_NMFIFO[i].NMbloc.height/(float)Pixy_NMFIFO[i].NMbloc.width;
                    if ((0.85<val_carre_balle) && (val_carre_balle<1.15)) val_x_balle =Pixy_NMFIFO[i].NMbloc.x+Pixy_NMFIFO[i].NMbloc.width/2;
                    i++;    
                }
                mesure_angle_pixy=(val_x_balle-160)*(37.5/160);
                Pc.printf("%lf %lf\r\n", val_x_balle, mesure_angle_pixy);
                wait_us(50000);
                erreur_asservissement = 2 * (-mesure_angle_pixy);
                motor_command(commande_moteur+erreur_asservissement, commande_moteur-erreur_asservissement, &mg_pwm, &md_pwm, &mg_sens, &md_sens);
                mesure_angle_pixy;
                Pwm_MD = md_pwm;
                Pwm_MG = mg_pwm;
                SensD = md_sens;
                SensG = mg_sens;
                */
    }
}