/*
4/27/2024
Salvador Monroy
Simplified "Scared" code for the esp32_rbt.0 board
Moves backward when an object is detected within 200mm.

GPIO22 SCL
GPIO21 SDA
GPIO19 M1A
GPIO18 M1B
GPIO17 M2A
GPIO16 M2B
GPIO4 WS2812
*/

#include <Wire.h>
#include <VL53L0X.h>
VL53L0X sensor;

#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18

void reverse(uint8_t mySpeed){
  ledcWrite(3, mySpeed); //Set M2B to mySpeed
  ledcWrite(2, 0); //Set M2A to LOW
  ledcWrite(1, mySpeed);//Set M1B to mySpeed
  ledcWrite(0, 0); //Set M1A to LOW
}

void forward(uint8_t mySpeed){
  ledcWrite(2, mySpeed); //Set M2A to mySpeed
  ledcWrite(3, 0); //Set M2B to LOW
  ledcWrite(0, mySpeed);//Set M1A to mySpeed
  ledcWrite(1, 0); //Set M1B to LOW
}

void setup() {
  Serial.begin(115200);
  
  // This section creates the I2C (Wire) connection and queries for a VL53LX
  Wire.begin();
  sensor.setTimeout(500);
  if (!sensor.init())
  {
    Serial.println("Failed to detect and initialize sensor!");
    while (1) { /* stay here forever */ }
  }
  
  // Set the 4 motor control lines as outputs
  pinMode(M1A, OUTPUT);
  pinMode(M1B, OUTPUT);
  pinMode(M2A, OUTPUT);
  pinMode(M2B, OUTPUT);

  // These lines connect 4 PWM channels to the 4 motor control Pins
  ledcSetup(0, 30000, 8);
  ledcAttachPin(M1A, 0);
  ledcSetup(1, 30000, 8);
  ledcAttachPin(M1B, 1);
  ledcSetup(2, 30000, 8);
  ledcAttachPin(M2A, 2);
  ledcSetup(3, 30000, 8);
  ledcAttachPin(M2B, 3);
}

void loop() {
  int distance = sensor.readRangeSingleMillimeters();
  if (distance < 200){ // Object detected within 200mm
    reverse(255);
    delay(200);
  } else {
    forward(230);
  }
  Serial.println(distance);
}