Project C.O.D.I. | Schematics, Arduino IDE Sketch, B.O.M, and Eagle Files!

Enter Your Electronics & Design Project for a chance to win a $100 Shopping Cart!
/* 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