#include <Servo.

h>
Servo servito;
int echoPin = 2; // SRF05 echo pin (digital 2)
int initPin = 3; // SRF05 trigger pin (digital 3)
int SS4Pin=A0; // cuadrante 4
int SS1Pin=A1; // cuadrante 1
int SS3Pin=A2; // cuadrante 3
int SS2Pin=A3; // cuadrante 2
int S4Pin=0;
int S1Pin=0;
int S3Pin=0;
int S2Pin=0;
int MOTDaPin=10;
int MOTDbPin=11;
int MOTIaPin=13;
int MOTIbPin=12;
int i=0;
unsigned long pulseTime = 0; // stores the pulse in Micro Sec
onds
unsigned long distance = 0;
unsigned long distancia = 50;
int angulo=0;
int sentido=0;
int LED = 8; // Red LED, connected to digital PWM p
in 8

long ultrasonico(){
digitalWrite(initPin, HIGH); // send 10 microsecond pulse
delayMicroseconds(10); // wait 10 microseconds before t
urning off
digitalWrite(initPin, LOW); // stop sending the pulse
pulseTime = pulseIn(echoPin, HIGH); // Look for a return pulse, it s
hould be high as the pulse goes low-high-low
distance = pulseTime/58; // Distance = pulse time / 58 to
convert to cm.
return (distance);
}
void localizacion(){
distancia=ultrasonico();
leerSensor();
while(distancia>=20&&S1Pin<=400&&S2Pin<=400&&S3Pin<=400&&S4Pin<=400){
leerSensor();
distancia=ultrasonico();
digitalWrite(LED,LOW);
if(sentido==0){
servito.write(angulo);
angulo=angulo+10;
if(angulo==180)
sentido=1;
}
if(sentido==1){
 servito.write(angulo);
angulo=angulo-10;
if(angulo==0)
sentido=0;
}
delay(75);
adelante();
}
preguntar();
ataque();
}
////////////////////////////////////////////////MOTOREEEEEEES///////////////////
//////////////////////////////////
void izquierda1(){
digitalWrite(MOTIaPin,LOW);
digitalWrite(MOTIbPin,LOW);
digitalWrite(MOTDaPin,HIGH);
digitalWrite(MOTDbPin,LOW);
delay(100);
}
void izquierda2(){
digitalWrite(MOTIaPin,LOW);
digitalWrite(MOTIbPin,HIGH);
digitalWrite(MOTDaPin,HIGH);
digitalWrite(MOTDbPin,LOW);
delay(100);
}
void derecha1(){
digitalWrite(MOTIaPin,HIGH);
digitalWrite(MOTIbPin,LOW);
digitalWrite(MOTDaPin,LOW);
digitalWrite(MOTDbPin,LOW);
delay(100);
}
void derecha2(){
digitalWrite(MOTIaPin,HIGH);
digitalWrite(MOTIbPin,LOW);
digitalWrite(MOTDaPin,LOW);
digitalWrite(MOTDbPin,HIGH);
delay(100);
}
void contrataque(){
for(i=0;i<4;i++){
digitalWrite(MOTIaPin,HIGH);
digitalWrite(MOTIbPin,LOW);
digitalWrite(MOTDaPin,LOW);
digitalWrite(MOTDbPin,HIGH);
delay(200);
digitalWrite(MOTIaPin,LOW);
digitalWrite(MOTIbPin,HIGH);
digitalWrite(MOTDaPin,HIGH);
digitalWrite(MOTDbPin,LOW);
delay(200);
}
digitalWrite(MOTIaPin,HIGH);
digitalWrite(MOTIbPin,LOW);
digitalWrite(MOTDaPin,HIGH);
digitalWrite(MOTDbPin,LOW);
delay(500);
}
void adelante(){
digitalWrite(MOTIaPin,HIGH);
digitalWrite(MOTIbPin,LOW);
digitalWrite(MOTDaPin,HIGH);
digitalWrite(MOTDbPin,LOW);
}

/////////////////////////////////////////ATAQUEEEEEE////////////////////////////
///////////////////////////////////
void ataque(){
//while(millis()<1000){
while(S1Pin<=400&&S2Pin<=400&&S3Pin<=400&&S4Pin<=400){
if(angulo==90){
servito.write(90);
while(ultrasonico()>20){ // WHILE PENDIENTE X ROMPER
derecha2();
ultrasonico();
}
adelante();
}
if(angulo>90){
servito.write(90);
while(ultrasonico()>30){ // WHILE PENDIENTE X ROMPER
izquierda2();
ultrasonico();
}
adelante();
}
if(angulo<90){
servito.write(90);
while(ultrasonico()>30){ // WHILE PENDIENTE X ROMPER
derecha2();
ultrasonico();
}
adelante();
}
leerSensor();
digitalWrite(LED,HIGH);
}
//previousMillis=0;
//}
}
/////////////////////////////////////////PREGUNTAAA/////////////////////////////
///////////////////////////////////
void preguntar(){
if(S1Pin<=400&&S2Pin<=400&&S3Pin<=400&&S4Pin>400){
izquierda1();
}
if(S1Pin>400&&S2Pin<=400&&S3Pin<=400&&S4Pin<=400){
 izquierda2();
}
if(S1Pin>400&&S2Pin<=400&&S3Pin<=400&&S4Pin>400){
izquierda2();
}
if(S1Pin<=400&&S2Pin>400&&S3Pin<=400&&S4Pin<=400){
derecha2();
}
if(S1Pin<=400&&S2Pin>400&&S3Pin<=400&&S4Pin>400){
izquierda2();
}
if(S1Pin>400&&S2Pin>400&&S3Pin<=400&&S4Pin<=400){
izquierda2();
}
if(S1Pin<=400&&S2Pin<=400&&S3Pin>400&&S4Pin<=400){
derecha1();
}
if(S1Pin<=400&&S2Pin<=400&&S3Pin>400&&S4Pin>400){
contrataque();
}
if(S1Pin>400&&S2Pin<=400&&S3Pin>400&&S4Pin<=400){
derecha2();
}
if(S1Pin<=400&&S2Pin>400&&S3Pin>400&&S4Pin<=400){
derecha2();
}
}
void leerSensor(){
S1Pin=analogRead(SS1Pin);
S2Pin=analogRead(SS2Pin);
S3Pin=analogRead(SS3Pin);
S4Pin=analogRead(SS4Pin);
}
void setup() {
servito.attach(9);
pinMode(LED, OUTPUT); // sets pin 9 as output
pinMode(initPin, OUTPUT); // set init pin 3 as output
pinMode(echoPin, INPUT); // set echo pin 2 as input
pinMode(S1Pin, INPUT);
pinMode(S2Pin, INPUT);
pinMode(S3Pin, INPUT);
pinMode(S4Pin, INPUT);
delay(1000);
}
// execute
void loop()
{
localizacion();
}