Robot de lucha (sumo)

Buenas estoy haciendo un robot de sumo con arduino UNO, tiene dos sensores de ultrasonidos, 4 CNY70 dos pilas de 9V que alimentan al controlador de motores y otra de 9 voltios que alimenta arduino, lo que debería hacer es buscar otro robot en un circulo sin sobresalirse de este mismo el problema que tengo es que hasta el momento mi robot solo hace la maniobra de buscar a otros robots, pero no los busca.

Es decir el robot se mueve en busca de ellos pero parece que los sensores no detectaran ningún objeto, llegue a pensar que los sensores de ultrasonidos estaban dañados y los comprobé con otro programa, pero con el serial print puedo ver perfectamente como me da la lectura de la distancia sin ningún tipo de problema, los CNY70 también funcionan sin ningún tipo de problema.

Así que no es el hardware sino el software, estuve mirando la programación durante horas y quitando condiciones para facilitarle las cosas a Arduino. También pense que podrían ser los delays así que fui quitando hasta más no poder, pero aún y así todavía quedan muchos los que creo que son totalmente necesarios.

Agradecería vuestra ayuda ya que soy relativamente nuevo en esto de arduino.

Os dejo mi programa aquí:

P.D. Gracias de antemano!

/*Sumobot sobre suelo blanco y círculo negro
el valor de color negro en los sensores infrarrojos
es de 480 .Se usó la comunicación serial para obtener estos valores.
*/
//Declaración de los pines
int IN1 = 5;    // Input1 conectada al pin 5
int IN2 = 6;    // Input2 conectada al pin 6 MOTOR DERECHO
int ENA = 10;    // ENB conectada al pin 12 de Arduino

int IN4 = 3;    // Input3 conectada al pin 3
int IN3 = 4;    // Input4 conectada al pin 4 MOTOR IZQUIERDO
int ENB = 11;    // ENB conectada al pin 11 de Arduino

int sensFs = 7; //sensor frontal delantero salida del pulso trig 
int sensFe = 8; //sensor frontal delantero entrada del pulso

int sensTs = 9; // sensor trasero salida del pulso trig 
int sensTe = 12; //sensor trasero entrada del pulso

int sensDeli = 0; //sensor IR delantero
int sensDeld = 2; //sensor IR delantero

int sensTrasi = 1; //sensor IR trasero
int sensTrasd = 13; //sensor IR trasero

long distancia;
long tiempo;


//Declaración de I/O's
void setup()
{

pinMode (IN1, OUTPUT);
pinMode (IN2, OUTPUT); //Motor derecho
pinMode (ENA, OUTPUT); 

pinMode (IN3, OUTPUT); 
pinMode (IN4, OUTPUT); //Motor izquierdo
pinMode (ENB, OUTPUT); 

pinMode (sensFs, OUTPUT);  /*activación del pin 7 como salida: para el pulso ultrasónico*/
pinMode (sensFe, INPUT);   /*activación del pin 8 como entrada: tiempo del rebote del ultrasonido*/

pinMode (sensTs, OUTPUT);  /*activación del pin 9 como salida: para el pulso ultrasónico*/
pinMode (sensTe, INPUT);   /*activación del pin 10 como entrada: tiempo del rebote del ultrasonido*/

pinMode (sensDeli, INPUT);  /*activación del pin 0 como salida*/
pinMode (sensDeld, INPUT);  /*activación del pin 2 como salida*/

pinMode (sensTrasi, INPUT); /*activación del pin 1 como salida*/
pinMode (sensTrasd, INPUT); /*activación del pin 13 como salida*/

}



void loop()
{

  // lectura del pin 0
int buttonStateDi = digitalRead(sensDeli);

// imprision del resultado
Serial.println(buttonStateDi);
delay(100);        // delay in between reads for stability

if(buttonStateDi=0){
  
  blancodelanteizq();
  
}/*
else
{
Buscar();
}*/


  // lectura del pin 0
int buttonStateDd = digitalRead(sensDeld);

// imprision del resultado
Serial.println(buttonStateDd);
delay(100);        // delay in between reads for stability

if(buttonStateDd=0){
  
blancodelanteder();
  
}/*
else
{
Buscar();
}*/

{
  // lectura del pin 1
int buttonStateTi = digitalRead(sensTrasi);

// imprision del resultado
Serial.println(buttonStateTi);
delay(100);        // delay in between reads for stability

if(buttonStateTi=1){
  
 blancoatrasizquierda();
  
}/*
else

{

Buscar();

}*/

{
  // lectura del pin 1
int buttonStateTd = digitalRead(sensTrasd);

// imprision del resultado
Serial.println(buttonStateTd);
delay(100);        // delay in between reads for stability

if(buttonStateTd=1){
  
 blancoatrasderecha();
  
}/*
else

{

Buscar();

}*/

//----------- ACCIONES IMPACTO-------------

digitalWrite(sensFs,LOW); /* Por cuestión de estabilización del sensor*/
delayMicroseconds(5);
digitalWrite(sensFs, HIGH); /* envío del pulso ultrasónico trig*/
delayMicroseconds(10);
tiempo=pulseIn(sensFe, HIGH); /* Función para medir la longitud del pulso entrante. Mide el tiempo que transcurrido entre el envío
del pulso ultrasónico y cuando el sensor recibe el rebote, es decir: desde que el pin 12 empieza a recibir el rebote, HIGH, hasta que
deja de hacerlo, LOW, la longitud del pulso entrante*/
distancia= int(0.017*tiempo); /*fórmula para calcular la distancia obteniendo un valor entero*/
/*Monitorización en centímetros por el monitor serial*/
Serial.println("Distancia ");
Serial.println(distancia);
Serial.println(" cm");
delay(100);

if(distancia< 25)
{
golpeFder(); 
}
else 
{
Buscar();
}



digitalWrite(sensTs,LOW); /* Por cuestión de estabilización del sensor*/
delayMicroseconds(5);
digitalWrite(sensTs, HIGH); /* envío del pulso ultrasónico trig*/
delayMicroseconds(10);
tiempo=pulseIn(sensTe, HIGH); /* Función para medir la longitud del pulso entrante. Mide el tiempo que transcurrido entre el envío
del pulso ultrasónico y cuando el sensor recibe el rebote, es decir: desde que el pin 12 empieza a recibir el rebote, HIGH, hasta que
deja de hacerlo, LOW, la longitud del pulso entrante*/
distancia= int(0.017*tiempo); /*fórmula para calcular la distancia obteniendo un valor entero*/
/*Monitorización en centímetros por el monitor serial*/
Serial.println("Distancia ");
Serial.println(distancia);
Serial.println(" cm");
delay(1000);

if(distancia< 25) 
{
golpeTder(); 
}/*
else 
{
Buscar();
}*/


/*
if(digitalRead (sensFizq)==HIGH)
{
golpeFizq(); 
}
else 
{
Buscar();
}
*/

/*
if(digitalRead (sensFizq)==HIGH)
{
golpeTizq();
} 
else 
{
Buscar();
}
*/

}
}
}

//---------------MOVIMIENTOS--------------
int adelante () {
digitalWrite (IN3, HIGH);
digitalWrite (IN4, LOW); //motor izquierdo
// Aplicamos PWM al pin ENB, haciendo girar el motor, cada 0.2 seg aumenta la velocidad
/*analogWrite(ENB,70);
delay(2);
analogWrite(ENB,120);
delay(2);*/
analogWrite(ENB,160);
/*delay(2);
analogWrite(ENB,255);
delay(2000);*/

digitalWrite (IN1, HIGH);
digitalWrite (IN2, LOW); //motor derecho
// Aplicamos PWM al pin ENB, haciendo girar el motor, cada 0.2 seg aumenta la velocidad
/*analogWrite(ENA,70);
delay(2);
analogWrite(ENA,120);
delay(2);*/
analogWrite(ENA,160);
/*delay(2);
analogWrite(ENA,255);
delay(2000);*/
}

int atras()
{
digitalWrite (IN3, LOW);
digitalWrite (IN4, HIGH); //motor izquierdo
// Aplicamos PWM al pin ENB, haciendo girar el motor, cada 0.2 seg aumenta la velocidad
/*analogWrite(ENB,70);
delay(2);
analogWrite(ENB,120);
delay(2);*/
analogWrite(ENB,160);
/*delay(2);
analogWrite(ENB,255);
delay(2000);*/

/*digitalWrite (IN1, LOW);
digitalWrite (IN2, HIGH); //motor derecho
// Aplicamos PWM al pin ENB, haciendo girar el motor, cada 0.2 seg aumenta la velocidad
 analogWrite(ENA,70);
delay(2);
analogWrite(ENA,120);
delay(2);*/
analogWrite(ENA,160);
/*delay(2);
analogWrite(ENA,255);
delay(2000);*/
}

int izquierda()
{

delay(100);
digitalWrite (IN3, LOW);
digitalWrite (IN4, HIGH); //motor izquierdo
// Aplicamos PWM al pin ENB, haciendo girar el motor, cada 0.2 seg aumenta la velocidad
/*analogWrite(ENB,70);
delay(2);
analogWrite(ENB,120);
delay(2);*/
analogWrite(ENB,100);
/*delay(2);
analogWrite(ENB,255);
delay(2000);*/

digitalWrite (IN1, HIGH);
digitalWrite (IN2, LOW); //motor derecho
// Aplicamos PWM al pin ENB, haciendo girar el motor, cada 0.2 seg aumenta la velocidad
/*analogWrite(ENA,70);
delay(2);
analogWrite(ENA,120);
delay(2);*/
analogWrite(ENA,100);
/*delay(2);
analogWrite(ENA,255);
delay(2000);*/
}

int derecha()
{
digitalWrite (IN3, HIGH);
digitalWrite (IN4, LOW); //motor izquierdo
// Aplicamos PWM al pin ENB, haciendo girar el motor, cada 0.2 seg aumenta la velocidad
/*analogWrite(ENB,70);
delay(2);
analogWrite(ENB,120);
delay(2);*/
analogWrite(ENB,100);
/*delay(2);
analogWrite(ENB,255);
delay(2000);*/

digitalWrite (IN1, LOW);
digitalWrite (IN2, HIGH); //motor derecho
// Aplicamos PWM al pin ENB, haciendo girar el motor, cada 0.2 seg aumenta la velocidad
/*analogWrite(ENA,70);
delay(2);
analogWrite(ENA,120);
delay(2);*/
analogWrite(ENA,100);
/*delay(2);
analogWrite(ENA,255);
delay(2000);*/
}

La programación continua aquí:

//--------------GOLPES-------------------

int Buscar()
{
izquierda();
delay(2000);
adelante();
delay(2000);
}


int golpeFder()
{
adelante();
delay(2000);
/*adelante();
delay(2000);
atras();
delay(2000);*/

}
/*
int golpeFizq()
{
atras();
delay(1000) ;
izquierda();
delay(300);
adelante();
}*/

int golpeTder()
{
atras();
delay(2000);
/*atras();
delay(2000);
adelante();
delay(2000);*/
}

/*
int golpeTizq()
{
adelante();
delay(1000);
derecha();
delay(300);
atras();
}*/

int blancodelanteizq()
{
atras();
delay(1000);
derecha();
delay(2000);
adelante();
delay(1000);
}

int blancodelanteder()
{
atras();
delay(1000);
izquierda();
delay(2000);
adelante();
delay(1000);
}

int blancoatrasizquierda()
{
adelante();
delay(1000);
derecha();
delay(2000);
adelante();
delay(1000);
}

int blancoatrasderecha()
{
adelante();
delay(1000);
izquierda();
delay(2000);
adelante();
delay(1000);
}

Francesc ya que esto es un proyecto sería bueno que leyeras las normas del foro para saber como nos manejamos en el foro.
Los códigos no se insertan como tu has posteado. Se usan tags, seleccionas todo el códigoi y luego usas </> para que luzcan de otro modo. Conservando espacios y haciendo mas fácil su lectura. Te invito a que los leas y puedas editar tu post#1.

Respecto de tu proyecto sería interesante que agregues algun diagrama de tu robot, sea como sea, nosotros debemos ver todo el contexto para comprender que hace tu programa. Ahora luce como descolgado una cosa de otra.
Luego un robot debe reaccionar rápido mas alla de la demora en algunas acciones pero no puedes usar delay(2000) a voluntad y esperas a que reaccione a algo porque es como que le pones pausa al programa cada 2 segundos, entiendes…
Escribe otro sketch y ponle carteles por el serial intercalados con delays cada 2 segundos 2000 mseg y verás de que hablo.

Debes ver el ejemplo BlinkWithoutDelay del IDE o en Documentación mira el post de MaxSaeta sobre millis()
Y luego modificar tu código para que haga lo que pretendes pero respondiendo a todo loque pasa a su alrededor.