Version 10

    Wearable Tech

    Enter Your Electronics & Design Project for a chance to win a $100 Shopping Cart!

    Back to The Project14 homepage

    Project14 Home
    Monthly Themes
    Monthly Theme Poll

     

     

    /* ESP32_TOF_VL5130_USRF
    
     This code is based on the four ""SingleRanging" examples in the VL53L0X API.
     The range readings are in units of mm and the Pololu library example
    
     Written By:
     Cameron Rodriguez
     cameron@notimpossiblelabs.com
    
    
     Last Modified:
     May 2nd, 2018
    
    
     */
    
    
    
    
    // ###################################
    // ############ Libraries ############
    // ###################################
    
    
    #include <Wire.h>
    #include <VL53L0X.h>
    
    
    // Math Library
    #include <math.h>
    
    
    
    
    
    
    // ###################################
    // ######### Program Options #########
    // ###################################
    
    
    // #define DEBUG // Enable debug prints
    // #define MotorsDisabled // Kill the PWM Motors
    #define USEnable // Enable US readings
    
    
    //
      #ifdef DEBUG
        boolean debug = true;
      #else
        boolean debug = false;
      #endif
    
    
    
    
      #ifdef USEnable
        boolean usenable = true;
      #else
        boolean usenable = false;
      #endif
    
    
    
    
      #ifdef MotorsDisabled
        boolean RunMotors = false;
      #else
        boolean RunMotors = true;
      #endif
      
    // Uncomment this line to use long range mode. This
    // increases the sensitivity of the sensor and extends its
    // potential range, but increases the likelihood of getting
    // an inaccurate reading because of reflections from objects
    // other than the intended target. It works best in dark
    // conditions.
    
    
    // #define LONG_RANGE
    
    
    // Uncomment ONE of these two lines to get
    // - higher speed at the cost of lower accuracy OR
    // - higher accuracy at the cost of lower speed
    
    
    #define HIGH_SPEED
    //#define HIGH_ACCURACY
    
    
    // ###################################
    // ####### GPIO DECLARATIONS #########
    // ###################################
    
    
    #define PWMPin1        10
    #define PWMPin2        11
    #define USpw            5
    #define USa            A0
    #define LuxPin         A1
    
    
    #define LEDC_CHANNEL_0     0
    #define LEDC_CHANNEL_1     1
    
    
    #define LEDC_TIMER_13_BIT  13 // use 13 bit precission for LEDC timer
    #define LEDC_BASE_FREQ     5000 // use 5000 Hz as a LEDC base frequency
    
    
    // ###################################
    // ############# GLOBALS #############
    // ###################################
    
    
    // ***** PWM Setup *****
    unsigned int pwmrange = 511;  // 32u4 Huzzah boards
    
    
    //Normalizing Scale Factor Calculations for Log Scale Mapping
    int count = 0; int NumAvg = 1;
    #ifdef LONG_RANGE
      double TOF_RangeMax = 2000.0;
    #else
      double TOF_RangeMax = 1200.0;
    #endif
    double TOF_RangeMin = 50.0;
    
    
    double US_RangeMax = 5000.0;
    double US_RangeMin = 300.0;
    
    
    int PWMmap1 = 0;
    int PWMmap2 = 0;
    
    
    double TOF_MAPSF = 1 / -log(1 / (double(TOF_RangeMax) - double(TOF_RangeMin) + 1.0)); // 255 *  1/-LOG(1/(RANGEMAX-RANGEMIN+1)) OR 255/MAX LOG VALUE GIVEN THE RANGE
    double US_MAPSF = 1 / -log(1 / (double(US_RangeMax) - double(US_RangeMin) + 1.0)); // 255 *  1/-LOG(1/(RANGEMAX-RANGEMIN+1)) OR 255/MAX LOG VALUE GIVEN THE RANGE
    
    
    // *********************
    
    
    
    
    // ***** Measurement Setup *****
    
    
    const int HistSz = 5; 
    long MI = 0; // Measurement Index
    #if defined HIGH_SPEED
      const int USMeasInt = 1;
    #else
      const int USMeasInt = 3;
    #endif
    
    
    // *********************
    
    
    // ***** Lux *****
    
    
    int LuxVal = 0;
    
    
    // *********************
    
    
    
    
    // ***** TOF Setup *****
    float lux = 0.0;
    int TOFrange = 0;
    int TOFstatus = 1;
    
    
    int TOF_Hist[HistSz]; // the Hist of the Status of the TOF
    int TOF_Hist_Ind; // Index of Hist Array
    int TOF_HistV = 0;
    
    
    //
    
    
    // ***** US Setup *****
    long USpulse, USrange;
    int USstatus = 0;
    
    
    int US_Hist[HistSz]; // the Hist of the Status of the US
    int US_Hist_Ind; // Index of Hist Array
    int US_HistV = 0;
    
    
    //
    
    
    // ***** COMM Setup *****
    const int serialSpeed = 115200;       // USB serial port speed
    
    
    
    
    VL53L0X sensor;
    
    
    // ###################################
    // ############ BASE FXNS ############
    // ###################################
    
    
    void AnalogWrite(uint8_t channel, uint32_t value, uint32_t valueMax = pwmrange) {
      // calculate duty, 8191 from 2 ^ 13 - 1
      uint32_t duty = (8191 / valueMax) * min(value, valueMax);
    
    
      // write duty to LEDC
      ledcWrite(channel, duty);
    }
    
    
    // ###################################
    // ############## SETUP ##############
    // ###################################
    
    
    void setup()
    {
      if(debug){ 
        Serial.begin(serialSpeed);
        while (!Serial) {
          ; // wait for serial port to connect. Needed for native USB port only
        }
        Serial.println( F( "UART Communication Established" ) );
      }
    
    
      // Initialize I2C
      Wire.begin();
      
      // Initialize the GPIO
      //pinMode(PWMPin1, OUTPUT); digitalWrite(PWMPin1, LOW);
      //pinMode(PWMPin2, OUTPUT); digitalWrite(PWMPin2, LOW);
    
    
      ledcSetup(LEDC_CHANNEL_0, LEDC_BASE_FREQ, LEDC_TIMER_13_BIT);
      ledcAttachPin(PWMPin1, LEDC_CHANNEL_0);
    
    
      ledcSetup(LEDC_CHANNEL_1, LEDC_BASE_FREQ, LEDC_TIMER_13_BIT);
      ledcAttachPin(PWMPin2, LEDC_CHANNEL_1);
      
      pinMode(USpw, INPUT);
      pinMode(USa, INPUT);
      pinMode(LuxPin, INPUT);
    
    
      if(debug){ Serial.println( F( "Looking 4 Sensor") );}
      sensor.init();
      sensor.setTimeout(500);
      if(debug){ Serial.println(F( "Sensor Starting") );}
    
    
      #if defined LONG_RANGE
        // lower the return signal rate limit (default is 0.25 MCPS)
        sensor.setSignalRateLimit(0.1);
        // increase laser pulse periods (defaults are 14 and 10 PCLKs)
        sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodPreRange, 18);
        sensor.setVcselPulsePeriod(VL53L0X::VcselPeriodFinalRange, 14);
      #endif
    
    
      #if defined HIGH_SPEED
        // reduce timing budget to 20 ms (default is about 33 ms)
        sensor.setMeasurementTimingBudget(20000);
      #elif defined HIGH_ACCURACY
        // increase timing budget to 200 ms
        sensor.setMeasurementTimingBudget(200000);
      #else // default is about 33 ms
        sensor.setMeasurementTimingBudget(33000);  
      #endif
    
    
      //Initialize the status histories for the TOF and US to 80% good
      for (int TOF_Hist_Ind = 0; TOF_Hist_Ind < HistSz; TOF_Hist_Ind++) {
        TOF_Hist[TOF_Hist_Ind] = 0;
      }
      TOF_Hist_Ind = 0;
      
      for (int US_Hist_Ind = 0; US_Hist_Ind < HistSz; US_Hist_Ind++) {
        US_Hist[US_Hist_Ind] = 0;
      }
      US_Hist_Ind = 0;
    }
    
    
    // ###################################
    // ############# MAIN LOOP ###########
    // ###################################
    
    
    void loop(){
    
    
      LuxVal = analogRead(LuxPin);
      if(LuxVal >= 950){
      //if((LuxVal >= 900)||(TOF_HistV <=3)){
        #ifdef USEnable
          usenable = true;
        #endif  
      }
      else{usenable = false;}
      
      if(MI%USMeasInt == 0){ 
        if(usenable){
          read_US();
          if( (USrange <= US_RangeMax) && (USrange > US_RangeMin) ){ 
            USstatus = 1; US_Tracker();}
          else{USstatus = 0; US_Tracker();}
        }
        else{USstatus = 0; USrange = 0; US_HistV = 0;}
      }
      
      TOFrange = sensor.readRangeSingleMillimeters();
      if( (!sensor.timeoutOccurred()) && (TOFrange <= TOF_RangeMax) && (TOFrange > 0) ){ 
        TOFstatus = 1; TOF_Tracker();}
      else{TOFstatus = 0; TOF_Tracker();}
      
      if(( TOFstatus == 1 )||(USstatus == 1 )){ // At least 1 channel of good data
    
    
        if((TOFstatus == 1) && (USstatus == 0)){// TOF GOOD, US BAD
          if (TOFrange < TOF_RangeMin) {PWMmap1 = pwmrange;}
          else { PWMmap1 = LogMapTOF(double(TOFrange));}
        }
        else if((TOFstatus == 0) && (USstatus == 1)){ // TOF BAD, US GOOD
          if (USrange > TOF_RangeMax) {PWMmap1 = 0;}
          else { PWMmap1 = LogMapTOF(double(USrange));}
        }
        else{ // BOTH GOOD
          if(TOF_HistV < US_HistV){ // Choose TOF over US if its history equal is better
            if (USrange > TOF_RangeMax) {PWMmap1 = 0;}
            else { PWMmap1 = LogMapTOF(double(USrange));}
          }
          else{
            if (TOFrange < TOF_RangeMin) {PWMmap1 = pwmrange;}
            else { PWMmap1 = LogMapTOF(double(TOFrange));}
          }
        }     
       
        if(RunMotors){
          AnalogWrite(PWMPin1, PWMmap1);
          AnalogWrite(PWMPin2, PWMmap1);
        }
        if(debug){
          if(( TOFstatus == 1 )&&(USstatus == 1 )){
            Serial.print("Lux Value: = "); Serial.print( LuxVal ); Serial.print( ",     ");
            Serial.print("PWM: = "); Serial.print( PWMmap1 ); Serial.print( ",     ");
            Serial.print("TOF: Status = "); Serial.print( TOF_HistV ); Serial.print(", Range = "); Serial.print( TOFrange ); Serial.print( "mm,     ");
            Serial.print("US: Status = " ); Serial.print( US_HistV ); Serial.print(", Range = ");Serial.print( USrange ); Serial.println( "mm" );}
          else if(( TOFstatus == 0 )&&(USstatus == 1 )){
            Serial.print("Lux Value: = "); Serial.print( LuxVal ); Serial.print( ",     ");
            Serial.print("PWM: = "); Serial.print( PWMmap1 ); Serial.print( ",     ");
            Serial.print("TOF: Status = "); Serial.print( TOF_HistV ); Serial.print(", Range = "); Serial.print( "----- " ); Serial.print( "mm,     ");
            Serial.print("US: Status = " ); Serial.print( US_HistV ); Serial.print(", Range = ");Serial.print( USrange ); Serial.println( "mm" );}
          else{
            Serial.print("Lux Value: = "); Serial.print( LuxVal ); Serial.print( ",     ");
            Serial.print("PWM: = "); Serial.print( PWMmap1 ); Serial.print( ",     ");
            Serial.print("TOF: Status = "); Serial.print( TOF_HistV ); Serial.print(", Range = "); Serial.print( TOFrange ); Serial.print( "mm,     ");
            Serial.print("US: Status = " ); Serial.print( US_HistV ); Serial.print(", Range = ");Serial.print( "----- "); Serial.println( "mm" );}
        }
        
      }
      else{
        if(RunMotors){
          digitalWrite(PWMPin1, LOW);
          digitalWrite(PWMPin2, LOW);
        }
        if(debug){
          Serial.print("Lux Value: = "); Serial.print( LuxVal ); Serial.print( ",     ");
          Serial.print("PWM: = 0,     ");
          Serial.print("TOF: Status = "); Serial.print( TOF_HistV ); Serial.print(", Range = "); Serial.print( "----- " ); Serial.print( "mm,     ");
          Serial.print("US: Status = " ); Serial.print( US_HistV ); Serial.print(", Range = ");Serial.print( "----- "); Serial.println( "mm" );
        } 
      }
    
    
      MI++;
      
    }
    
    
    
    
    // ###################################
    // ########### SUBFUNCTIONS ##########
    // ###################################
    
    
    void US_Tracker(){
    
    
      US_HistV = US_HistV - US_Hist[US_Hist_Ind]; // Remove old value from the running total
      US_Hist[US_Hist_Ind] = USstatus; // add new val to index
      US_HistV = US_HistV + US_Hist[US_Hist_Ind]; // Add new value to the running total
      US_Hist_Ind ++;
    
    
      if (US_Hist_Ind >= HistSz) {US_Hist_Ind = 0;}  // if we're at the end of the array, then wrap around to the beginning
      
    }
    
    
    void TOF_Tracker(){
    
    
      TOF_HistV = TOF_HistV - TOF_Hist[TOF_Hist_Ind];
      TOF_Hist[TOF_Hist_Ind] = TOFstatus;
      TOF_HistV = TOF_HistV + TOF_Hist[TOF_Hist_Ind];
      TOF_Hist_Ind ++;
    
    
      if (TOF_Hist_Ind >= HistSz) {TOF_Hist_Ind = 0;} // if we're at the end of the array, then wrap around to the beginning
    }
    
    
    void read_US(){
      #if defined HIGH_SPEED
        USrange = analogRead(USa)*5;
      #else
        USrange = pulseIn(USpw, HIGH);
      #endif  
    }
    
    
    int LogMapTOF(double val) {
      double MAPval = (val - TOF_RangeMin + 1);
      MAPval = TOF_MAPSF * pwmrange * ( -log( MAPval / (TOF_RangeMax - TOF_RangeMin + 1) ));
      return int(MAPval);
    } // END LogMap
    
    
    int LogMapUS(double val) {
      double MAPval = (val - TOF_RangeMin + 1);
      MAPval = US_MAPSF * pwmrange * ( -log( MAPval / (US_RangeMax - US_RangeMin + 1) ));
      return int(MAPval);
    } // END LogMap

    Parts from:

    Product NameManufacturer Part Number
    Quantity
    Part Link
    Transistor NPN T0922n2222a1Buy NowBuy Now
    Diode1N40011Buy NowBuy Now
    Resistor 180Ω
    180Ω1Buy NowBuy Now


    Parts From:

    Product NameManufacturer Part Number
    Quantity
    TOF Sensor33171
    US Sensor9841
    ESP32 Feather
    34041
    Female Header Pin28861
    2000mah LiPo20111

     

    Additional Parts:

     

    Product NameManufacturer Part Number
    Quantity
    Haptic MotorJinLong Machinery Z7AL2B16920824
    Custom PCBOSH Park1
    Wrist BraceMueller 74676862729
    1
    Velcro
    3M B00006IC2T
    1

     

    Design for a Cause | Project C.O.D.I:  Help Build a Superhero Costume for Cody!

     

    Project C.O.D.I. | Overview, Current Proof of Concept, and Next Steps