Sonsonmóvel com variação de velocidade proporcional

#include "Ultrasonic.h"
#define echoPin 10
#define trigPin 9
#define Motor_E1 8
#define Motor_E2 7
#define Motor_EV 6
#define Motor_DV 5
#define Motor_D2 4
#define Motor_D1 3
#define inPin 2
#define Velocidade 120
#define Dif 30
#define Divisor_inicial 2
#define Taxa_divisor 0.1

int valor = 0;
float divisor = 2.0;
bool lado = 0, flag = 0; // 0 - esq, 1 - dir

Ultrasonic ultrasonic(9,10);

void desviaObstaculo()
{
  analogWrite(Motor_EV, 0);
  analogWrite(Motor_DV, 0);

  delay(400);

  analogWrite(Motor_EV, Velocidade-Dif);
  analogWrite(Motor_DV, Velocidade);

  // INDO PRA ESQUERDA
  digitalWrite(Motor_D1, LOW);
  digitalWrite(Motor_D2, HIGH);
  digitalWrite(Motor_E1, LOW);
  digitalWrite(Motor_E2, LOW);

  // PASSA 200 MS SE POSICIONANDO
  delay(700);

  // INDO PRA FRENTE
  digitalWrite(Motor_D1, LOW);
  digitalWrite(Motor_D2, HIGH);
  digitalWrite(Motor_E1, LOW);
  digitalWrite(Motor_E2, HIGH);

  // PASSA 500 MS INDO P FRENTE
  delay(400);

  analogWrite(Motor_EV, 0);
  analogWrite(Motor_DV, 0);

  delay(400);

  analogWrite(Motor_EV, Velocidade-Dif);
  analogWrite(Motor_DV, Velocidade);


  // INDO PRA DIREITA
  digitalWrite(Motor_D1, LOW);
  digitalWrite(Motor_D2, LOW);
  digitalWrite(Motor_E1, LOW);
  digitalWrite(Motor_E2, HIGH);

  // PASSA 200 MS SE POSICIONANDO
  delay(500);

  // INDO PRA FRENTE
  digitalWrite(Motor_D1, LOW);
  digitalWrite(Motor_D2, HIGH);
  digitalWrite(Motor_E1, LOW);
  digitalWrite(Motor_E2, HIGH);

  delay(800);


  analogWrite(Motor_EV, 0);
  analogWrite(Motor_DV, 0);

  delay(400);

  analogWrite(Motor_EV, Velocidade-Dif);
  analogWrite(Motor_DV, Velocidade);

  // INDO PRA DIREITA
  digitalWrite(Motor_D1, LOW);
  digitalWrite(Motor_D2, LOW);
  digitalWrite(Motor_E1, LOW);
  digitalWrite(Motor_E2, HIGH);

  // PASSA 200 MS SE POSICIONANDO
  delay(500);

  // INDO PRA FRENTE
  digitalWrite(Motor_D1, LOW);
  digitalWrite(Motor_D2, HIGH);
  digitalWrite(Motor_E1, LOW);
  digitalWrite(Motor_E2, HIGH);

  delay(450);
}

void seguePista()
{
  if (lado)
  {
    // INDO PRA DIREITA
    digitalWrite(Motor_D1, LOW);
    digitalWrite(Motor_D2, HIGH);
    digitalWrite(Motor_E1, LOW);
    digitalWrite(Motor_E2, HIGH); //

    analogWrite(Motor_EV, (int) (Velocidade-Dif)*((divisor - Divisor_inicial) * Divisor_inicial * 10);
    analogWrite(Motor_DV, (int) Velocidade/divisor);

    if (valor == HIGH)
    {
      if (!flag)
      {
          lado ^= 1;
          flag = 1;
          divisor = Divisor_inicial;
      }
      else
      {
        divisor += Taxa_divisor;
      }
    }
      else
        flag = 0;
  }
  else
  {
    // INDO PRA ESQUERDA
    digitalWrite(Motor_D1, LOW);
    digitalWrite(Motor_D2, HIGH); //
    digitalWrite(Motor_E1, LOW);
    digitalWrite(Motor_E2, HIGH);

    analogWrite(Motor_EV, (int)  (Velocidade-dif)/divisor);
    analogWrite(Motor_DV, (int) Velocidade * ((divisor - Divisor_inicial) * Divisor_inicial * 10);


    if (valor == HIGH)
    {
      if (!flag)
      {
          lado ^= 1;
          flag = 1;
          divisor = Divisor_inicial;
      }
    }
      else
        flag = 0;
        divisor += Taxa_divisor;
  }
}

void setup()
{
   pinMode(Motor_E1, OUTPUT);
   pinMode(Motor_E2, OUTPUT);
   pinMode(Motor_EV, OUTPUT);
   pinMode(Motor_DV, OUTPUT);
   pinMode(Motor_D2, OUTPUT);
   pinMode(Motor_D1, OUTPUT);

   pinMode(echoPin, INPUT);
   pinMode(trigPin, OUTPUT);

   Serial.begin(9600);

   pinMode(inPin, INPUT);
}

void loop()
{
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  int distancia = (ultrasonic.Ranging(CM));
  valor = digitalRead(inPin); //realizando a leitura no sensor optico

  if (distancia <= 15 && valor == LOW)
  {
    desviaObstaculo();
  }
  else
  {
    seguePista();
  }
}