Controle Ultrasonico Arduino com Potenciometro.pde

#include <Ultrasonic.h>

#define TRIGGER_PIN  12
#define ECHO_PIN     11

// motor um
int enA = 5;
int in1 = 6;
int in2 = 7;
// motor dois
int enB = 10;
int in3 = 8;
int in4 = 9;
// potenciometro
int pot = A5;

Ultrasonic ultrasonic(TRIGGER_PIN, ECHO_PIN);

void setup()
{
  Serial.begin(9600);
  pinMode(13, OUTPUT);
  
  // saidas dos motores
  pinMode(enA, OUTPUT);
  pinMode(enB, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  
  // potenciometro
  pinMode(pot, INPUT);
}

int liga_motores(int a = 0, int b = 0, int c = 0, int d = 0){
  digitalWrite(in1, a);
  digitalWrite(in2, b);
  digitalWrite(in3, c);
  digitalWrite(in4, d);
}

int velocidadeCorrigida(){
  int vel = 0;
  for (int i = 0; i < 10; i++){
    int vel = analogRead(pot);
    delay(10);
  }
  vel = vel / 10;
  vel = map(analogRead(pot), 0, 1023, 0, 255);
  return vel;
}

void loop()
{
  float cmMsec;
  long microsec = ultrasonic.timing();
  cmMsec = ultrasonic.convert(microsec, Ultrasonic::CM);
  
  if(cmMsec > 50.00){
    // gira motores
    digitalWrite(13, 0);
    liga_motores(0, 0, 1, 0);
  } else if(cmMsec < 20.00){
    // gira motor direita
    digitalWrite(13, 1);
    liga_motores(0, 1, 0, 0);
  } else  {
    // gira os dois motores
    liga_motores(1, 0, 1, 0);
  }
  
  
  analogWrite(enA, vel);
  analogWrite(enB, vel);
  
  Serial.print("MS: ");
  Serial.print(microsec);
  Serial.print(", CM: ");
  Serial.println(cmMsec);
  delay(500);
}