File content as of revision 1:d95546f84105:

/** 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

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

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

typedef union {
    lWord   header;
    Byte    tab[4];
} T_tmpbuffer;

typedef struct {
    Word    checksum;
    Word    signature;
    Word    x;
    Word    y;
    Word    width;
    Word    height;
    Word    angle; // angle is only available for color coded blocks
} T_pixyCCBloc;


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


/** 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, 460800);

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);

PwmIn       PWMG    (PB_2);
PwmIn       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         MotG    (PC_12, PC_11, PC_10);
SPI         MotD    (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      timer;
Timer       temps;

/** 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;

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

//  Dialogue avec la Pixy
T_pixyData  Pixy_FIFO[20];
Byte        Pixy_nbObjet = 0, Pixy_wObjet = 0, Pixy_rObjet = 0;

// 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 echo1Rise ()
{
    Echo1Start = temps.read_us();
}

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

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

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

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

void echo3Fall ()
{
    DistUS3 = (double)(temps.read_us() - Echo3Start)/5.8;
    FlagUS3 = 1;
}

void getPixyByte ()
{

    static T_tmpbuffer  tmpBuffer;
    static Byte         bytecount = 0;
    static int          Pixy_synced = 0;
    int i;

    Led2 = ! Led2;
    if (!Pixy_synced) {                                                     // On n'a pas trouvé le départ (0x55aa55aa ou 0x55aa56aa)
        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 {                                                            // Si on a 4 octets de données
            if (tmpBuffer.header == 0x55aa56aa) {                           // Si on a un Color Code Bloc
                if (Pixy_wObjet < 19)   Pixy_wObjet++;                      // On incrémente le pointeur d'écriture dans la FIFO Objet
                else                    Pixy_wObjet = 0;
                Pixy_nbObjet++;                                             // On dit que l'on a un objet de plus
                if (Pixy_nbObjet >= 20) FlagPixyOverflow = 1;               // Si on a plus de 20 objets (en attente) => Overflow
                Pixy_synced = 1;                                            // Et on peut dire que l'on a synchronisé la Pixy
                bytecount = 0;
            } else {                                                        // Si on n'a pas vu le header d'un Color Code Bloc
                for (i=1; i<4; i++) tmpBuffer.tab[i-1] = tmpBuffer.tab[i];  // On décalle les cases du tableau
            }
        }
    } else {                                                                // La Pixy est synchronisée
        Pixy_FIFO[Pixy_wObjet].tab[bytecount] = Pixy.getc();                // On stocke les octets un à un dans la structure CCBloc
        if (bytecount < 14) bytecount++;                                    // tant que la structure n'est pas pleine
        else {                                                              // Quand elle est pleine
            bytecount = 0;                                                  // On réinitialise
            Pixy_synced = 0;
            FlagPixy = 1;                                                   // Et on signale au main qu'un objet est pret à être analysé.
        }
    }
}

int main()
{

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

    double      SERVO_angle = 0, SERVO_angleMAX = 180, SERVO_angleMIN = 0;
    Word        SERVO_pos;
    T_SERVODIR  SERVO_dir = S_monte;

    Byte        PIXY_red = 0, PIXY_green = 0, PIXY_blue = 0;
    char        MENU_choix = 0;

    char        BOUSSOLE_status[1] = {0}/*, BOUSSOLE_bearingWord[1] = {2}*/;
    char        I2C_registerValue[4];

    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;
    double      periode;

    temps.reset();
    temps.start();


    timer.attach(&tickTime, 0.001);

    Bus_I2C.frequency (100000);

    BP.rise (&BPevent);

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

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

    Pixy.attach (&getPixyByte);

    Pwm_MG.period_us(50);
    Pwm_MD.period_us(50);
    Pwm_MG = 0;
    Pwm_MD = 0;
    En = 0;
    SensG = 0;
    SensD = 0;
    Led2 = 0;

    Servo.period_ms(20);

    while(1) {

        do {
            Led1 = 0;
            Pc.printf ("\n\n\n\n\rProgramme de test\n\n\rEntrez le code du test a effectuer :\n\n");
            Pc.printf ("\r1- Capteurs Ultra Son (les 3)\n");
            Pc.printf ("\r2- Boussole et I2C\n");
            Pc.printf ("\r3- Capteurs GP2 (les 4)\n");
            Pc.printf ("\r4- Capteurs CNY70 (les 3)\n");
            Pc.printf ("\r5- VBAT\n");
            Pc.printf ("\r6- Moteur Gauche -- NON CODE\n");
            Pc.printf ("\r7- Moteur Droit -- NON CODE\n");
            Pc.printf ("\r8- Servomoteur\n");
            Pc.printf ("\r9- Test de la PIXY\n\r");
            MENU_choix = Pc.getc ();
        } while (((MENU_choix-'0')<1) || ((MENU_choix-'0')>9));

        switch (MENU_choix-'0') {

            case 1 :
                Pc.printf ("\n\n\rTest des captreurs Ultrason\n");
                Pc.printf ("\rAppuyez sur Entree pour quitter\n");
                do {

                    if (FlagTickLed) {
                        Led1 = !Led1;
                        FlagTickLed = 0;
                    }

                    // Gestion des US
                    if (((Tick%150)==0) && FlagTick) {
                        Trig1 = 1;
                        wait_us(20);
                        Trig1 = 0;
                        FlagTick = 0;
                    }

                    if (((Tick%150)==50) && FlagTick) {
                        Trig2 = 1;
                        wait_us(20);
                        Trig2 = 0;
                        FlagTick = 0;
                    }

                    if (((Tick%150)==100) && FlagTick) {
                        Trig3 = 1;
                        wait_us(20);
                        Trig3 = 0;
                        FlagTick = 0;
                    }

                    if (FlagUS1==1) {
                        Pc.printf ("\rUS 1 = %04d mm", (int)DistUS1);
                        FlagUS1 = 0;
                    }

                    if (FlagUS2==1) {
                        Pc.printf ("\r\t\t\tUS 2 = %04d mm", (int)DistUS2);
                        FlagUS2 = 0;
                    }

                    if (FlagUS3==1) {
                        Pc.printf ("\r\t\t\t\t\t\tUS 3 = %04d mm", (int)DistUS3);
                        FlagUS3 = 0;
                    }
                } while(!Pc.readable());
                MENU_choix = Pc.getc();
                break;

            case 2 :
                Pc.printf ("\n\n\rBoussole\n");
                Pc.printf ("\rAppuyez sur Entree pour quitter\n");

                Pc.printf ("\n\rVerif du bus I2C :");
                I2C_check = Bus_I2C.write (BOUSSOLE_adress,BOUSSOLE_status,1,false);
                if (I2C_check==0) {
                    Pc.printf (" OK\n");
                    Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true);
                    Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4);
                    Pc.printf ("\rVersion Firmware boussole : %03d\n", I2C_registerValue[0]);
                } else {
                    Pc.printf (" FAIL\n");
                }

                periode = PWMB.period();
                Pc.printf ("\rVerif de la PWM :");
                if ((periode > 0.11) || (periode < 0.06)) {
                    Pc.printf (" FAIL\n\n");
                } else {
                    Pc.printf (" OK\n\n");
                    BOUSSOLE_check = 0;
                }

                do {
                    if (FlagTickLed) {
                        Led1 = !Led1;
                        FlagTickLed = 0;
                    }

                    if (((Tick%150)==0) && FlagTick) {
                        FlagTick = 0;
                        if (BOUSSOLE_check==0) {
                            CAP_PWM = ((PWMB.pulsewidth()*1000)-1)*10;
                            Pc.printf ("\r PWM = %4.1lf", CAP_PWM);
                        }
                        //if (I2C_check==0) {
                            Bus_I2C.write(BOUSSOLE_adress,BOUSSOLE_status, 1, true);
                            Bus_I2C.read (BOUSSOLE_adress,I2C_registerValue,4);
                            CAP_I2C = (double)(((unsigned short)I2C_registerValue[2]<<8)+(unsigned short)I2C_registerValue[3])/10.0;
                            Pc.printf ("\r\t\t I2C = %4.1lf", CAP_I2C);
                        //}
                    }
                } while(!Pc.readable());
                MENU_choix = Pc.getc();
                break;

            case 3 :
                Pc.printf ("\n\n\rGP2xx\n");
                Pc.printf ("\rAppuyez sur Entree pour quitter\n");
                do {
                    if (FlagTickLed) {
                        Led1 = !Led1;
                        FlagTickLed = 0;

                        SD1_val = SD1;
                        SD2_val = SD2;
                        LD1_val = LD1;
                        LD2_val = LD2;

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

                        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 (LD1_val < 0.1) {
                            LD1_val = 0;
                            LD1_dist = 150;
                        } else {
                            LD1_dist = 59.175/(LD1_val*3.3-0.0275);
                            if (LD1_dist > 150) LD1_dist = 150;
                        }

                        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;
                        }

                        Pc.printf ("\r SD1 = %3.1lf cm - SD2 = %3.1lf cm - LD1 = %4.1lf cm - LD2 = %4.1lf cm", SD1_dist, SD2_dist, LD1_dist, LD2_dist);
                    }
                } while(!Pc.readable());
                MENU_choix = Pc.getc();
                break;

            case 4 :
                Pc.printf ("\n\n\rCNY70\n");
                Pc.printf ("\rAppuyez sur Entree pour quitter\n");
                do {
                    if (FlagTickLed) {
                        Led1 = !Led1;
                        FlagTickLed = 0;

                        CNY1_val = CNY1;
                        CNY2_val = CNY2;
                        CNY3_val = CNY3;

                        Pc.printf ("\r CNY1 = %3.2lf V\t CNY2 = %3.2lf V\t CNY3 = %3.2lf V", CNY1_val*3.3, CNY2_val*3.3, CNY3_val*3.3);
                    }
                } while(!Pc.readable());
                MENU_choix = Pc.getc();
                break;

            case 5 :
                Pc.printf ("\n\n\rVbat\n");
                Pc.printf ("\rAppuyez sur Entree pour quitter\n");
                do {
                    if (FlagTickLed) {
                        Led1 = !Led1;
                        FlagTickLed = 0;

                        Vbat_val = Vbat;
                        Pc.printf ("\rVbat = %5.3lf V", Vbat_val*3.3*4.3);
                    }
                } while(!Pc.readable());
                MENU_choix = Pc.getc();
                break;

            case 6 :
                Pc.printf ("\n\n\rMoteur Gauche\n");
                Pc.printf ("\rAppuyez sur Entree pour quitter\n");
                do {
                    if (FlagTickLed) {
                        Led1 = !Led1;
                        FlagTickLed = 0;
                    }
                    
                    

                } while(!Pc.readable());
                MENU_choix = Pc.getc();
                break;

            case 7 :
                Pc.printf ("\n\n\rMoteur Droit\n");
                Pc.printf ("\rAppuyez sur Entree pour quitter\n");
                do {
                    if (FlagTickLed) {
                        Led1 = !Led1;
                        FlagTickLed = 0;
                        wait (0.1);
                    }
                } while(!Pc.readable());
                MENU_choix = Pc.getc();
                break;

            case 8 :
                Pc.printf ("\n\n\rServo Moteur\n");
                Pc.printf ("\rAppuyez sur Entree pour quitter\n");
                do {
                    if (FlagTickLed) {
                        Led1 = !Led1;
                        FlagTickLed = 0;
                    }

                    if (((Tick%250)==0) && FlagTick) {
                        if (SERVO_dir == S_monte) {
                            if (SERVO_angle >= SERVO_angleMAX) {
                                SERVO_dir = S_descente;
                                SERVO_angle = SERVO_angleMAX;
                            } else {
                                SERVO_angle += 5;
                            }
                        } else {
                            if (SERVO_angle <= SERVO_angleMIN) {
                                SERVO_dir = S_monte;
                                SERVO_angle = SERVO_angleMIN;
                            } else {
                                SERVO_angle -= 5;
                            }
                        }
                        SERVO_pos = (lWord)(SERVO_angle*50/9) + 1000;
                        Servo.pulsewidth_us(SERVO_pos);
                        Pc.printf ("\rAngle = %4.1lf", SERVO_angle);
                    }
                } while(!Pc.readable());
                MENU_choix = Pc.getc();
                break;

            case 9 :
                Pc.printf ("\n\n\rPixy\n");
                Pc.printf ("\rAppuyez sur Entree pour quitter\n");
                do {
                    if (((Tick%50)==0) && FlagTick) {
                        FlagTick = 0;
                        switch (phase) {
                            case 0 :
                                PIXY_red += 5;
                                if (PIXY_red == 255) {
                                    phase = 1;
                                    //Pc.printf ("\rPhase  1");
                                }
                                break;
                            case 1 :
                                PIXY_green += 5;
                                if (PIXY_green == 255) {
                                    phase = 2;
                                    //Pc.printf ("\rPhase  2");
                                }
                                break;
                            case 2 :
                                PIXY_red -= 5;
                                if (PIXY_red == 0)  {
                                    phase = 3;
                                    //Pc.printf ("\rPhase  3");
                                }
                                break;
                            case 3 :
                                PIXY_blue += 5;
                                if (PIXY_blue == 255)  {
                                    phase = 4;
                                    //Pc.printf ("\rPhase  4");
                                }
                                break;
                            case 4 :
                                PIXY_green -= 5;
                                if (PIXY_green == 0)  {
                                    phase = 5;
                                    //Pc.printf ("\rPhase  5");
                                }
                                break;
                            case 5 :
                                PIXY_red += 5;
                                if (PIXY_red == 255)  {
                                    phase = 6;
                                    //Pc.printf ("\rPhase  6");
                                }
                                break;
                            case 6 :
                                PIXY_green += 5;
                                if (PIXY_green == 255)  {
                                    phase = 7;
                                    //Pc.printf ("\rPhase  7");
                                }
                                break;
                            case 7 :
                                PIXY_red -= 5;
                                PIXY_green -= 5;
                                PIXY_blue -=5;
                                if (PIXY_red == 0)  {
                                    phase = 0;
                                    //Pc.printf ("\rPhase  0");
                                }
                                break;
                        }
                        Pixy.putc(0);
                        Pixy.putc(0xFD);
                        Pixy.putc(PIXY_red);
                        Pixy.putc(PIXY_green);
                        Pixy.putc(PIXY_blue);

                        while (Pixy_nbObjet) {
                            Pc.printf ("\r%5hd = %5hd,%5hd : %5hdx%5hd", Pixy_FIFO[Pixy_rObjet].CCbloc.signature, Pixy_FIFO[Pixy_rObjet].CCbloc.x, Pixy_FIFO[Pixy_rObjet].CCbloc.y, Pixy_FIFO[Pixy_rObjet].CCbloc.width, Pixy_FIFO[Pixy_rObjet].CCbloc.height);
                            
                            if (Pixy_nbObjet>1) Pc.printf("\n");
                        
                            if (Pixy_rObjet<19) Pixy_rObjet++;
                            else Pixy_rObjet = 0;

                            Pixy_nbObjet--;
                        }                                
                        FlagPixy = 0;
                    }

                    if (FlagTickLed) {
                        Led1 = !Led1;
                        FlagTickLed = 0;
                    }
                } while(!Pc.readable());
                MENU_choix = Pc.getc();
                break;

        }
    }
}