Coche Autonomo

#include <Servo.h> //servo1 library


Servo servo;
/*Echo/Trig del ultrasonido */
int Echo = A6;  
int Trig = A5;


/*almacena distancia leida por ultrasonido*/
int dist;


/*MOTORES*/
/*define pins para controlar los motores*/
int in1=2;
int in2=4;
int in3=7;
int in4=8;
int in5=10;
int in6=11;
int in7=12;
int in8=13;


/*define pins de enable (PWM). Estos controlan la potencia de los motores*/
int ENA1=6;
int ENB1=9;
int ENA2=3;
int ENB2=5;


/*Calcula y devuelve distancia leida por ultrasonido*/
float Distance_test()  {
  digitalWrite(Trig, LOW);   
  delayMicroseconds(1);
  digitalWrite(Trig, HIGH);  
  delayMicroseconds(10);
  digitalWrite(Trig, LOW);   
  float Fdistance = pulseIn(Echo, HIGH);  
  Fdistance = (Fdistance)/ 58;       
  Serial.print(“distancia: “);
  Serial.println(Fdistance);
  return Fdistance;
} 


/*Barrido del servo con el ultrasonido. Devuelve entero que corresponde con una direccion*/
int Servo_scan() {
    delay(500);
    servo.write(180);
    delay(500);
    dist = Distance_test(); 
    if (dist > 25) {
      servo.write(80);
      delay(1000);
      Serial.println(‘1’);
      return 1;
    } else {
      servo.write(135);
      delay(500);
      dist = Distance_test();
      delay(1000);
      if (dist > 25) {
        servo.write(80);
        delay(1000);
        Serial.println(‘2’);
        return 2;
      } else {       
        servo.write(80);
        delay(500);
        dist = Distance_test();
        delay(1000);
        if (dist > 25) {
          servo.write(80);
          delay(1000);
          Serial.println(‘3’);
          return 3;
        } else {         
          servo.write(40);
          delay(500);
          dist = Distance_test();
          delay(1000);
          if (dist > 25) {
            servo.write(80);
            delay(1000);
            Serial.println(‘4’);
            return 4;
          } else {         
            servo.write(0);
            delay(500);
            dist = Distance_test();
            delay(1000);
            if (dist > 25) {
              servo.write(80);
              delay(1000);
              Serial.println(‘5’);
              return 5;
            } else {
              servo.write(80);
              delay(1000);
              Serial.println(‘6’);
              return 6;
            }
          }
        }
      }
    }
}


/*MOTORES*/
/*retroceso NO EN USO*/
void _mback()
{ 
  analogWrite(ENA1,255);
  analogWrite(ENB1,255);
  analogWrite(ENA2,255);
  analogWrite(ENB2,255);
  
  digitalWrite(in1,HIGH);
  digitalWrite(in2,LOW);  //Right front wheel backward
  
  digitalWrite(in3,LOW);
  digitalWrite(in4,HIGH); //Left front wheel backward
  
  digitalWrite(in5,LOW);
  digitalWrite(in6,HIGH); //Right back wheel backward
  
  digitalWrite(in7,HIGH);
  digitalWrite(in8,LOW);  //Left back wheel backward
  
  Serial.println(“back”);
}


/*avanzar*/
void _mforward()
{
  analogWrite(ENA1,200);
  analogWrite(ENB1,200);
  analogWrite(ENA2,200);
  analogWrite(ENB2,200);
  
  digitalWrite(in1,LOW);
  digitalWrite(in2,HIGH); //Right Front wheel forward
  
  digitalWrite(in3,HIGH);
  digitalWrite(in4,LOW);  //Left Front wheel forward
  
  digitalWrite(in5,HIGH);
  digitalWrite(in6,LOW);  //Right back wheel forward
  
  digitalWrite(in7,LOW);
  digitalWrite(in8,HIGH); //Left back wheel forward
  
  Serial.println(“forward”);
}


/*funcion para girar a la izquierda*/
void _mleft()
{
  analogWrite(ENA1,255);
  analogWrite(ENB1,255);
  analogWrite(ENA2,255);
  analogWrite(ENB2,255);
  
  digitalWrite(in1,LOW);
  digitalWrite(in2,HIGH);
  
  digitalWrite(in3,LOW);
  digitalWrite(in4,HIGH);
  
  digitalWrite(in5,LOW);
  digitalWrite(in6,HIGH);
  
  digitalWrite(in7,LOW);
  digitalWrite(in8,HIGH);
  
  Serial.println(“left”);
}


/*funcion para girar a la derecha*/
void _mright()
{
  analogWrite(ENA1,255);
  analogWrite(ENB1,255);
  analogWrite(ENA2,255);
  analogWrite(ENB2,255);
  
  digitalWrite(in1,HIGH);
  digitalWrite(in2,LOW);
  
  digitalWrite(in3,HIGH);
  digitalWrite(in4,LOW);
  
  digitalWrite(in5,HIGH);
  digitalWrite(in6,LOW);
  
  digitalWrite(in7,HIGH);
  digitalWrite(in8,LOW);
  
  Serial.println(“right”);
}


void _stop()
{
  digitalWrite(ENA1,LOW);
  digitalWrite(ENB1,LOW);
  digitalWrite(ENA2,LOW);
  digitalWrite(ENB2,LOW);
  Serial.println(“Stop”);
}


/*decide el movimiento autonomo del coche*/
void movimientosAuto() {
  dist = Distance_test();   //Lee distancia de frente
  if (dist <= 30) {            
    _stop();                
    gira(Servo_scan());     //Gira en direccion devuelta por el servo scan
    _stop();
  } else {
    gira(3);
  }              
}


/*recibe un entero desde el Servo_scan() que dice en que direccion debe girar el coche*/
void gira(int dir) {
  switch(dir) {
    case 1:
      _mleft();     //gira izq este
      delay(500);   //90 grados
      break;
    case 2:
      _mleft();     //gira izq noreste
      delay(234);   //45 grados
      break;
    case 3:
      _mforward();  //recto
      break;
    case 4:
      _mright();    //gira der noroeste
      delay(250);   //45 grados
      break;
    case 5:
      _mright();    //gira der oeste
      delay(500);  //90 grados
      break;
    case 6:
      _mright();    //gira 180
      delay(935);
  }
  
}


void setup() 
{
  /*Conectamos Servo al pin 30*/
  servo.attach(30);
  /*Declaro el Echo como input y el Trig como output*/
  pinMode(Echo, INPUT);    
  pinMode(Trig, OUTPUT);
  /*MOTOR A1*/
  pinMode(in1,OUTPUT);
  pinMode(in2,OUTPUT);
  pinMode(ENA1,OUTPUT);
  /*Motor B1*/
  pinMode(in3,OUTPUT);
  pinMode(in4,OUTPUT);
  pinMode(ENB1,OUTPUT);
  /*Motor A2*/
  pinMode(in5,OUTPUT);
  pinMode(in6,OUTPUT);
  pinMode(ENA2,OUTPUT);
  /*Motor B2*/
  pinMode(in7,OUTPUT);
  pinMode(in8,OUTPUT);
  pinMode(ENB2,OUTPUT);
  
  Serial.begin(9600);
}


void loop() { 
  movimientosAuto();
  delay(250);
}