main.cpp

00001 /*
00002  *   Crealab : version Scratch
00003  */
00004 
00005 #undef printf
00006 #include "Crealab.h"
00007 #include "LED_WS2812.h"
00008 #include "VL53L0X.h"
00009 
00010 //#include "AsyncToF.h"
00011 
00012 Serial pc_uart(USBTX, USBRX); // USBTX = PA2
00013 Serial bt_uart(D1, D0);  // PA9 = Tx, PA10 = Rx
00014 //Serial bt_uart(D5, D4);  // PA9 = Tx, PA10 = Rx
00015 uint32_t go;
00016 bool ShowDataLog=false;
00017 uint16_t TOF_UseRange_mm = 450;
00018 // ---------------- PIN DEFINITIONS ---------------------
00019 
00020 InterruptIn  buttonBox(PA_12);
00021 
00022 // --- Define the Four PINs & Time of movement used for Motor drive
00023 
00024 Motor motorRG(D13,D11,D10,D9);
00025 Motor motorRD(A0,A1,A2,A3);
00026 
00027 
00028 Creabot mybot(&motorRG, &motorRD, 8.10f,8.3f); // insert the motors and indicate wheel diameter and distance between wheels
00029 LED_WS2812 ledBand(A4,2);
00030 
00031 DigitalIn isCmd(A5); // relier le pin A5 a la masse (GND) pour faire fonctionner le robot en mode demo autonome
00032 
00033 //static I2C busI2C(D4,D5);
00034 //static I2C busI2C(D0,D1);
00035 static I2C busI2C(D12,A6);
00036 
00037 static DigitalOut shutDownPin(D6);
00038 static VL53L0X TOF1 (&busI2C, &shutDownPin, D7);
00039 
00040 bool receivedCOMMAND;
00041 char commandRECEIVED;
00042 int parameterRECEIVED;
00043 int state;
00044 char commandLine[256];
00045 int commandPosition;
00046 VL53L0X_Error TOFError;
00047 
00048 void init_TOF( void )
00049 {
00050     pc_uart.printf("\n\r---  Initializing the TOF sensor ---\n\r"); 
00051     
00052     /* Init the TOF sensor with default params */
00053     TOFError = TOF1.init_sensor();
00054     /*  This configuration "Range_Config_HIGH_SPEED" sets following parameters:
00055             signalLimit = (FixPoint1616_t)(0.15*65536); ==> VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE
00056             sigmaLimit = (FixPoint1616_t)(32*65536); ==> VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE
00057             timingBudget = 10000; Must be > c_min_timing_budget_us  = 20000;
00058             preRangeVcselPeriod = 14;
00059             finalRangeVcselPeriod = 10;  */
00060 
00061     if (TOFError == VL53L0X_ERROR_NONE) { TOFError = TOF1.start_measurement(range_continuous_polling, NULL, Range_Config_DEFAULT); }
00062     pc_uart.printf("TOFError    = %d.  ( 0 = OK ) \n\r",  TOFError);     
00063     pc_uart.printf(" TOF1.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE] = %4.2f   \n\r",
00064                     TOF1.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGNAL_RATE_FINAL_RANGE] /65536.1);
00065     pc_uart.printf(" TOF1.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] = %4.2f  \n\r",
00066         TOF1.CurrentParameters.LimitChecksValue[VL53L0X_CHECKENABLE_SIGMA_FINAL_RANGE] /65536.1);
00067        
00068     if (TOFError == VL53L0X_ERROR_NONE) { TOFError = TOF1.VL53L0X_set_measurement_timing_budget_us(100000); }
00069     TOF1.CurrentParameters.RangeOffset_um = 14; // seems it is wrongly configured by default to 54mm offset!
00070     pc_uart.printf("TOFError    = %d.  ( 0 = OK ) \n\r",  TOFError);     
00071        
00072     // Report_Deep_Infos(TOF1);
00073     
00074     if (TOFError != VL53L0X_ERROR_NONE) 
00075       { pc_uart.printf("\n\r--- Error Initializing the TOF sensor ---\n\r"); 
00076         //while(1) { }; // consider to hang up the device here
00077         // alternatively just clear the Error State:
00078         // TOF1.ErrState = VL53L0X_ERROR_NONE;
00079       }
00080       else 
00081       {  pc_uart.printf("\n\r--- Success Initializing the TOF sensor ---\n\r");  }    
00082 }
00083 
00084 bool TOF_OK;
00085 VL53L0X_RangingMeasurementData_t RangeResults;
00086 uint16_t TOF_Dist_mm;
00087 double TOF_RelDist; // measured distance relative to the Used Range. Should be clamped to 0 ... 1, but can actually also be negative or >1
00088 const uint32_t out_of_range = 0xffff;
00089 uint16_t TOF_MinDist_mm=5;
00090 
00091 bool GetNewTOF()
00092 {   uint32_t NewDist_mm;
00093     TOFError =  TOF1.get_measurement(range_continuous_polling, &RangeResults);
00094     if ( (TOFError == VL53L0X_ERROR_NONE) && (RangeResults.RangeStatus == 0) )
00095         { // we have a valid range.
00096         // Report_Range_Infos( RangeResults );
00097         if (ShowDataLog) pc_uart.printf("Dist=%3d, SigRate=%4.2fM/s, AmbRate=%4.2fM/s, SpadRtnCnt=%3.1f, SigmEst=%3.3f\n\r", 
00098             RangeResults.Range_mm, RangeResults.SignalRateRtn_MHz/65536.1,  RangeResults.AmbientRateRtn_MHz/65536.1,  
00099             RangeResults.EffectiveSpadRtnCount/256.1, RangeResults.SigmaEstimate/65536.1);
00100         //Report_Deep_Infos(TOF1);
00101         NewDist_mm = RangeResults.Range_mm;
00102         // 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.
00103          
00104         if (TOF_Dist_mm == out_of_range) // previous value was useless
00105             { TOF_Dist_mm = NewDist_mm; }  // only take the new value
00106           else // average old with new value!!
00107             { TOF_Dist_mm = ( NewDist_mm * 2 + TOF_Dist_mm * 3 ) / 5 ; 
00108             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
00109 
00110             // The device theoretical working distance range is from 0 up to 1270mm, distance is always expressed in mm!
00111             // However here we transform to a relative number, of which only the range 0 ... 1 should be used.
00112             TOF_RelDist = ( (double) TOF_Dist_mm - TOF_MinDist_mm) / TOF_UseRange_mm; 
00113             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!
00114             }  
00115         return true; // new measurement is available
00116         } 
00117       else 
00118         { // pc_uart.printf("  --\r");
00119         TOF_Dist_mm = out_of_range;
00120         TOF_RelDist = out_of_range;
00121         return false; // no new measurement is available
00122         }    
00123 }
00124 
00125 
00126 
00127 void help() // Display list of Commands
00128 {
00129     DEBUG("List of commands:\n\r");
00130     DEBUG(" h --> Help, display list of commands\n\r");
00131 }
00132 
00133 void callback()
00134 {
00135     switch(state) {
00136         case 0:
00137             break;
00138         case 1:
00139             break;
00140         case 2:
00141             break;
00142         case 3:
00143             break;
00144         default:
00145     }
00146     state=state+1;
00147 }
00148 
00149 /* Stop all processes */
00150 void stop_all()
00151 {
00152     mybot.stopMove();
00153 }
00154 
00155 void clicked()
00156 {
00157     DEBUG("Labyrinthe\n\r");
00158     commandRECEIVED = 'l';
00159     receivedCOMMAND = true;
00160 }
00161 
00162 void labyrinthe()
00163 {
00164     wait(1);
00165     mybot.move(FORWARD,15);
00166     mybot.waitEndMove();
00167     mybot.move(ROTATE, 90);
00168     mybot.waitEndMove();
00169     mybot.move(FORWARD,15);
00170     mybot.waitEndMove();
00171     mybot.move(ROTATE, 90);
00172     mybot.waitEndMove();
00173 }
00174 
00175 void executeCommand(char c, int parameter)
00176 {
00177     bool flaghelp = false;
00178     switch (c) {
00179         case 'h':
00180             help();
00181             state=0;
00182             flaghelp=true;
00183             CASE('a', "Avance   <cm>  (a <cm>)  => Avance du nombre de cm indiques", mybot.move(FORWARD,parameter);    )
00184             CASE('r', "Recule   <cm>  (r <cm>)  => Recule du nombre de cm indiques", mybot.move(BACKWARD,parameter);    )
00185             CASE('d', "Droite   <deg> (d <deg>) => Tourne a droite du nombre de degres indiques", mybot.move(RIGHT,parameter,0);    )
00186             CASE('g', "Gauche   <deg> (g <deg>) => Tourne a gauche du nombre de degres indiques", mybot.move(LEFT,parameter,0);    )
00187             CASE('p', "Pivote_d <deh> (p <deg>) => pivote a droite du nombre de degres indiques", mybot.move(ROTATE,parameter);    )
00188             CASE('q', "Pivote_g <deg> (q <deg>) => pivote a gauche du nombre de degres indiques", mybot.move(ROTATE,-parameter);    )
00189             CASE('s', "Stop                     => Arrete les moteurs", mybot.stopMove(); state=0;  )
00190             CASE('l', "Labyrinthe               => Lance le parcours Labyrinthe", labyrinthe();  )
00191         default :
00192             DEBUG("invalid command; use: 'h' for help()\n\r");
00193             state=0;
00194     }
00195 }
00196 
00197 void analyseCommand(char *command, int parameter)
00198 {
00199     parameterRECEIVED = parameter;
00200     switch(command[0]) {
00201         case 'A':
00202         case 'a':
00203             commandRECEIVED = 'a';
00204             break;
00205         case 'R':
00206         case 'r':
00207             commandRECEIVED = 'r';
00208             break;
00209         case 'D':
00210         case 'd':
00211             commandRECEIVED = 'd';
00212             break;
00213         case 'G':
00214         case 'g':
00215             commandRECEIVED = 'g';
00216             break;
00217         case 'L':
00218         case 'l':
00219             commandRECEIVED = 'l';
00220             break;
00221         case 'P':
00222             if(command[7]=='d') {
00223                 commandRECEIVED = 'p';
00224             } else if (command[7]=='g') {
00225                 commandRECEIVED = 'q';
00226             } else {
00227                 commandRECEIVED = 'h';
00228             }
00229             break;
00230         case 'p':
00231             commandRECEIVED = 'p';
00232             break;
00233         case 'q':
00234             commandRECEIVED = 'q';
00235             break;
00236         case 'S':
00237         case 's':
00238             commandRECEIVED = 's';
00239             mybot.stopMove();
00240             state=0;
00241             break;
00242         default:
00243             commandRECEIVED = 'h';
00244     }
00245 }
00246 
00247 void checkCommand(int result, char *command, int parameter)
00248 {
00249     if(result==2 || command[0]=='h' || command[0]=='s') {
00250         if(command[0]=='c') {
00251             DEBUG("CREABOT PRESENT\n\r");
00252         } else {
00253             analyseCommand(command, parameter);
00254             receivedCOMMAND = true;
00255         }
00256     }
00257 }
00258 
00259 void split(char *line, int length)
00260 {
00261     char command[256];
00262     int parameter=0;
00263     int result = 1;
00264     int i=0;
00265     int j=0;
00266     while(i<length && line[i]==' ') {
00267         i++;
00268     }
00269     while(i<length && line[i]!=' ') {
00270         command[j]=line[i];
00271         i++;
00272         j++;
00273     }
00274     command[j]=0;
00275     i++;
00276     j=0;
00277     while(i<length && line[i]!=' ' && line[i]>='0' && line[i]<='9') {
00278         i++;
00279         j++;
00280     }
00281     if(j>0) {
00282         result++;
00283         i--;
00284         int k=1;
00285         while(j>0) {
00286             parameter += (line[i]-'0')*k;
00287             j--;
00288             i--;
00289             k=k*10;
00290         }
00291     }
00292     checkCommand(result, command, parameter);
00293 }
00294 
00295 void storeC(char c)
00296 {
00297     if(c==10 || c==13|| commandPosition >= 254) {
00298         split(commandLine, commandPosition);
00299         commandPosition=0;
00300     } else {
00301         commandLine[commandPosition++]=c;
00302         commandLine[commandPosition]=0;
00303     }
00304 }
00305 
00306 void getBT()
00307 {
00308     char c = bt_uart.getc();
00309     storeC(c);
00310 }
00311 
00312 void getPC()
00313 {
00314     char c = pc_uart.getc();
00315     storeC(c);
00316 }
00317 
00318 void endOfMove(int status)
00319 {
00320     DEBUG("ENDOFMOVE\n\r");
00321     state=0;
00322 }
00323 void demoMode(void)
00324 {
00325     init_TOF();
00326     TOF_Dist_mm=200;
00327     while(1){   
00328         if(GetNewTOF()){
00329             pc_uart.printf("\n\rDistance : %d mm\n\r",TOF_Dist_mm);
00330 //            DEBUG("\n\rDistance : %d mm\n\r",TOF_Dist_mm)
00331         }
00332         
00333         if (TOF_Dist_mm < 60 & TOF_Dist_mm > 0){
00334             mybot.move(BACKWARD,5);
00335             mybot.waitEndMove(5000000); // 5 seconds
00336             mybot.move(ROTATE,-90);
00337             mybot.waitEndMove(5000000); // 5 seconds
00338         }
00339         else{
00340             mybot.move(FORWARD,5);
00341 //            mybot.waitEndMove(5000000); // 5 seconds
00342         }
00343     }    
00344 }
00345 /*  Routine */
00346 int main()
00347 {
00348     /* Rename HC-C6 BT*/
00349 
00350     commandPosition=0;  
00351     bt_uart.attach(getBT);
00352     pc_uart.attach(getPC);
00353     mybot.setCallBack(endOfMove);
00354     mybot.setSpeed(4.5); // max 6.9 cm.s, average 5 cm.s
00355     state=0;
00356     receivedCOMMAND = false;
00357     DEBUG("CREABOT alpha version\n\r");
00358     __disable_irq();
00359     pc_uart.printf("Init Done ...\n\r");
00360     __enable_irq();
00361     pc_uart.printf("start passed ok\n");
00362 
00363    
00364     ledBand.SetColor(BLUE,0);
00365     ledBand.SetColor(BLUE,1);
00366 
00367     isCmd.mode(PullUp);
00368     pc_uart.printf("switchState=%d\n\r", isCmd.read());
00369     if (isCmd.read()==0){
00370         demoMode();
00371     }
00372     while(1) {
00373         if(state==0 && receivedCOMMAND) {
00374             receivedCOMMAND = false;
00375             state=1;
00376             executeCommand(commandRECEIVED, parameterRECEIVED);
00377         }
00378         wait(0.1);
00379     }
00380 }