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
mainCopy.txt
- Committer:
- elab
- Date:
- 2020-02-08
- Revision:
- 0:0e577ce96b2f
File content as of revision 0:0e577ce96b2f:
/* * 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); } }