Issues about programming an Arduino Sumo Robot - arduino

Question

I am building a Sumo robot, I am using 3 HCRSO4 ultrasonic sensors and e TRC5000 change color detectors, all this over an Arduino Mega with Two IBT_2 H bridges. I have probles with this code because I don´t know what is not working. Can sombedody give me a hand?
`
#include <BTS7960.h>
#define L_EN 48
#define R_EN 49
#define L_PWM 3
#define R_PWM 4
#define LineaR 41
#define LineaL 27
#define LineaB 32
int ledr=25;
int leda=26;
int Estado=0;
int SLineaR; //sensor de linea delantero derecho
int SLineaL; //sensor de linea delantero izquierdo
int SLineaB; //sensor de linea trasero
#define L_EN_R 50
#define R_EN_R 51
#define L_PWM_R 5
#define R_PWM_R 6
BTS7960 motor1(L_EN, R_EN, L_PWM, R_PWM); //motor izquierdo
BTS7960 motor2(L_EN_R, R_EN_R, L_PWM_R, R_PWM_R); //motor derecho
#include <EasyUltrasonic.h>
int TRIGPIN_F=8;
int ECHOPIN_F=9;
EasyUltrasonic ultrasonic_F; //ultrasonico frontal
int TRIGPIN_L=10;
int ECHOPIN_L=11;
EasyUltrasonic ultrasonic_L; //ultrasonico izquierdo
int TRIGPIN_R=12;
int ECHOPIN_R=13;
EasyUltrasonic ultrasonic_R; //ultrasonico derecho
void setup() {
pinMode(ledr,OUTPUT);
pinMode(leda,OUTPUT);
//ARRANQUE MOTOR
Serial.begin(9600);
motor1.begin();
motor1.enable();
motor2.begin();
motor2.enable();
//ARRANQUE ULTRASÓNICOS
ultrasonic_F.attach(TRIGPIN_F,ECHOPIN_F);
ultrasonic_L.attach(TRIGPIN_L,ECHOPIN_L);
ultrasonic_R.attach(TRIGPIN_R,ECHOPIN_R);
//ARRANQUE SENSORES DE LINEA
pinMode(LineaR, INPUT);
pinMode(LineaL, INPUT);
pinMode(LineaB, INPUT);
delay(5000);
}
void adelante(){
motor1.pwm=255;
motor1.front();
motor2.pwm=255;
motor2.back();
Serial.println("ADELANTE");
}
void izquierda(){
motor1.pwm=255;
motor1.back();
motor2.pwm=255;
motor2.back();
Serial.println("IZQUIERDA");
}
void derecha(){
motor1.pwm=255;
motor1.front();
motor2.pwm=255;
motor2.front();
Serial.println("DERECHA");
}
void reversa(){
motor1.pwm=255;
motor1.back();
motor2.pwm=255;
motor2.front();
Serial.println("REVERSA");
}
void MapeoFrontal(){
SLineaR=digitalRead(LineaR);
SLineaL=digitalRead(LineaL);
SLineaB=digitalRead(LineaB);
int distanceIN_F= ultrasonic_F.getDistanceIN();
int distanceCM_F= convertToCM(distanceIN_F);
Serial.print("DISTANCIA FRONTAL: ");
Serial.println(distanceCM_F);
if (distanceCM_F > 0 && distanceCM_F <=50 && SLineaR == Estado && SLineaL == Estado && SLineaB == Estado){
adelante();
Serial.println("ENCONTRE ALGO ADELANTE");
}
}
void MapeoIzquierda(){
SLineaR=digitalRead(LineaR);
SLineaL=digitalRead(LineaL);
SLineaB=digitalRead(LineaB);
int distanceIN_L= ultrasonic_L.getDistanceIN();
int distanceCM_L= convertToCM(distanceIN_L);
Serial.print("DISTANCIA IZQUIERDA: ");
Serial.println(distanceCM_L);
if (distanceCM_L > 0 && distanceCM_L <=50 && SLineaR == Estado && SLineaL == Estado && SLineaB == Estado){
izquierda();
Serial.println("ENCONTRE ALGO A LA IZQUIERDA");
}
}
void MapeoDerecha(){
SLineaR=digitalRead(LineaR);
SLineaL=digitalRead(LineaL);
SLineaB=digitalRead(LineaB);
int distanceIN_R= ultrasonic_R.getDistanceIN();
int distanceCM_R= convertToCM(distanceIN_R);
Serial.print("DISTANCIA DERECHA: : ");
Serial.println(distanceCM_R);
if (distanceCM_R > 0 && distanceCM_R <=50 && SLineaR == Estado && SLineaL == Estado && SLineaB == Estado){
derecha();
Serial.println("ENCONTRE ALGO A LA DERECHA");
}
}
void LineasFrente(){
SLineaR=digitalRead(LineaR);
SLineaL=digitalRead(LineaL);
if(SLineaR ==Estado || SLineaL ==Estado){
reversa();
Serial.println("LINEA BLANCA AL FRENTE");
}
}
void LineaTrasera(){
SLineaB=digitalRead(LineaB);
if(SLineaB !=Estado){
adelante();
Serial.println("LINEA BLANCA ATRÁS");
}
}
void Busqueda(){
int SLineaR=0;
int SLineaL=0;
int SLineaB=0;
SLineaR=digitalRead(LineaR);
SLineaL=digitalRead(LineaL);
SLineaB=digitalRead(LineaB);
int distanceIN_F= ultrasonic_F.getDistanceIN();
int distanceCM_F= convertToCM(distanceIN_F);
int distanceIN_L= ultrasonic_L.getDistanceIN();
int distanceCM_L= convertToCM(distanceIN_L);
int distanceIN_R= ultrasonic_R.getDistanceIN();
int distanceCM_R= convertToCM(distanceIN_R);
if(distanceCM_R>80 && distanceCM_F>80 && distanceCM_L>80 && SLineaR == Estado && SLineaL == Estado && SLineaB==Estado);
Serial.println("Buscando");
izquierda2();
delay(50);
adelante2();
delay(100);
derecha2();
delay(50);
reversa2();
delay(100);
}
void luz(){
digitalWrite(ledr,HIGH);
digitalWrite(leda,LOW);
delay(70);
digitalWrite(ledr,LOW);
digitalWrite(leda,HIGH);
delay(70);
}
void loop() {
luz();
delay(1);
Busqueda();
delay(1);
MapeoFrontal();
delay(1);
MapeoIzquierda();
delay(1);
MapeoDerecha();
delay(1);
LineasFrente();
delay(1);
LineaTrasera();
delay(1);
adelante();
delay(5000);
reversa();
delay(5000);
}
`
I have already checked the wires, everything is well conected, every sensor was tested and each of the are working perfectly.
This image show the output in the serial monitor
I am expecting that somebody finds the erros I cant see and try to fix my code or someonte that help me building a new code that workes only with the front ultrasonic sensor and the change colors detectors.

Answer 1

You'll need to provide a lot more detail into the problem you want the community to help you solve. We can't debug your entire program that you say "is not working" without any details about what is not working.
I suggest you start with the first function you believe is not working properly and provide a minimally reproducible example.