#include <Servo.

h>
Servo servito;
int echoPin = 2;
int initPin = 3;
int
int
int
int

SS4Pin=A0;
SS1Pin=A1;
SS3Pin=A2;
SS2Pin=A3;

int
int
int
int

S4Pin=0;
S1Pin=0;
S3Pin=0;
S2Pin=0;

int
int
int
int

MOTDaPin=10;
MOTDbPin=11;
MOTIaPin=13;
MOTIbPin=12;

// SRF05 echo pin (digital 2)

// SRF05 trigger pin (digital 3)
//
//
//
//

cuadrante
cuadrante
cuadrante
cuadrante

4
1
3
2

int i=0;
unsigned long pulseTime = 0;
onds
unsigned long distance = 0;
unsigned long distancia = 50;
int angulo=0;
int sentido=0;
int LED = 8;
in 8

// stores the pulse in Micro Sec

// Red LED, connected to digital PWM p

long ultrasonico(){
digitalWrite(initPin, HIGH);
delayMicroseconds(10);
urning off
digitalWrite(initPin, LOW);
pulseTime = pulseIn(echoPin, HIGH);
hould be high as the pulse goes low-high-low
distance = pulseTime/58;
convert to cm.
return (distance);
}

// send 10 microsecond pulse

// wait 10 microseconds before t
// stop sending the pulse
// Look for a return pulse, it s
// Distance = pulse time / 58 to

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){
derecha2();
ultrasonico();
}
adelante();
}
leerSensor();
digitalWrite(LED,HIGH);

// WHILE PENDIENTE X ROMPER

}
//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);
pinMode(initPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(S1Pin, INPUT);
pinMode(S2Pin, INPUT);
pinMode(S3Pin, INPUT);
pinMode(S4Pin, INPUT);
delay(1000);
}
// execute
void loop()
{
localizacion();
}

// sets pin 9 as output

// set init pin 3 as output
// set echo pin 2 as input