Crawler V2! A Obstacle Avoiding Robot

#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_MotorShield.h>
#include <ESP8266WiFi.h>
#include <BlynkSimpleEsp8266.h>

#include <ESP8266mDNS.h>
#include <WiFiUdp.h>
#include <ArduinoOTA.h>

Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *leftMotor = AFMS.getMotor(1);
Adafruit_DCMotor *rightMotor = AFMS.getMotor(2);

WidgetLED irDetected(V15);

const byte irInputPin = 14;
const byte battInputPin = A0;
const byte redLED = 0;
const byte blueLED = 2;
const float voltageDividor = 0.1145;

char auth[] = "authCode";
char ssid[] = "IoT";
char pass[] = "password";

int prevDist;
int dist;
int error = 1000;

///////
byte maxSpeed;
byte objectDetectDist;
byte drive;
byte normSpeed;

///////

byte driving;
byte turning;
int i = 0;
unsigned long sleepDuration = 0;

unsigned long timer1 = 0;
unsigned long timer2 = 0;

void setup()
{
  pinMode(irInputPin, INPUT_PULLUP);
  pinMode(battInputPin, INPUT);
  pinMode(redLED, OUTPUT);
  pinMode(blueLED, OUTPUT);
  digitalWrite(redLED, HIGH);
  digitalWrite(blueLED, HIGH);

  Serial.begin(9600);

  Blynk.begin(auth, ssid, pass, IPAddress(10, 32, 1, 2), 8080);
  wifi_station_set_hostname("ESP-CrawlerV2");

  ArduinoOTA.onProgress([](unsigned int progress, unsigned int total) {
    digitalWrite(redLED, LOW);
    delay(10);
    digitalWrite(redLED, HIGH);
    digitalWrite(blueLED, HIGH);
    delay(10);
    digitalWrite(blueLED, LOW);
    delay(10);
  });

  ArduinoOTA.setHostname("ESP-CrawlerV2");
  ArduinoOTA.begin();

  AFMS.begin();

  // wifi_set_opmode(NULL_MODE);
  // wifi_set_sleep_type(LIGHT_SLEEP_T);
  // // wifi_set_sleep_type(MODEM_SLEEP_T);
  // wifi_fpm_auto_sleep_set_in_null_mode(1);
  // ESP.deepSleep(10000 * 1000);
  //ESP.deepSleep(0);
  irDetected.off();
}

void loop()
{
  Blynk.run();
  ArduinoOTA.handle();
  driveLogic();

  if (millis() - timer2 > 1000)
  {
    getPing();
    checkIR();
    timer2 = millis();
  }

  if (millis() - timer1 > 10000)
  {
    maint();
    blink();
    timer1 = millis();
  }
}

/////////////////////////////////////////////

void driveLogic()
{
  if (drive)
  {
    if (!driving)
    {
      if (!checkIR)
      {
        leftMotor->run(FORWARD);
        leftMotor->setSpeed(normSpeed);
        rightMotor->run(FORWARD);
        rightMotor->setSpeed(normSpeed);
        turning = 0;
        driving = 1;
        i = 0;
      }
    }

    if (dist < objectDetectDist)
    {
      if (!checkIR())
      {
        leftMotor->run(FORWARD);
        leftMotor->setSpeed(normSpeed - i);
        rightMotor->run(FORWARD);
        rightMotor->setSpeed(normSpeed + i);
        driving = 1;
        turning = 1;
      }

      else
      {
        leftMotor->run(BACKWARD);
        leftMotor->setSpeed(70);
        rightMotor->run(FORWARD);
        rightMotor->setSpeed(70);
        turning = 1;

        while (checkIR())
        {
          delay(0);
        }
        leftMotor->run(FORWARD);
        leftMotor->setSpeed(normSpeed);
        rightMotor->run(FORWARD);
        rightMotor->setSpeed(normSpeed);
        driving = 0;
        turning = 1;
      }
    }

    else
    {
      if (i > 0)
      {
        i -= 2;
      }

      leftMotor->run(FORWARD);
      leftMotor->setSpeed(normSpeed - i);
      rightMotor->run(FORWARD);
      rightMotor->setSpeed(normSpeed + i);

      if (i <= 0)
      {
        turning = 0;
        driving = 0;
      }
    }

    if (turning)
    {
      i++;
    }
  }
}

/////////////////////////////////////////////

void getPing()
{
  prevDist = dist;

  Serial.write(0X55);
  Serial.flush();

  while (Serial.available() < 2)
  {
    delay(0);
  }

  if (Serial.available() >= 2)
  {
    int HighLen = Serial.read();
    int LowLen = Serial.read();
    int pingResult = HighLen * 256 + LowLen;

    if ((pingResult > 1) && (pingResult < 10000))
    {
      dist = pingResult / 10;
    }
    else
    {
      dist = error;
      error++;
    }
  }

  Blynk.virtualWrite(V0, dist);
}

/////////////////////////////////////////////

byte checkIR()
{
  if (!digitalRead(irInputPin))
  {
    irDetected.on();
    // leftMotor->run(RELEASE);
    // rightMotor->run(RELEASE);
    return 1;
  }
  else
  {
    irDetected.off();
    return 0;
  }
}

/////////////////////////////////////////////

void blink()
{
  digitalWrite(blueLED, LOW);
  delay(50);
  digitalWrite(blueLED, HIGH);
}

/////////////////////////////////////////////

void maint()
{
  float battReading = analogRead(battInputPin);
  float battVoltage = (battReading / 1024) / voltageDividor;

  if (battVoltage < 7.0)
  {
    digitalWrite(redLED, LOW);
  }

  int rssi = wifi_station_get_rssi();

  Blynk.virtualWrite(V20, rssi);
  Blynk.virtualWrite(V22, battVoltage);
}

/////////////////////////////////////////////

BLYNK_CONNECTED()
{
  Blynk.syncAll();
}

/////////////////////////////////////////////

BLYNK_WRITE(V10)
{
  int leftMotorSpeed;
  int rightMotorSpeed;

  int x = param[0].asInt();
  int y = param[1].asInt();

  int xLeft = map(x, 0, 500, maxSpeed, 0);
  int xRight = map(x, 500, 1000, 0, maxSpeed);
  int yForward = map(y, 0, 500, maxSpeed, 0);
  int yBackward = map(y, 500, 1000, 0, maxSpeed);

  if (y < 500)
  {
    if (x < 500)
    {
      leftMotorSpeed = constrain(yForward + xRight, 0, maxSpeed);
      rightMotorSpeed = yForward;
    }
    if (x > 500)
    {
      leftMotorSpeed = yForward;
      rightMotorSpeed = constrain(yForward + xLeft, 0, maxSpeed);
    }

    leftMotor->run(FORWARD);
    leftMotor->setSpeed(leftMotorSpeed);
    rightMotor->run(FORWARD);
    rightMotor->setSpeed(rightMotorSpeed);
  }

  if (y > 500)
  {
    if (x < 500)
    {
      leftMotorSpeed = constrain(yBackward + xRight, 0, maxSpeed);
      rightMotorSpeed = yBackward;
    }
    if (x > 500)
    {
      leftMotorSpeed = yBackward;
      rightMotorSpeed = constrain(yBackward + xLeft, 0, maxSpeed);
    }

    leftMotor->run(BACKWARD);
    leftMotor->setSpeed(leftMotorSpeed);
    rightMotor->run(BACKWARD);
    rightMotor->setSpeed(rightMotorSpeed);
  }

  if (x == 500 && y == 500)
  {
    leftMotor->run(RELEASE);
    rightMotor->run(RELEASE);
  }
}

/////////////////////////////////////////////

BLYNK_WRITE(V11)
{
  maxSpeed = param.asInt();
}

/////////////////////////////////////////////

BLYNK_WRITE(V12)
{
  objectDetectDist = param.asInt();
}

/////////////////////////////////////////////

BLYNK_WRITE(V13)
{
  drive = param.asInt();

  if (!drive)
  {
    leftMotor->run(RELEASE);
    rightMotor->run(RELEASE);
  }
}

/////////////////////////////////////////////

BLYNK_WRITE(V14)
{
  normSpeed = param.asInt();
}

/////////////////////////////////////////////

BLYNK_WRITE(V21) // restart chip
{
  byte reset = param.asInt();

  if (reset)
  {
    ESP.restart();
  }
}

/////////////////////////////////////////////

BLYNK_WRITE(V23)
{
  byte sleepNow = param.asInt();

  if (sleepNow)
  {
    // wifi_set_opmode(NULL_MODE);
    // wifi_set_sleep_type(LIGHT_SLEEP_T);
    // // wifi_set_sleep_type(MODEM_SLEEP_T);
    // wifi_fpm_auto_sleep_set_in_null_mode(1);

    ESP.deepSleep(sleepDuration * 60000 * 1000);
    //ESP.deepSleep(0);
  }
}

/////////////////////////////////////////////

BLYNK_WRITE(V24)
{
  sleepDuration = param.asInt();
}