obstacle avoidance with states

//Obstacle avoidance with states program

#include <ComponentObject.h>
#include <RangeSensor.h>
#include <SparkFun_VL53L1X.h>
#include <vl53l1x_class.h>
#include <vl53l1_error_codes.h>

#include <Wire.h>
#include "SparkFun_VL53L1X.h"        //everything above is brought in from distance sensor
//Optional interrupt and shutdown pins.
#define SHUTDOWN_PIN 2
#define INTERRUPT_PIN 3

int distance;
SFEVL53L1X distanceSensor;

unsigned long startMillis;  //used for taking time for reference later
unsigned long currentMillis;    //used for exiting play state
unsigned long currentMillis2;   //used for vibrating motor every 3 minutes
const unsigned long period = 600000;  //the value is a number of milliseconds (10 minutes) timer for play mode
const unsigned long period2 = 180000; //this is 3 minutes. timer for vibration

#define sensingRangeMax 250     //in mm, 250mm max distance
#define sensingRangeMin 5      //in mm 5mm min distance


int Left_Distance = 0;        //variable for left distance
int Right_Distance = 0;     //variable for right distance
int state = 0;          //declared for possible states
const int LeftMotorForward = 6;
const int LeftMotorBackward = 8;        //these are PWM pins for motor control
const int RightMotorForward = 9;
const int RightMotorBackward = 12;

void setup() {
  // put your setup code here, to run once:
  pinMode(RightMotorForward, OUTPUT);
  pinMode(LeftMotorForward, OUTPUT);
  pinMode(LeftMotorBackward, OUTPUT);
  pinMode(RightMotorBackward, OUTPUT);
  Wire.begin();
  Serial.begin(9600);
  startMillis = millis();      //write start time in varible for comparisons later
}

void loop() {
  // put your main code here, to run repeatedly:
  Left_Distance = Left_Distance_test();        //takes distance from sensor on left side
  Serial.println("Your left distance is ");
  Serial.println(Left_Distance);
  Right_Distance = Right_Distance_test();  /   / takes distance from sensor on right side
  Serial.println("Your right distance is ");
  Serial.println(Right_Distance);

  switch (state) {


    case 0:                            //search mode (slow)

      currentMillis2 = millis();          //will vibrate motor every 3 minutes
      if (currentMillis2 - startMillis >= period2) {
        entice();                   //function that turns on vibrating motor
        startMillis = currentMillis;    //restarts count to do again in 3 minutes
      }

      else if (Left_Distance > Right_Distance) {    //if there is no object detected in left side, angle left
        //Angle_left();
      }
      else if (Right_Distance > Left_Distance) {  //if there is no object detected in right side, angle right
        //Angle_right();
      }
      else if (Right_Distance == 0 && Left_Distance == 0) { //if both sensors 0 or "read out of range" then move forward because its clear
        //mForward();
      }
      else if (Right_Distance < 40 || Left_Distance < 40) {  //if object is detected closer than 40 mm, turn hard
        //hardTurn();
      }
      break;

    case 1:                             //play mode (fast)
      currentMillis = millis();         //only is in this state for 10 minutes then goes to search mode
      if (currentMillis - startMillis >= period) {
        state = 0;                    //goes  back to search mode time expires
        startMillis = currentMillis;
      }
      else if (Left_Distance > Right_Distance) {    //if there is no object detected in left side, angle left
        //Angle_left();
      }
      else if (Right_Distance > Left_Distance) {    //if there is no object detected in right side, angle right
        //Angle_right();
      }

      else if (Right_Distance == 0 && Left_Distance == 0) { //if both sensors read out of range then move forward
        //mForward_fast();
      }
      else if (Right_Distance < 40 || Left_Distance < 40) { //if closer than 40 mm, turn hard
        //hardTurn();
      }

      break;

    case 2:       //this will stop motors if z is negative for 5 seconds (upside down)
      // mStop();
      break;
  }
}

int Left_Distance_test() {
  distanceSensor.startRanging();              //reads distance
  int distance = distanceSensor.getDistance();
  distanceSensor.stopRanging();

  if (distance >= sensingRangeMin && distance <= sensingRangeMax)
  {
    return distance; //send back the distance only if within range
  } else {
    return 0; //else return zero
  }
}

int Right_Distance_test() {
  /*
    distanceSensor.startRanging();              //reads distance
    int distance = distanceSensor.getDistance();
    distanceSensor.stopRanging();

    if (distance >= sensingRangeMin && distance <= sensingRangeMax)
    {
      return distance; //send back the distance only if within range
    } else {
      return 0; //else return zero
    }
  */
}
void entice() {
  digitalWrite(5, HIGH);    //this is the vibrating motor
  delay(1000);
}