File content as of revision 1:3589e8f6e99c:

/*
 *   OvniBot
 *   
 */
 
#include "XNucleo6180XA1.h"
#undef printf
#include "Crealab.h"

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

// ---------------- PIN DEFINITIONS ---------------------
InterruptIn  buttonBox(PC_13);
Motor motorRG(PB_13, PB_14, PB_15, PB_1);
Motor motorRD(PA_0, PA_1, PA_4, PB_0);

// insert the motors and indicate wheel diameter and distance between wheels
Creabot mybot(&motorRG, &motorRD, 9.0f,13.2f); // proto CréaLab

// PIN & number of LEDS. Available color ==> BLUE, ORANGE, RED, GREEN, BLACK, WHITE, PURPLE, PINK, YELLOW
LED_WS2812 ledBand(PB_3,4);

bool receivedCOMMAND;
char commandRECEIVED;
int parameterRECEIVED;
int state;
char commandLine[256];
int commandPosition;
uint32_t dist;

// ----------- PIN Definitions 6180 -------------
#define VL6180X_I2C_SDA   D14 
#define VL6180X_I2C_SCL   D15 
static XNucleo6180XA1 *board = NULL;
// penser à modifier dans vl6180x_platform.h -> commenter la ligne // #define MY_LOG 1

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

void callback() {
    switch(state){
     case 0:
       break;
     case 1:
       break;
     case 2:
       break;
     case 3:
       break;
     default:
     }
    state=state+1;
}

void detect(int parameter)
{
    mybot.move(FORWARD,300);
    dist=250;
    while (dist>parameter)
       {wait (0.2); 
       board->sensor_bottom->get_distance(&dist) ; }
//    DEBUG ("Distance: %d \n\r", dist);
    mybot.stopMove();
}

void routine1() 
{
     DEBUG("routine1\n\r");
     wait(1);
     mybot.move(FORWARD,200);
     ledBand.SetColor(BLUE,0) ;
     ledBand.SetColor(BLACK,1) ;
     ledBand.SetColor(BLACK,2) ;
     ledBand.SetColor(BLACK,3) ;
     ledBand.StartRotation(0.5) ;
     wait(2);
      ledBand.SetColor(WHITE,0) ;
     ledBand.SetColor(BLACK,1) ;
     ledBand.SetColor(BLACK,2) ;
     ledBand.SetColor(BLACK,3) ;
     ledBand.StartRotation(0.5) ;    
     state=0;  
/*     mybot.move(FORWARD,9);
      dist=200;
     while (dist>120)
           {wait (0.3); 
           board->sensor_bottom->get_distance(&dist) ; }
     DEBUG ("Distance: %d \n\r", dist);
     mybot.stopMove();
          ledBand.SetColor(WHITE,0) ;
     ledBand.SetColor(RED,1) ;
     ledBand.SetColor(BLUE,2) ;
     ledBand.SetColor(GREEN,3) ; */                  
} 

void routine2() 
{
     DEBUG("routine2\n\r");
     wait(1);
     ledBand.SetColor(WHITE,0) ;
     ledBand.SetColor(RED,1) ;
     ledBand.SetColor(BLUE,2) ;
     ledBand.SetColor(GREEN,3) ;
     ledBand.StartRotation(0.6) ;
     mybot.move(BACKWARD,200);
     state=0;  
/*     mybot.move(FORWARD,9);
      dist=200;
     while (dist>120)
           {wait (0.3); 
           board->sensor_bottom->get_distance(&dist) ; }
     DEBUG ("Distance: %d \n\r", dist);
     mybot.stopMove(); */                  
} 

void routines() 
{
     DEBUG("routines\n\r");
     wait(1);
     ledBand.StopRotation();
     ledBand.SetColor(BLACK,0) ;
     ledBand.SetColor(BLACK,1) ;
     ledBand.SetColor(BLACK,2) ;
     ledBand.SetColor(BLACK,3) ; 
     mybot.stopMove();
     state=0;                 
} 


void executeCommand(char c) {
        bool flaghelp = false;
        switch (c) {
            case 'h':
                help();
                state=0;
                flaghelp=true;
                CASE('l', "lance la routine 1", routine1();  )
                CASE('m', "lance la routine 2", routine2();  )
                CASE('s', "stoppe les routines", routines();  )
                CASE('t', "accelere", routines();  )                          
            default :
                DEBUG("invalid command; use: 'h' for help()\n\r");
                state=0;
        }}

void analyseCommand(char *command) {
    switch(command[0]) {
        case 'l':
            commandRECEIVED = 'l';
             break;
        case 's':
            commandRECEIVED = 's';
             break;
        case 'm':
            commandRECEIVED = 'm';
             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,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);
}

/* Stop all processes */
void stop_all()
{
    mybot.stopMove();
}

/* Interrupt routine, switch of end of course */
void clicked()
{
//    DEBUG("Labyrinthe\n\r");
    commandRECEIVED = 'l';
    receivedCOMMAND = true;
}

void endOfMove(int status) {
    DEBUG("ENDOFMOVE\n\r");
    state=0;
}

/* Main Routine */
int main()
{
  wait(1);
    
    /* Connect EoC button */
    CATCH_BUTTON(buttonBox,clicked);
    
     /* Init 6180 X-Nucleo  */
    int status;
    DevI2C *device_i2c = new DevI2C(VL6180X_I2C_SDA, VL6180X_I2C_SCL);
    board = XNucleo6180XA1::instance(device_i2c, A5, A4, D13, D4);
    status = board->init_board();
    if (status) {DEBUG("Failed to init board!\n\r"); } 
     
    commandPosition=0;
//    bt_uart.attach(getBT);
    pc_uart.attach(getPC);
    mybot.setCallBack(endOfMove);
    mybot.setSpeed(5); // max 8 cm.s, average 5 cm.s
    ledBand.SetColor(BLACK);
    state=0;
    receivedCOMMAND = false;
    DEBUG("OvniBot\n\r");
    while(1) {
       if(state==0 && receivedCOMMAND) {
            receivedCOMMAND = false;
            state=1;
            executeCommand(commandRECEIVED);
        }
       wait (0.3); 
       board->sensor_bottom->get_distance(&dist) ;  
       DEBUG ("Distance bottom: %d \n\r", dist);
       if (dist<100) {
           commandRECEIVED = 'l';
           executeCommand(commandRECEIVED);}
       wait (0.3); 
       board->sensor_top->get_distance(&dist) ;  
       DEBUG ("Distance top: %d \n\r", dist);
       if (dist<100) {
           commandRECEIVED = 's';
           executeCommand(commandRECEIVED);}
       wait(0.1);
    } }