#include <Ultrasonic.

h>
#include <Servo.h>

#define MOTOR_1_ADELANTE 9
#define MOTOR_1_ATRAS 10
#define MOTOR_2_ADELANTE 5
#define MOTOR_2_ATRAS 6

#define TRIG 3
#define ECHO 2

#define MY_SERVO 4
#define OBSTACULO_DISTANCIA 30.0
#define GIRO_DELAY 200

#define LOG false

Ultrasonic ultrasonic(TRIG, ECHO);

Servo myservo;
int vel = 255;
int servoAngulo = 90;
int anguloPaso = 30;
int estado = 'g';
float distancia = 0;
const int led = 8;

void setup()
{
Serial.begin(9600);
pinMode(MOTOR_1_ADELANTE, OUTPUT);
pinMode(MOTOR_1_ATRAS, OUTPUT);
pinMode(MOTOR_2_ADELANTE, OUTPUT);
pinMode(MOTOR_2_ATRAS, OUTPUT);
pinMode (led, OUTPUT);
pararMovimiento();
myservo.attach(MY_SERVO);
if(LOG) Serial.begin( 9600 );
}
void loop(){
if(Serial.available()>0){
estado = Serial.read();
}
if(estado=='a'){
irAdelante();
}
if(estado=='b'){
irIzquierda();
}
if(estado=='c'){
pararMovimiento();
}
if(estado=='d'){
irDerecha();
}
if (estado=='e'){
irAtras();
}
if (estado =='f'){
 actualizacionAngulo();
medirDistancia();
movimiento();
delay(10);
}
if(estado=='g'){
pararMovimiento();
}
if(estado=='m'){
ledParpadeante();
}
}
void medirDistancia()
{
distancia = ultrasonic.Ranging(CM);
if(LOG) Serial.println(distancia);
}
void movimiento()
{
if( distancia > OBSTACULO_DISTANCIA )
{
if(LOG) Serial.println("FORWARD");
irAdelante();
delay(GIRO_DELAY);
}
else
{
pararMovimiento();
checkObstracle();
}
}
void checkObstracle()
{
int obsLeft = 0;
int obsRight = 0;

for(servoAngulo = 0; servoAngulo <= 180; servoAngulo += 30)

{
myservo.write(servoAngulo);
delay(GIRO_DELAY);

medirDistancia();
if(distancia < OBSTACULO_DISTANCIA && servoAngulo < 90) obsLeft++;
else if(distancia < OBSTACULO_DISTANCIA) obsRight++;
}
if(LOG) Serial.print("TURN");
if(obsLeft && obsRight)
{
irAtras();
delay(GIRO_DELAY * 5);
if(obsLeft < obsRight) irIzquierda();
else irDerecha();
delay(GIRO_DELAY * 7);
}
else if(obsRight)
{
irIzquierda();
delay(GIRO_DELAY * 7);
}
 else if(obsLeft)
{
irDerecha();
delay(GIRO_DELAY);
}
else
{
irAdelante();
delay(GIRO_DELAY);
}
}
void actualizacionAngulo()
{
myservo.write(servoAngulo);
servoAngulo += anguloPaso;
if(servoAngulo >= 150)
{
servoAngulo = 150;
anguloPaso *= -1;
}
if(servoAngulo <= 30)
{
servoAngulo = 30;
anguloPaso *= -1;
}
}
void irAdelante()
{
analogWrite(MOTOR_1_ADELANTE, vel);
analogWrite(MOTOR_1_ATRAS, 0);
analogWrite(MOTOR_2_ADELANTE, vel);
analogWrite(MOTOR_2_ATRAS, 0);
}
void irAtras()
{
analogWrite(MOTOR_1_ADELANTE, 0);
analogWrite(MOTOR_1_ATRAS, vel);
analogWrite(MOTOR_2_ADELANTE, 0);
analogWrite(MOTOR_2_ATRAS, vel);
}
void irIzquierda()
{
analogWrite(MOTOR_1_ADELANTE, 0);
analogWrite(MOTOR_1_ATRAS, vel);
analogWrite(MOTOR_2_ADELANTE, vel);
analogWrite(MOTOR_2_ATRAS, 0);
}
void irDerecha()
{
analogWrite(MOTOR_1_ADELANTE, vel);
analogWrite(MOTOR_1_ATRAS, 0);
analogWrite(MOTOR_2_ADELANTE, 0);
analogWrite(MOTOR_2_ATRAS, vel);
}
void pararMovimiento()
{
analogWrite(MOTOR_1_ADELANTE, 0);
analogWrite(MOTOR_1_ATRAS, 0);
analogWrite(MOTOR_2_ADELANTE, 0);
 analogWrite(MOTOR_2_ATRAS, 0);
}
void ledParpadeante()
{
digitalWrite(led,HIGH);
delay(50);
digitalWrite(led,LOW);
delay(50);
digitalWrite(led,HIGH);
delay(50);
digitalWrite(led,LOW);
delay(50);
digitalWrite(led,HIGH);
delay(50);
digitalWrite(led,LOW);
delay(800);
}