Diff: main.cpp

Revision:

0:0e577ce96b2f

Child:

1:69b5d8f0ba9c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Feb 08 09:48:46 2020 +0000
@@ -0,0 +1,380 @@
+/*
+ *   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(D1, D0);  // PA9 = Tx, PA10 = Rx
+//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 motorRG(D13,D11,D10,D9);
+Motor motorRD(A0,A1,A2,A3);
+
+
+Creabot mybot(&motorRG, &motorRD, 8.10f,8.3f); // insert the motors and indicate wheel diameter and distance between wheels
+LED_WS2812 ledBand(A4,2);
+
+DigitalIn isCmd(A5); // relier le pin A5 a la masse (GND) pour faire fonctionner le robot en mode demo autonome
+
+//static I2C busI2C(D4,D5);
+//static I2C busI2C(D0,D1);
+static I2C busI2C(D12,A6);
+
+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;
+    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;  
+    bt_uart.attach(getBT);
+    pc_uart.attach(getPC);
+    mybot.setCallBack(endOfMove);
+    mybot.setSpeed(4.5); // 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;
+            executeCommand(commandRECEIVED, parameterRECEIVED);
+        }
+        wait(0.1);
+    }
+}
\ No newline at end of file