/*
6/5/2024
Graham Dopierala
This is code for my crawdad bot

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

#include <Wire.h>
#include <VL53L0X.h>
VL53L0X sensor;
const unsigned long move1 = 9000;
const unsigned long move2 = 9800;
const unsigned long move3 = 2500;
unsigned long prevTime = 0;

//Each motor (M1 and M2) has two pins connected.
//Setting one pin HIGH and the other LOW will make the motor turn one way
//Setting them in reverse will make it turn the opposite way
//Seting them both HIGH or both LOW stops the motor
#define M2A 17
#define M2B 16
#define M1A 19
#define M1B 18

// Based on the weight of the robot, some PWM speeds are too slow for
// the robot to be able to move. Decreasing SLOWEST_SPEED will
// let your robot go slower, but too low and it might not move on the
// lowest settings
#define SLOWEST_SPEED 190

//This function accepts values for the left and right motor.
//Values of 128 will stop the motor.
//Increasing values over 128 will make the motor go faster and faster forward
//Decreasing values below 128 will make the motor go faster and faster in reverse
void motor(uint8_t left, uint8_t right){
  if (left == 128){ //Set left to stop
    ledcWrite(3, 0); //Set M2B to LOW
    ledcWrite(2, 0); //Set M2A to LOW
  } else if (left > 128){ //Set left to forward
    left = map(left, 128, 255, SLOWEST_SPEED, 255);
//    left = (left - 128) + 200;
    ledcWrite(3, 0); //Set M2B to LOW
    ledcWrite(2, left); //Set M2A to pwm
  } else { //else set left to reverse
    left = map(left, 128, 0, SLOWEST_SPEED, 255);
//    left = (128 - left) + 200;
    ledcWrite(3, left); //Set M2B to pwm
    ledcWrite(2, 0); //Set M2A to LOW
  }

  if (right == 128){ //Set right to stop
    ledcWrite(1, 0); //Set M2B to LOW
    ledcWrite(0, 0); //Set M2A to LOW
  } else if (right > 128){ //Set right to forward
    right = map(right, 128, 255, SLOWEST_SPEED, 255);
    //right = (right - 128) + 200;
    ledcWrite(1, 0); //Set M2B to LOW
    ledcWrite(0, right); //Set M2A to pwm
  } else { //else set right to reverse
    right = map(right, 128, 0, SLOWEST_SPEED, 255);
    //right = (128 - right) + 200;
    ledcWrite(1, right); //Set M2B to pwm
    ledcWrite(0, 0); //Set M2A to LOW
  }
}

void setup() {
  Serial.begin(115200);

  Wire.begin();
  sensor.setTimeout(500);
  if (!sensor.init())
  {
    Serial.println("Error with sensor!");
  }
  
  
  
  // Set the 4 motor controll 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() {                        //the main loop function for robot movement
  unsigned long currTime = millis(); //setting up millis and the sensor
  int distance = sensor.readRangeSingleMillimeters();
  Serial.println( millis() );
  if (distance > 65){                //if the sensor is more than 65mm away from an object it "swims around" (moves in a straight line and turns around every 9 seconds)
   motor(220,225);

    if (currTime - prevTime >= move1){
      motor(150, 106);
    }
      if (currTime - prevTime >= move2){
        prevTime = currTime;
      }
  }
  if (distance < 65){               //if the sensor is less that 65mm away from an object it swims very fast in reverse and turns around
    motor(25,25);
    delay(2500);
    motor(150,106);
    delay(800);
    motor(220,225);
  }
 
}