ESP32 Servo corrupts I2C time of flight sensors VL53L0X

Other Hardware 3rd Party Boards
1

Hey!

Im trying to use servos to position TOF sensors that communicate with i2c.

The sensors work fine and wiring is not a problem, without the servos on, I can twist and turn the sensors all I want and get readings from the I2C.

As soon as I turn on the Servos, the I2C returns error values(distance over 60000mm) and will not recover until I reset the Lolin D32. It then works fine the first reading, but as soon as the Servos turn, the signal is lost.

I read here that I should add dummy servos so the ESP32Servo library would not use the same timers for the Servo and the I2C. That seems to delay the problem, but eventually, the I2C sensors will timeout and deliver 65535mm over and over again.

Any tips? Thanks

#include <Arduino.h>
#include <Wire.h>
#include <VL53L0X.h>
#include <ESP32Servo.h>
// #include <NewPing.h>

// Steps for obstacles
int step = 1;

/*
Digital pins 34,36,39 are input only

21  SDA       (ESP32 default)
22  SCL       (ESP32 default)

XSHUT    (For two i2c modules)

USB = 5V
BAT = 3V

*/
const int servoFrontPin = 33;
const int servoTopPin = 32;
const int XSHUT_pin1 = 23;
const int motor1Pin1 = 15; // IN1
const int motor1Pin2 = 2; // IN2
const int motorTwoPin1 = 0; // IN3
const int motorTwoPin2 = 4; // IN4
const int  Sensor2_newAddress = 42;
VL53L0X Sensor1;
VL53L0X Sensor2;

int sensorFront;
int sensorLeft;
int sensorFrontEdge;
int sensorRightEdge;

// Servo
// Possible PWM GPIO pins on the ESP32: 0(used by on-board button),2,4,5(used by on-board LED),12-19,21-23,25-27,32-33 
Servo dummyServo1;  // Because otherwise it corrupts i2c
Servo dummyServo2;  // because the first servos share timer with i2c
                    // https://www.esp32.com/viewtopic.php?t=9271
                    
Servo myservoFront;  // create servo object to control a servo
Servo myservoTop;  // create servo object to control a servo
int servoFrontPosition = 90;
int servoTopPosition = 90;
int oldServoPosition = 90; // Compare before setting and delaying

void turnServo(String servoName, String direction){
  if(servoName == "front"){
    oldServoPosition = servoFrontPosition;
    if(direction == "left") servoFrontPosition = 180;
    if(direction == "right") servoFrontPosition = 0;
    if(oldServoPosition != servoFrontPosition){
      myservoFront.write(servoFrontPosition);
      delay(1000);
    }
    myservoFront.write(servoFrontPosition);
  } else if(servoName == "top"){
    oldServoPosition = servoTopPosition;
    if(direction == "left") servoTopPosition = 10; // Zero was off
    if(direction == "right") servoTopPosition = 180; 
    if(oldServoPosition != servoTopPosition){
      myservoTop.write(servoTopPosition);
      delay(1000);
    }
  }
}



/*




 Setup ====================================================================




*/

void setup() {

  Serial.begin(115200);
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(XSHUT_pin1, OUTPUT);

  Wire.begin();
  //Change address of sensor
  Sensor2.setAddress(Sensor2_newAddress);
  pinMode(XSHUT_pin1, INPUT);
  delay(10);

  Sensor1.init();
  Sensor2.init();

  Sensor1.setTimeout(500);
  Sensor2.setTimeout(500);

  Sensor1.startContinuous();
  Sensor2.startContinuous();

    // Servo
  myservoFront.setPeriodHertz(50);// Standard 50hz servo
  myservoFront.attach(servoFrontPin, 500, 2400);
  myservoFront.write(servoFrontPosition); // LEFT is 180
  myservoTop.setPeriodHertz(50);// Standard 50hz servo
  myservoTop.attach(servoTopPin, 500, 2400);   
  myservoTop.write(servoTopPosition); // RIGHT is 180
  delay(1000);
}


/*




 Loop =====================================================================




*/

void loop() {
  sensorFront = Sensor1.readRangeContinuousMillimeters();
  sensorLeft = Sensor2.readRangeContinuousMillimeters();
  Serial.print(sensorFront);
  Serial.print(" \t ");
  Serial.print(sensorLeft);
  Serial.println(); 
  digitalWrite(LED_BUILTIN, HIGH);
  turnServo("front", "left");
  turnServo("top", "left");
  delay(2000);
  digitalWrite(LED_BUILTIN, LOW);
  turnServo("front", "right");
  turnServo("top", "right");
}
2

The ESP32Servo library uses the LED Control API LED Control (LEDC) - ESP32 - — ESP-IDF Programming Guide latest documentation, the last time I checked. The ESP32Servo allows the ESP32 to select which hardware timer to use instead of assigning hardware timers, which can cause issues with other modules.

The ESP32 has two MCPWM modules Motor Control Pulse Width Modulator (MCPWM) - ESP32 - — ESP-IDF Programming Guide latest documentation that I find much easier and less buggy to work with then using the LED Control API to control servos. The MCPWM has its own hardware timers, freeing up the systems hardware timers for other operations.

#include "sdkconfig.h"
#include "esp_system.h" //This inclusion configures the peripherals in the ESP system.
#include "driver/mcpwm.h"

setup()
{
  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1A, GPIO_NUM_4 ); // NS
  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, GPIO_NUM_12 );
  ////
  mcpwm_config_t pwm_config = {};
  pwm_config.frequency = 50;    //frequency = 50Hz, i.e. for every servo motor time period should be 20ms
  pwm_config.cmpr_a = 0;    //duty cycle of PWMxA = 0
  pwm_config.cmpr_b = 0;    //duty cycle of PWMxb = 0
  pwm_config.counter_mode = MCPWM_UP_COUNTER;
  pwm_config.duty_mode = MCPWM_DUTY_MODE_0;
  mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_0, &pwm_config);    //Configure PWM0A timer 0 with above settings
  mcpwm_init(MCPWM_UNIT_0, MCPWM_TIMER_1, &pwm_config);    //Configure PWM0A timer 1 with above settings
}

void mcpwm_gpio_initialize(void)
{
  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM1A, GPIO_NUM_4 );
  mcpwm_gpio_init(MCPWM_UNIT_0, MCPWM0A, GPIO_NUM_12 );
}

void fMoveNS( int MoveTo )
{
  mcpwm_set_duty_in_us(MCPWM_UNIT_0, MCPWM_TIMER_0, MCPWM_OPR_A, MoveTo);
}

Should be enough to get you started with using the MCPWM for controlling servos.

With an ESP32-WROOM, I'd not use GPIO2, which is connected to the on-board LED.

If you want to use the LED Control API to control your servos, which allows you to assign the hardware times to each servo group, instead of a random timer assignment from the ESP32Sero library.

I'd go with using the MCPWM.

3

Thanks for the reply. I will have a look at those suggestions.

It turns out that the vibrations were causing the voltage to drop. Once the voltage drops the I2C cannot reconnect, since it has to connect using the XSHUT pin.

The sensor without the XSHUT address change can handle loss of voltage, but the other cannot.

I soldered the VCC GND pins on the sensors and forced two wires into one breadboard pinhole, making it more securely fastened.