Working version without LEDs
Voici le dernier schéma de cablage (version du 08/02/2020)
https://os.mbed.com/media/uploads/max_ence/schemarobot_fev2020.pdf
Diff: mainCopy.txt
- Revision:
- 0:0e577ce96b2f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mainCopy.txt Sat Feb 08 09:48:46 2020 +0000 @@ -0,0 +1,379 @@ +/* + * 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); + } +} \ No newline at end of file