File content as of revision 1:69b5d8f0ba9c:

/*
 *   Crealab : version Scratch
 */

#undef printf
#include "Crealab.h"
#include "LED_WS2812.h"
#include "VL53L0X.h"

//#include "AsyncToF.h"

Serial pc_uart(USBTX, USBRX); // USBTX = PA2
Serial bt_uart(D5, D4);  // PA9 = Tx, PA10 = Rx
uint32_t go;
bool ShowDataLog=false;
uint16_t TOF_UseRange_mm = 450;
// ---------------- PIN DEFINITIONS ---------------------

InterruptIn  buttonBox(PA_12);

// --- Define the Four PINs & Time of movement used for Motor drive
//Motor motorRD(PA_4, PA_3, PA_1, PA_0);
//Motor motorRG(PA_8, PA_11, PB_5, PB_4);
Motor motorRG(D12,D11,D10,D9);
Motor motorRD(A3,A2,A1,A0);
Creabot mybot(&motorRG, &motorRD, 8.10f,8.3f); // insert the motors and indicate wheel diameter and distance between wheels
LED_WS2812 ledBand(A4,2);
/*AsyncToF myToF(D4,D5,D13,A2,0x52);*/
//AsyncToF myToF(D4,D5,D13,NC,0x52);
DigitalIn isCmd(A5); // relier le pin A5 a la masse (GND) pour faire fonctionner le robot en mode demo autonome

static I2C busI2C(D0,D1);
static DigitalOut shutDownPin(D6);
static VL53L0X TOF1 (&busI2C, &shutDownPin, D7);

bool receivedCOMMAND;
char commandRECEIVED;
int parameterRECEIVED;
int state;
char commandLine[256];
int commandPosition;
VL53L0X_Error TOFError;

void init_TOF( void )
{
    pc_uart.printf("\n\r---  Initializing the TOF sensor ---\n\r"); 
    
    /* Init the TOF sensor with default params */
    TOFError = TOF1.init_sensor();
    /*  This configuration "Range_Config_HIGH_SPEED" sets following parameters:
            signalLimit = (FixPoint1616_t)(0.15*65536); ==> VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE
            sigmaLimit = (FixPoint1616_t)(32*65536); ==> VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE
            timingBudget = 10000; Must be > c_min_timing_budget_us  = 20000;
            preRangeVcselPeriod = 14;
            finalRangeVcselPeriod = 10;  */

    if (TOFError == VL53L0X_ERROR_NONE) { TOFError = TOF1.start_measurement(range_continuous_polling, NULL, Range_Config_DEFAULT); }
    pc_uart.printf("TOFError    = %d.  ( 0 = OK ) \n\r",  TOFError);     
    pc_uart.printf(" TOF1.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE] = %4.2f   \n\r",
                    TOF1.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE] /65536.1);
    pc_uart.printf(" TOF1.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = %4.2f  \n\r",
        TOF1.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] /65536.1);
       
    if (TOFError == VL53L0X_ERROR_NONE) { TOFError = TOF1.VL53L0X_set_measurement_timing_budget_us(100000); }
    TOF1.CurrentParameters.RangeOffset_um = 14; // seems it is wrongly configured by default to 54mm offset!
    pc_uart.printf("TOFError    = %d.  ( 0 = OK ) \n\r",  TOFError);     
       
    // Report_Deep_Infos(TOF1);
    
    if (TOFError != VL53L0X_ERROR_NONE) 
      { pc_uart.printf("\n\r--- Error Initializing the TOF sensor ---\n\r"); 
        //while(1) { }; // consider to hang up the device here
        // alternatively just clear the Error State:
        // TOF1.ErrState = VL53L0X_ERROR_NONE;
      }
      else 
      {  pc_uart.printf("\n\r--- Success Initializing the TOF sensor ---\n\r");  }    
}

bool TOF_OK;
VL53L0X_RangingMeasurementData_t RangeResults;
uint16_t TOF_Dist_mm;
double TOF_RelDist; // measured distance relative to the Used Range. Should be clamped to 0 ... 1, but can actually also be negative or >1
const uint32_t out_of_range = 0xffff;
uint16_t TOF_MinDist_mm=5;

bool GetNewTOF()
{   uint32_t NewDist_mm;
    TOFError =  TOF1.get_measurement(range_continuous_polling, &RangeResults);
    if ( (TOFError == VL53L0X_ERROR_NONE) && (RangeResults.RangeStatus == 0) )
        { // we have a valid range.
        // Report_Range_Infos( RangeResults );
        if (ShowDataLog) pc_uart.printf("Dist=%3d, SigRate=%4.2fM/s, AmbRate=%4.2fM/s, SpadRtnCnt=%3.1f, SigmEst=%3.3f\n\r", 
            RangeResults.Range_mm, RangeResults.SignalRateRtn_MHz/65536.1,  RangeResults.AmbientRateRtn_MHz/65536.1,  
            RangeResults.EffectiveSpadRtnCount/256.1, RangeResults.SigmaEstimate/65536.1);
        //Report_Deep_Infos(TOF1);
        NewDist_mm = RangeResults.Range_mm;
        // pc_uart.printf("%6ld\r", NewDist_mm); // Distance is ~1700 when looking at the ceiling, so approximately 1 per mm. A really usable distance range is only up to 1270.
         
        if (TOF_Dist_mm == out_of_range) // previous value was useless
            { TOF_Dist_mm = NewDist_mm; }  // only take the new value
          else // average old with new value!!
            { TOF_Dist_mm = ( NewDist_mm * 2 + TOF_Dist_mm * 3 ) / 5 ; 
            if (TOF_Dist_mm < TOF_MinDist_mm) { TOF_MinDist_mm = TOF_Dist_mm; pc_uart.printf("DMin = %3d. \r\n", TOF_MinDist_mm);} // adapt the min dist parameter

            // The device theoretical working distance range is from 0 up to 1270mm, distance is always expressed in mm!
            // However here we transform to a relative number, of which only the range 0 ... 1 should be used.
            TOF_RelDist = ( (double) TOF_Dist_mm - TOF_MinDist_mm) / TOF_UseRange_mm; 
            if (TOF_RelDist <0) {TOF_RelDist = 0;} // clamping values <0 (that should actually never occur with above adaption of Min dist!, values >1 can still happen!
            }  
        return true; // new measurement is available
        } 
      else 
        { // pc_uart.printf("  --\r");
        TOF_Dist_mm = out_of_range;
        TOF_RelDist = out_of_range;
        return false; // no new measurement is available
        }    
}



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

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

void clicked()
{
    DEBUG("Labyrinthe\n\r");
    commandRECEIVED = 'l';
    receivedCOMMAND = true;
}

void labyrinthe()
{
    wait(1);
    mybot.move(FORWARD,15);
    mybot.waitEndMove();
    mybot.move(ROTATE, 90);
    mybot.waitEndMove();
    mybot.move(FORWARD,15);
    mybot.waitEndMove();
    mybot.move(ROTATE, 90);
    mybot.waitEndMove();
}

void executeCommand(char c, int parameter)
{
    bool flaghelp = false;
    pc_uart.printf("hffehihgiehighegieighieughei\n");
    switch (c) {
        case 'h':
            help();
            state=0;
            flaghelp=true;
            CASE('a', "Avance   <cm>  (a <cm>)  => Avance du nombre de cm indiques", mybot.move(FORWARD,parameter);    )
            CASE('r', "Recule   <cm>  (r <cm>)  => Recule du nombre de cm indiques", mybot.move(BACKWARD,parameter);    )
            CASE('d', "Droite   <deg> (d <deg>) => Tourne a droite du nombre de degres indiques", mybot.move(RIGHT,parameter,0);    )
            CASE('g', "Gauche   <deg> (g <deg>) => Tourne a gauche du nombre de degres indiques", mybot.move(LEFT,parameter,0);    )
            CASE('p', "Pivote_d <deh> (p <deg>) => pivote a droite du nombre de degres indiques", mybot.move(ROTATE,parameter);    )
            CASE('q', "Pivote_g <deg> (q <deg>) => pivote a gauche du nombre de degres indiques", mybot.move(ROTATE,-parameter);    )
            CASE('s', "Stop                     => Arrete les moteurs", mybot.stopMove(); state=0;  )
            CASE('l', "Labyrinthe               => Lance le parcours Labyrinthe", labyrinthe();  )
        default :
            DEBUG("invalid command; use: 'h' for help()\n\r");
            state=0;
    }
}

void analyseCommand(char *command, int parameter)
{
    parameterRECEIVED = parameter;
    switch(command[0]) {
        case 'A':
        case 'a':
            commandRECEIVED = 'a';
            break;
        case 'R':
        case 'r':
            commandRECEIVED = 'r';
            break;
        case 'D':
        case 'd':
            commandRECEIVED = 'd';
            break;
        case 'G':
        case 'g':
            commandRECEIVED = 'g';
            break;
        case 'L':
        case 'l':
            commandRECEIVED = 'l';
            break;
        case 'P':
            if(command[7]=='d') {
                commandRECEIVED = 'p';
            } else if (command[7]=='g') {
                commandRECEIVED = 'q';
            } else {
                commandRECEIVED = 'h';
            }
            break;
        case 'p':
            commandRECEIVED = 'p';
            break;
        case 'q':
            commandRECEIVED = 'q';
            break;
        case 'S':
        case 's':
            commandRECEIVED = 's';
            mybot.stopMove();
            state=0;
            break;
        default:
            commandRECEIVED = 'h';
    }
}

void checkCommand(int result, char *command, int parameter)
{
    if(result==2 || command[0]=='h' || command[0]=='s') {
        if(command[0]=='c') {
            DEBUG("CREABOT PRESENT\n\r");
        } else {
            analyseCommand(command, parameter);
            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, parameter);
}

void storeC(char c)
{
    if(c==10 || c==13|| commandPosition >= 254) {
        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 endOfMove(int status)
{
    DEBUG("ENDOFMOVE\n\r");
    state=0;
}
void demoMode(void)
{
    init_TOF();
    TOF_Dist_mm=200;
    while(1){
        if(GetNewTOF()){
//            pc_uart.printf("\n\rDistance : %d mm\n\r",TOF_Dist_mm);
//            DEBUG("\n\rDistance : %d mm\n\r",TOF_Dist_mm)
        }
        
        if (TOF_Dist_mm < 60 & TOF_Dist_mm > 0){
            mybot.move(BACKWARD,5);
            mybot.waitEndMove(5000000); // 5 seconds
            mybot.move(ROTATE,-90);
            mybot.waitEndMove(5000000); // 5 seconds
        }
        else{
            mybot.move(FORWARD,5);
//            mybot.waitEndMove(5000000); // 5 seconds
        }
    }    
}
/*  Routine */
int main()
{
    /* Rename HC-C6 BT*/

    commandPosition=0;  
    pc_uart.attach(getPC);
    bt_uart.attach(getBT);

    mybot.setCallBack(endOfMove);
    mybot.setSpeed(6.0); // max 6.9 cm.s, average 5 cm.s
    state=0;
    receivedCOMMAND = false;
    DEBUG("CREABOT alpha version\n\r");
    __disable_irq();
    pc_uart.printf("Init Done ...\n\r");
    __enable_irq();
    pc_uart.printf("start passed ok\n");

    
    ledBand.SetColor(BLUE,0);
    ledBand.SetColor(BLUE,1);

    isCmd.mode(PullUp);
    pc_uart.printf("switchState=%d\n\r", isCmd.read());
    if (isCmd.read()==0){
        demoMode();
    }
    while(1) {
        if(state==0 && receivedCOMMAND) {
            receivedCOMMAND = false;
            state=1;
            pc_uart.printf("tototo\n");
            executeCommand(commandRECEIVED, parameterRECEIVED);
        }
        wait(0.1);
    }
}