motor+sensor

#include <VL6180X.h>
#include <Wire.h>


VL6180X sensor;

int leftWheelB = 23; //A1
int leftWheelA = 22; //A2
int rightWheelA = 20; //B1
int rightWheelB = 21; //B2
int onboardLED = 13; 

int speed = 50;
int SenVal;

void setup() {
  // Pin configuration
  pinMode(leftWheelB, OUTPUT);
  pinMode(leftWheelA, OUTPUT);
  pinMode(rightWheelB, OUTPUT);
  pinMode(rightWheelA, OUTPUT);
  
  pinMode(onboardLED, OUTPUT);
  
  // Show when robot is on
  digitalWrite(onboardLED, HIGH);
  
  Serial.begin(9600);

  Wire.begin();

  sensor.init();
  sensor.configureDefault();

  // Reduce range max convergence time and ALS integration
  // time to 30 ms and 50 ms, respectively, to allow 10 Hz
  // operation (as suggested by Table 6 ("Interleaved mode
  // limits (10 Hz operation)") in the datasheet).
  sensor.writeReg(VL6180X::SYSRANGE__MAX_CONVERGENCE_TIME, 30);
  sensor.writeReg16Bit(VL6180X::SYSALS__INTEGRATION_PERIOD, 50);

  sensor.setTimeout(500);

   // stop continuous mode if already active
  sensor.stopContinuous();
  // in case stopContinuous() triggered a single-shot
  // measurement, wait for it to complete
  delay(3000);
  // start interleaved continuous mode with period of 100 ms
  sensor.startInterleavedContinuous(100);

  
}
void setDriveSpeed(int reading) {
  if (reading < 20){
    analogWrite(leftWheelA, speed);
    analogWrite(leftWheelB, 0);

    analogWrite(rightWheelA, speed);
    analogWrite(rightWheelB, 0);
  }
  else {
    analogWrite(leftWheelA, 0);
    analogWrite(leftWheelB, 0);

    analogWrite(rightWheelA, 0);
    analogWrite(rightWheelB, 0);
  }
  

    //analogWrite(leftWheelA, 0);
    //analogWrite(leftWheelB, speed);
    
    //analogWrite(rightWheelA, 0);
    //analogWrite(rightWheelB, speed);

    //delay(1000);
}
void loop() {

    SenVal = sensor.readRangeContinuousMillimeters();
    setDriveSpeed(SenVal);
    Serial.println(SenVal);
    
  /*
    Serial.print("Ambient: ");
    Serial.print(sensor.readAmbientContinuous());
    
    if (sensor.timeoutOccurred()) { Serial.print(" TIMEOUT"); }

    Serial.print("\tRange: ");
    Serial.print(sensor.readRangeContinuousMillimeters());
    if (sensor.timeoutOccurred()) { Serial.print(" TIMEOUT"); }

    Serial.println();

  */ 
  }