-#include "Adafruit_LIS3DH.h"
 #include "Adafruit_VL53L0X.h"
 
-//#include <Wire.h>
+Adafruit_VL53L0X left_sensor = Adafruit_VL53L0X();
+Adafruit_VL53L0X front_sensor = Adafruit_VL53L0X();
 
-Adafruit_VL53L0X sensor1 = Adafruit_VL53L0X();
-Adafruit_VL53L0X sensor2 = Adafruit_VL53L0X();
-
-//SYSTEM_THREAD(ENABLED);
-//SYSTEM_MODE(MANUAL);
-
-Adafruit_LIS3DH sample;
-int xAccel = 0;
-int yAccel = 0;
-int WKP = 1;
-
-//Adafruit_LIS3DH accel(Wire, 0, WKP);
-
-double time_x;
-double prevTime_x;
-double distance_x;
-double velocity_x;
-double time_y;
-double prevTime_y;
-double distance_y;
-double velocity_y;
-
-//Previous values
-double prevVelocity_x;
-double prevVelocity_y;
-double prevAccel_x;
-double prevAccel_y;
-double tolerance = .1;
-double maxRatio = 70;
-double startingPos [2] = {0, 0}; //x and y coordinates
-
-Adafruit_LIS3DH lis;
-
-void accel_init() {
-    lis = Adafruit_LIS3DH();
-    delay(500);
-    lis.setRange(LIS3DH_RANGE_4_G);
-}
+double tolerance = .1; //yes
+double max_ratio = 70; //i agree
 
 void setup()
 {

  //ratio is left to right
  //if percentage is 1, turn all the way left
  //if percentage is 0, turn straight
  if (sensorLV > tolerance && sensorMV > tolerance) {
    motorLV = 0.0;
    motorRV = 0.25; //go slowly at first
  } else {
    double ratio = sensorLV / sensorMV;
    Serial.print("Ratio: ");
    Serial.print(ratio);
    Serial.println();
    double percentage = ratio / maxRatio;
    //max .5 to account for mid/left vs right/left
    //min = 0
    double adjustedValue = percentage / 2;
    motorLV = adjustedValue;
    motorRV = (1 - adjustedValue);
  }
  Serial.print("Right capacity: ");
  Serial.print(motorRV);
  Serial.println();
  Serial.print("Left capacity: ");
  Serial.print(motorLV);
  Serial.println();
  //Set the right motor to motorRV of max value
  //Set the left motor of motorLV of max value
  /* CODE INSERTED HERE */
  Serial.println();
  Serial.println();

  prevTime_x = time_x;
  prevTime_y = time_y;
  prevAccel_y = a_y;
  prevAccel_x = a_x;
  prevVelocity_x = velocity_x;
  prevVelocity_y = velocity_y;

}

  pinMode(A1, OUTPUT);
  digitalWrite(A0, LOW);
  digitalWrite(A1, LOW);

  pinMode(A0, INPUT);
  left_sensor.begin();
  left_sensor.setAddress(0x30);
  
  pinMode(A1, INPUT);
  front_sensor.begin();
  front_sensor.setAddress(0x31);

}

void loop() {

  VL53L0X_RangingMeasurementData_t measure;

  double sensor_left_val = front_sensor.rangingTest(&measure, false);
  double sensor_front_val = left_sensor.rangingTest(&measure, false);
  Serial.print(sensor_left_val);
  Serial.print("   -----   ");
  Serial.print(sensor_front_val);
  Serial.println();
  double motor_right_val;
  double motor_left_val; //percentages
  // if (left_sensor.timeoutOccurred()) { Serial.print(" TIMEOUT"); }
  // max is 2m away = 2000 mm
  // min = 30 mm
  //max ratio is 70
