#include<AFMotor.

h>
#include<Ultrasonic.h>
Ultrasonic sonic1(26,27);
Ultrasonic sonic2(30,31);
Ultrasonic sonic3(34,35);
AF_DCMotor motor1(1,MOTOR12_64KHZ);
AF_DCMotor motor2(2,MOTOR12_64KHZ);
const int sensorPin1 = 22;
const int sensorPin2 = 23;
int ldr1=A8;
int ldr2=A9;
void setup() {
motor1.setSpeed(255);
motor2.setSpeed(255);
Serial.begin(9600);
pinMode(sensorPin1,INPUT);
pinMode(sensorPin2,INPUT);

void loop() {
if(Serial.available()){
char aux=Serial.read();
switch(aux){
case'F':
motor1.run(FORWARD);
motor2.run(FORWARD);
motor1.setSpeed(255);
motor2.setSpeed(253);
break;
case'B':
motor1.run(BACKWARD);
motor2.run(BACKWARD);
motor1.setSpeed(255);
motor2.setSpeed(253);
break;
case'R':
motor1.run(BACKWARD);
motor2.run(FORWARD);
motor1.setSpeed(255);
motor2.setSpeed(255);
break;
case'L':
motor1.run(FORWARD);
motor2.run(BACKWARD);
motor1.setSpeed(255);
motor2.setSpeed(255);
break;
case'S':
motor1.run(RELEASE);
motor2.run(RELEASE);
break;
case'W':
seguidorde_linea();
break;
case'U':
resuelve_laverintos();
break;
case'P':
 seguidorde_luz();
break;
}
}
}
void resuelve_laverintos(){
int aux1=sonic1.Ranging(CM);
int aux2=sonic2.Ranging(CM);
int aux3=sonic3.Ranging(CM);
if(aux1<6){
if(aux2<6){
motor1.run(FORWARD);
motor2.run(BACKWARD);
delay(500);
}
else if(aux3<6){
motor1.run(BACKWARD);
motor2.run(FORWARD);
delay(500);
}
}
else{
motor1.run(FORWARD);
motor2.run(FORWARD);
}
}

void seguidorde_luz(){
int valorldr1 =analogRead(ldr1);
int valorldr2 =analogRead(ldr2);
if(valorldr1 >600 && valorldr2 > 600){
motor1.run(FORWARD);
motor2.run(FORWARD);
motor1.setSpeed(255);
motor2.setSpeed(242);
}
else if(valorldr1 <600 && valorldr2 > 600){
motor1.run(BACKWARD);
motor2.run(FORWARD);
motor1.setSpeed(255);
motor2.setSpeed(255);
}
else if(valorldr1 >600 && valorldr2 < 600){
motor1.run(FORWARD);
motor2.run(BACKWARD);
motor1.setSpeed(255);
motor2.setSpeed(255);
}
else if(valorldr1<600 && valorldr2<600){
motor1.run(RELEASE);
motor2.run(RELEASE);
motor1.setSpeed(255);
motor2.setSpeed(245);
}
}

void seguidorde_linea(){
int value1 = 0;
int value2 = 0;
 motor1.setSpeed(150);
motor2.setSpeed(150);
value1 = digitalRead(sensorPin1);
value2 = digitalRead(sensorPin2);
if(value1 == LOW && value2 == LOW){
motor1.run(FORWARD);
motor2.run(FORWARD);
}
if(value1 == LOW && value2 == HIGH){
motor1.run(FORWARD);
motor2.run(RELEASE);
}
else if(value1 == HIGH && value2 == LOW){
motor1.run(RELEASE);
motor2.run(FORWARD);
}
else if(value1 == HIGH && value2 == HIGH){
motor1.run(RELEASE);
motor2.run(RELEASE);
}
}