#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 4int SS1Pin=A1; // cuadrante 1int SS3Pin=A2;
// cuadrante 3int SS2Pin=A3; // cuadrante 2int 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 Seconds
unsigned long distance = 0; unsigned long distancia = 50; int angulo=0;
int sentido=0;
int LED = 8; // Red LED, connected to digital PWM pin 8
long ultrasonico() { digitalWrite(initPin, HIGH) ; // send 10 microsecond pulse
delayMicroseconds(10) ; // wait 10 microseconds before turning off
digitalWrite(initPin, LOW) ; // stop sending the pulsepulseTime = pulseIn(echoPin,
HIGH) ; // Look for a return pulse, it should 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() ;
}