File content as of revision 22:82611fe41c5c:

/*
 *   MJBot : version 2019
 *   
 */
 
/* Includes ------------------------------------------------------------------*/

#include "mbed.h"
#include "DevSPI.h"
#include "x_nucleo_ihm02a1_class.h"
#undef printf
#include "Robot.h"

Serial bt_uart(PA_9, PA_10);  // PA9 = Tx, PA10 = Rx
Serial pc_uart(USBTX, USBRX); // USBTX = PA2, USBRX = PA3

/* Definitions ---------------------------------------------------------------*/

/* Number of movements per revolution. */
#define MPR_1 4

/* Number of steps. */
#define STEPS_1 (40000 * 128)   /* 1 revolution given a 400 steps motor configured at 1/128 microstep mode. */
#define STEPS_2 (STEPS_1 * 2)

/* Delay in milliseconds. */
#define DELAY_1 1000
#define DELAY_2 2000
#define DELAY_3 5000

#define MID         1500
#define MIN         800   // 750
#define MAX         2200  // 2250

bool receivedCOMMAND;
char commandRECEIVED;
int parameterRECEIVED;
int state;
char commandLine[256];
int commandPosition;
int action;
int pince;
int repos;
int inter;
int attente;
int trigger;
int Bot; // MJBot1 = 0, MJBot2 = 1 and STBot = 2

LED_WS2812 ledBand(PB_10,4);
PwmOut myservo(PB_4);
DigitalOut ventilo(PB_0);

// Buzzer buzzer(PB_0); 

/* Variables -----------------------------------------------------------------*/

/* Motor Control Expansion Board. */
X_NUCLEO_IHM02A1 *x_nucleo_ihm02a1;

/* Initialization parameters of the motors connected to the expansion board. */
L6470_Init_t init[L6470DAISYCHAINSIZE] =
{
    /* First Motor. */    
    {
        12.0,                           /* Motor supply voltage in V. */
        200,                           /* Min number of steps per revolution for the motor. */
        0.9,                           /* Max motor phase voltage in A. */
        3.0,                          /* Max motor phase voltage in V. */
        50.0,                         /* Motor initial speed [step/s]. */
        100.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
        100.0,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
        200.0,                         /* Motor maximum speed [step/s]. */
        0.0,                           /* Motor minimum speed [step/s]. */
        490.0,                         /* Motor full-step speed threshold [step/s]. */
        3.06,                          /* Holding kval [V]. */
        3.06,                          /* Constant speed kval [V]. */
        3.06,                          /* Acceleration starting kval [V]. */
        3.06,                          /* Deceleration starting kval [V]. */
        61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
        392.1569e-6,                   /* Start slope [s/step]. */
        643.1372e-6,                   /* Acceleration final slope [s/step]. */
        643.1372e-6,                   /* Deceleration final slope [s/step]. */
        0,                             /* Thermal compensation factor (range [0, 15]). */
        3.06 * 1000 * 1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
        3.06 * 1000 * 1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
        StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
        0x00,                          /* Alarm conditions enable. */
        0x2E88                         /* Ic configuration. */
    },

    /* Second Motor. */
    {
        12,                           /* Motor supply voltage in V. */
        200,                           /* Min number of steps per revolution for the motor. */
        0.9,                           /* Max motor phase voltage in A. */
        3.0,                          /* Max motor phase voltage in V. */
        50.0,                         /* Motor initial speed [step/s]. */
        100.0,                         /* Motor acceleration [step/s^2] (comment for infinite acceleration mode). */
        100.0,                         /* Motor deceleration [step/s^2] (comment for infinite deceleration mode). */
        200.0,                         /* Motor maximum speed [step/s]. */
        0.0,                           /* Motor minimum speed [step/s]. */
        490.0,                         /* Motor full-step speed threshold [step/s]. */
        3.06,                          /* Holding kval [V]. */
        3.06,                          /* Constant speed kval [V]. */
        3.06,                          /* Acceleration starting kval [V]. */
        3.06,                          /* Deceleration starting kval [V]. */
        61.52,                         /* Intersect speed for bemf compensation curve slope changing [step/s]. */
        392.1569e-6,                   /* Start slope [s/step]. */
        643.1372e-6,                   /* Acceleration final slope [s/step]. */
        643.1372e-6,                   /* Deceleration final slope [s/step]. */
        0,                             /* Thermal compensation factor (range [0, 15]). */
        3.06 * 1000 * 1.10,            /* Ocd threshold [ma] (range [375 ma, 6000 ma]). */
        3.06 * 1000 * 1.00,            /* Stall threshold [ma] (range [31.25 ma, 4000 ma]). */
        StepperMotor::STEP_MODE_1_128, /* Step mode selection. */
        0x00,                          /* Alarm conditions enable. */
        0x2E88                         /* Ic configuration. */
    }
};

void help() // Display list of Commands
{
    DEBUG("List of commands:\n\r");
    DEBUG(" h --> Help, display list of commands\n\r");
}

void executeCommand(char c) {
        bool flaghelp = false;
        switch (c) {
            case 'h':
                help();
                action=0;
                flaghelp=true;
                CASE('a', "Avance", action=2; )
                CASE('r', "Recule", action=3; )
                CASE('d', "Droite AV", action=4; )
                CASE('g', "Gauche AV", action=5; )
                CASE('p', "Pivote D", action=6; )
                CASE('q', "Pivote G", action=7; )
                CASE('s', "Stop",   action=1; )
                CASE('n', "Debrayer", action=9; )
                CASE('k', "Pince haute", action=10; )
                CASE('j', "Pince basse", action=11; )
                CASE('b', "Ventilo On", action=12; )
                CASE('l', "Ventilo Off", action=13; )
                CASE('c', "LEDs ON", action=14; )
                CASE('x', "LEDs OFF", action=15; )
                CASE('y', "Pince haute/basse", action=16; )
                CASE('D', "Droite AR", action=17; )
                CASE('G', "Gauche AR", action=18; )
                CASE('t', "Ventilo ON/OFF", action=19; )
                          
            default :
                DEBUG("invalid command; use: 'h' for help()\n\r");
                action=0;
        }}

void analyseCommand(char *command) {
    switch(command[0]) {
        case 'a':
            commandRECEIVED = 'a';
            break;
        case 'r':
            commandRECEIVED = 'r';
             break;
        case 'd':
            commandRECEIVED = 'd';
             break;
        case 'g':
            commandRECEIVED = 'g';
             break;
        case 'D':
            commandRECEIVED = 'D';
             break;
        case 'G':
            commandRECEIVED = 'G';
             break;
        case 's':
            commandRECEIVED = 's';
             break;
        case 'p':
            commandRECEIVED = 'p';
             break;
        case 'q':
            commandRECEIVED = 'q';
             break;
        case 'n':
            commandRECEIVED = 'n';
             break;
        case 'k':
            commandRECEIVED = 'k';
             break;
        case 'j':
            commandRECEIVED = 'j';
             break;
        case 'b':
            commandRECEIVED = 'b';
             break;
        case 'l':
            commandRECEIVED = 'l';
             break;
        case 'c':
            commandRECEIVED = 'c';
             break;
        case 'x':
            commandRECEIVED = 'x';
             break;
        case 'y':
            commandRECEIVED = 'y';
             break;
        case 't':
            commandRECEIVED = 't';
             break;

       default:
            commandRECEIVED = 'h';
    } }

void checkCommand(int result, char *command) {
    if(result==1) {
      analyseCommand(command);        
//    DEBUG("ANALYZED COMMAND %c %d state=%d\n\r",commandRECEIVED, parameterRECEIVED,state);
      receivedCOMMAND = true;
    } }

void split(char *line, int length) {
    char command[256];
    int parameter=0;
    int result = 1;
    int i=0;
    int j=0;
    while(i<length && line[i]==' ') {
        i++;}
    while(i<length && line[i]!=' ') {
         command[j]=line[i];
         i++;
         j++;}
    command[j]=0;
    i++;
    j=0;
     while(i<length && line[i]!=' ' && line[i]>='0' && line[i]<='9') {
       i++;
       j++;}
    if(j>0) {
        result++;
           i--;
        int k=1;
        while(j>0) {
           parameter += (line[i]-'0')*k;
            j--;
           i--;
           k=k*10;}
     }
  checkCommand(result, command);
}

void storeC(char c) {
    if(c==10 || c==13|| commandPosition >= 255) {
       split(commandLine, commandPosition);   
       commandPosition=0;}
    else {
      commandLine[commandPosition++]=c;
      commandLine[commandPosition]=0;}   
}

void getBT() {
    char c = bt_uart.getc();
    storeC(c);
} 

void getPC() {
    char c = pc_uart.getc();
    storeC(c);
}

void leda(int parameter) {
    ledBand.SetColor(WHITE,0) ;
    ledBand.SetColor(RED,1) ;
    ledBand.SetColor(BLUE,2) ;
    ledBand.SetColor(GREEN,3) ;
    if (parameter == 0) {
        ledBand.StopRotation();
//        ledBand.StopBlink() ;
        ledBand.SetColor(BLACK,0) ;
        ledBand.SetColor(BLACK,1) ;
        ledBand.SetColor(BLACK,2) ;
        ledBand.SetColor(BLACK,3) ;}
    if (parameter == 1) {
        ledBand.SetColor(RED,0) ;
        ledBand.SetColor(RED,1) ;
        ledBand.SetColor(RED,2) ;
        ledBand.SetColor(RED,3);}
    if (parameter == 2) {ledBand.StartRotation(0.6);}
    if (parameter == 3) {ledBand.StartRotation(1);}
    state=0;
}

/* Main ----------------------------------------------------------------------*/

int main()
{
     /*----- Initialization. -----*/
    wait(1); 
//    bt_uart.printf("AT+NAMEPETITBULLES"); // Changement nom module BT 
//    wait(2);  

    /* Initializing SPI bus. */
    DevSPI dev_spi(D11, D12, D3);
    /* Initializing Motor Control Expansion Board. */
    x_nucleo_ihm02a1 = new X_NUCLEO_IHM02A1(&init[0], &init[1], A4, A5, D4, A2, &dev_spi);
    /* Building a list of motor control components. */
    L6470 **motors = x_nucleo_ihm02a1->GetComponents();
    
    pc_uart.printf("MJBot2 \r\n\n");  // Printing to the console
    commandPosition=0;
    bt_uart.attach(getBT);
    pc_uart.attach(getPC);
    state=0;
    action=0;
    pince=0;
    inter=0;
    repos=0;
    attente=0; // temps d'inactivité avant débrayage
    trigger=0; // est à 1 si Stop envoyé une seule fois 
    Bot=0; // MJBot Tullins = 0, MJBot R2D2 = 1 and STBot = 2
    ventilo=0;
    myservo.period_ms(20);
    myservo.pulsewidth_us(MID);
    receivedCOMMAND = false;
    while(1) {
       if(receivedCOMMAND) {
            receivedCOMMAND = false;
            executeCommand(commandRECEIVED);
         if (action>=1) {
            if (action==1) {
                if (repos==1) {
                    pc_uart.printf("stop hard \r\n\n");
                    motors[0]->PrepareHardStop();
                    motors[1]->PrepareHardStop();
                    repos=0;
                    attente=0;
                    trigger=1;
                    inter=0; }
                else {
                    pc_uart.printf("stop debraye \r\n\n");
                    motors[0]->PrepareSoftHiZ();
                    motors[1]->PrepareSoftHiZ();
                    trigger=0;}
                x_nucleo_ihm02a1->PerformPreparedActions();}
            if (action==2) {
                pc_uart.printf("Avance \r\n\n");
                motors[0]->PrepareRun(StepperMotor::BWD, 150);
                motors[1]->PrepareRun(StepperMotor::FWD, 150);
                x_nucleo_ihm02a1->PerformPreparedActions();
                repos=1; }
            if (action==3) {
                pc_uart.printf("Recule \r\n\n");
//                motors[0]->Run(StepperMotor::BWD, 200);
//                motors[1]->Run(StepperMotor::FWD, 200);}
                motors[0]->PrepareRun(StepperMotor::FWD, 150);
                motors[1]->PrepareRun(StepperMotor::BWD, 150);
                x_nucleo_ihm02a1->PerformPreparedActions();
                repos=1; }
            if (action==4) {
                pc_uart.printf("droite AV \r\n\n");
 //               motors[0]->Run(StepperMotor::FWD, 100);
                motors[0]->PrepareHardStop();
                motors[1]->PrepareRun(StepperMotor::FWD, 150);
                x_nucleo_ihm02a1->PerformPreparedActions();
                repos=1; }
            if (action==5) {
                pc_uart.printf("gauche AV \r\n\n");
                motors[0]->PrepareRun(StepperMotor::BWD, 150);
                motors[1]->PrepareHardStop();
                x_nucleo_ihm02a1->PerformPreparedActions();
                repos=1; }
            if (action==17) {
                pc_uart.printf("droite AR \r\n\n");
                motors[0]->PrepareHardStop();
                motors[1]->PrepareRun(StepperMotor::BWD, 150);
                x_nucleo_ihm02a1->PerformPreparedActions();
                repos=1; }
            if (action==18) {
                pc_uart.printf("gauche AR \r\n\n");
                motors[0]->PrepareRun(StepperMotor::FWD, 150);
                motors[1]->PrepareHardStop();
                x_nucleo_ihm02a1->PerformPreparedActions();
                repos=1; }
            if (action==6) {
                pc_uart.printf("pivote d \r\n\n");
                motors[0]->PrepareRun(StepperMotor::FWD, 75);
                motors[1]->PrepareRun(StepperMotor::FWD, 75);
                x_nucleo_ihm02a1->PerformPreparedActions();
                repos=1; }
            if (action==7) {
                pc_uart.printf("pivote g\r\n\n");
                motors[0]->PrepareRun(StepperMotor::BWD, 75);
                motors[1]->PrepareRun(StepperMotor::BWD, 75);
                x_nucleo_ihm02a1->PerformPreparedActions();
                repos=1; }
//                motors[0]->Run(StepperMotor::FWD, 200);
//                motors[1]->Run(StepperMotor::FWD, 200); }
            if (action==9) {
                pc_uart.printf("debrayer \r\n\n");
                motors[0]->PrepareSoftHiZ();
                motors[1]->PrepareSoftHiZ();
                x_nucleo_ihm02a1->PerformPreparedActions();
                repos=0;
                trigger=0;
                attente=0;}
            if (action==10) {
                pc_uart.printf("pince haute \r\n\n");
                myservo.pulsewidth_us(MIN);}
            if (action==11) {
                pc_uart.printf("pince basse \r\n\n");
                myservo.pulsewidth_us(MAX);}
           if (action==12) {
                pc_uart.printf("Ventilo On \r\n\n");
                ventilo=1;
                }
            if (action==13) {
                pc_uart.printf("Ventilo Off \r\n\n");
                ventilo=0;
                }
            if (action==19) {
                pc_uart.printf("Ventilo ON/OFF \r\n\n");
                if (ventilo==0) {
                   ventilo=1;}
                else {ventilo=0;}}
            if (action==14) {
                pc_uart.printf("LEDs ON \r\n\n");
                leda(3);}
            if (action==15) {
                pc_uart.printf("LEDs OFF \r\n\n");
                leda(0);}
            if (action==16) {
                pc_uart.printf("pince haute/basse \r\n\n");
                if (pince==0) {
                   leda(1);
                   inter=0;
                   myservo.pulsewidth_us(MIN);
                   pince=1;}
                else {
                    myservo.pulsewidth_us(MAX);
                    leda(2);
                    pince=0;}
                
          action=0;
        } }
        wait(0.1);
        attente++;
        if (attente>50 && repos==0 && trigger==1) {
            pc_uart.printf("attente OK\r\n\n");
            motors[0]->PrepareSoftHiZ();
            motors[1]->PrepareSoftHiZ();
            x_nucleo_ihm02a1->PerformPreparedActions();
            attente=0;
            trigger=0; }       
    } } }