#include 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
