Arduino Leonardo getting ¿overloaded?

Good morning guys.

My project consist in a bot with some functions, follow line, remote control through bluetooth, sumo and labirynth.

By now follow line, R/C and sumo are “done”. Everyone is in a separated sketch and when ended should be in same one but that is another subject.

The problem i am facing is that i want to the bot being stopped when there is something 5 cm or less in its front, usually another bot in the circuit.

I managed to make it work with ultrasounds distance sensors in the sumo mode so this shouldnt be a deal, but for some reason it is.

Seems like the board gets overload if has to be constantly reading CNYs and calculating the distance in the follow line sketch. ¿Why do i say so?, i do it because without distance calculating it works perfectly, but if i add the distance measure without unchanging the original code the bot gets off the line all the time and the comeback function starts but doesnt disable when the CNYs are on the line, it keeps steering.

The weird thing for me is that the sumo mode also works with constant distance measure and CNY reading, the bot does it on a black circular table wth white borders so it goes back when finds the white border preventing it of falling off.

I’ll put the codes below, Follow line without distance measure, with it and the sumo code.

I am a beginner, started a month and half ago to learn programming without previus knowledge so don’t expect them to be nice or efficient. I will be gratefull of any advice, tip and correction of yours.

//-------------------------FOLLOW LINE WITHOUT DISTANCE MEASURE----------------------
/*
 * Trasero Derecha          = A0
 * Trasero Izquierda        = A1
 * Frontal Izquierdo        = A2
 * Frontal Centro-Izquierda = A3
 * Frontal Centro-Derecha   = A4
 * Frontal Derecha          = A5
*/

const int pinServo1=6;                                      
const int pinServo2=9;                                     
const int dirservo1=5;
const int dirservo2=10;
int CNY_CI, CNY_CD;
int funcion;
const int blanco = 150;
const int negro = 350;
const int correccionE = 109; 
const int correccionEi = 109;
boolean girod, giroi;

void setup() {
Serial1.begin(9600);
}  

void cny(){
  CNY_CI = analogRead(A3);
  CNY_CD = analogRead(A4);
}

void serial(){
  Serial1.print("CNY CENTRO IZQUIERDO: ");
  Serial1.print(CNY_CI);
  Serial1.print(" - CNY CENTRO DERECHO: ");
  Serial1.print(CNY_CD);
  Serial1.println();
}

void loop() {
  cny();

  if ((CNY_CI >= negro) & (CNY_CD >= negro)){ // Adelante
    girod = false;
    giroi = false;
    digitalWrite (dirservo1,LOW);     
    digitalWrite (dirservo2,HIGH);   
 
    analogWrite (pinServo1,255);      // Izquierda, 255
    analogWrite (pinServo2,0);       // Derecha, 0
  }

  else if (girod == true) { 
    if (funcion == 1){             // Giro a la Izquierda
      digitalWrite (dirservo1,HIGH); 
      digitalWrite (dirservo2,HIGH);   

      analogWrite (pinServo1,0 + correccionEi);  //0
      analogWrite (pinServo2,255); //0
    }
   
    else if (funcion == 2){        //Giro a la Derecha
      digitalWrite (dirservo1,LOW); 
      digitalWrite (dirservo2,LOW);   
 
      analogWrite (pinServo1,255);                    //255
      analogWrite (pinServo2,255 - correccionE);      //255
    };
  }  
  else if (giroi == true) { 
    if (funcion == 1){            // Giro a la Izquierda.
      digitalWrite (dirservo1,HIGH); 
      digitalWrite (dirservo2,HIGH);   

      analogWrite (pinServo1,0 + correccionEi);      //0
      analogWrite (pinServo2,0);      //0
    }
   
    else if (funcion == 2){       //Giro a la Derecha.
      digitalWrite (dirservo1,LOW); 
      digitalWrite (dirservo2,LOW);   
 
      analogWrite (pinServo1,255);                    //255
      analogWrite (pinServo2,255 - correccionE);      //255
    };
  }

  else if (CNY_CD < (CNY_CI +100)) { // Giro a la Izquierda
    giroi = true;
    funcion = 1;
  }
  
  else if (CNY_CI < (CNY_CD +100)) { // Giro a la Derecha
    girod = true;
    funcion = 2;
  };
}
//-----------------------------FOLLOW LINE WITH DISTANCE MEASURE-------------------------------
/*
 * Trasero Derecha          = A0
 * Trasero Izquierda        = A1
 * Frontal Izquierdo        = A2
 * Frontal Centro-Izquierda = A3
 * Frontal Centro-Derecha   = A4
 * Frontal Derecha          = A5
*/
const int USF=2;
const int pinServo1=6;                                      
const int pinServo2=9;                                     
const int dirservo1=5;
const int dirservo2=10;
int CNY_CI, CNY_CD;
int funcion;
const int blanco = 150;
const int negro = 350;
const int correccionE = 109;
const int correccionEi = 109;
boolean girod, giroi;
long duration, cm;

void setup() {
Serial1.begin(9600);
}  

void cny(){
  CNY_CI = analogRead(A3);
  CNY_CD = analogRead(A4);
}

void distancia(){
  pinMode(USF, OUTPUT);
  digitalWrite(USF, LOW);
  delayMicroseconds(2);
  digitalWrite(USF, HIGH);
  delayMicroseconds(5);
  digitalWrite(USF, LOW);
  pinMode(USF, INPUT);
  duration = pulseIn(USF, HIGH);
  cm = microsecondsToCentimeters(duration);
}

long microsecondsToCentimeters(long microseconds) {
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}

void serial(){
  Serial1.print("CNY CENTRO IZQUIERDO:");
  Serial1.print(CNY_CI);
  Serial1.print(" - CNY CENTRO DERECHO:");
  Serial1.print(CNY_CD);
  Serial1.println();
}

void loop() {
  cny();
  distancia();

  if (cm <5){
    digitalWrite (dirservo1,LOW);     
    digitalWrite (dirservo2,HIGH);   
 
    analogWrite (pinServo1,0);      // Izquierda, 255
    analogWrite (pinServo2,255);       // Derecha, 0
    delay (100);
  }

  if ((CNY_CI >= negro) & (CNY_CD >= negro)){ // Adelante
    girod = false;
    giroi = false;
    digitalWrite (dirservo1,LOW);     
    digitalWrite (dirservo2,HIGH);   
 
    analogWrite (pinServo1,255);      // Izquierda, 255
    analogWrite (pinServo2,0);       // Derecha, 0
  }

  else if (girod == true) { 
    if (funcion == 1){             // Giro a la Izquierda
      digitalWrite (dirservo1,HIGH); 
      digitalWrite (dirservo2,HIGH);   

      analogWrite (pinServo1,0 + correccionEi);  //0
      analogWrite (pinServo2,255); //0
    }
   
    else if (funcion == 2){        //Giro a la Derecha
      digitalWrite (dirservo1,LOW); 
      digitalWrite (dirservo2,LOW);   
 
      analogWrite (pinServo1,255);                    //255
      analogWrite (pinServo2,255 - correccionE);      //255
    };
  }  
  else if (giroi == true) { 
    if (funcion == 1){            // Giro a la Izquierda.
      digitalWrite (dirservo1,HIGH); 
      digitalWrite (dirservo2,HIGH);   

      analogWrite (pinServo1,0 + correccionEi);      //0
      analogWrite (pinServo2,0);      //0
    }
   
    else if (funcion == 2){       //Giro a la Derecha.
      digitalWrite (dirservo1,LOW); 
      digitalWrite (dirservo2,LOW);   
 
      analogWrite (pinServo1,255);                    //255
      analogWrite (pinServo2,255 - correccionE);      //255
    };
  }

  else if (CNY_CD < (CNY_CI +100)) { // Giro a la Izquierda
    giroi = true;
    funcion = 1;
  }
  
  else if (CNY_CI < (CNY_CD +100)) { // Giro a la Derecha
    girod = true;
    funcion = 2;
  };
}

Tried to fix it only measuring when a counter comes to 100, then it does distancia function and if cm <= 5 it should stop, but sometimes this happend in the middle of a corner and again it fucks up everything. I can force it to only do the measure when it is in the straight line but is relatively useless beacause normally the bot crash with each other in the corners not the straight line.
If i use a bigger range it can work but i preffer to dont make it work that way.

I’ll put the sudo code in a post below, if not i exceed the character limit.

Here it comes…

//------------------------------------------SUMO----------------------------------------
#define serial_begin 'p'
const int vel1 = 255;
const int vel2 = 230;
const int vel3 = 205;
const int vel4 = 180;
const int vel5 = 155;
const int vel6 = 130;
const int vel7 = 105;
const int vel8 = 80;
const int vel9 = 55;
const int vel10 = 30;
const int vel11 = 0; 
const int pinServo1=6;                                      
const int pinServo2=9;                                     
const int dirservo1=5;
const int dirservo2=10; 
const int USF= 2;
float CNY_TD; 
float CNY_TI;             
float CNY_FI;
float CNY_CI;
float CNY_CD;
float CNY_FD;
const int blanco = 150;
const int negro = 350;
int contador1;
long duration, cm;
boolean parado, avance, busqueda, prebusqueda;
void setup() {
  // initialize serial communication:
  Serial1.begin(9600);
}

void distancia(){
  pinMode(USF, OUTPUT);
  digitalWrite(USF, LOW);
  delayMicroseconds(2);
  digitalWrite(USF, HIGH);
  delayMicroseconds(5);
  digitalWrite(USF, LOW);
  pinMode(USF, INPUT);
  duration = pulseIn(USF, HIGH);
  cm = microsecondsToCentimeters(duration);
}

long microsecondsToCentimeters(long microseconds) {
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}
void cny(){
  CNY_TD = analogRead(A0);
  CNY_TI = analogRead(A1);
  CNY_FI = analogRead(A2);
  CNY_CI = analogRead(A3);
  CNY_CD = analogRead(A4);
  CNY_FD = analogRead(A5);
}
void loop() {
  cny();
  distancia();

  if (Serial1.available()>0){
    if (Serial1.read() == serial_begin){
      serial();  
    }
  }
  
  else if (contador1 >= 50){
    avance == false;
    busqueda == true;
    contador1 = 0;
    Serial1.print("Reset contador");
  }

  else if ((prebusqueda == false) & (busqueda == false) & (parado == false) & (avance == false)){
    busqueda = true;
    Serial1.print("Flag de Busqueda 2");
  }
  
  else if ((parado == true) & ((CNY_CI <= blanco) or (CNY_CD <= blanco))){

    digitalWrite (dirservo1,HIGH); 
    digitalWrite (dirservo2,LOW);   
 
    analogWrite (pinServo1,vel11);     
    analogWrite (pinServo2,vel1);    
    avance = false;
    prebusqueda = true;
    Serial1.print("Marcha atras - ");
  }
  
  else if ((CNY_CI <= blanco) & (CNY_CD <= blanco)) {    
    avance = false;
    parado = true;
    busqueda = false;
    contador1 = 0;
    Serial1.print("Flag de marcha atras en blanco - ");
  }
  
  else if (prebusqueda == true){
    parado = false;
    busqueda = true;
    prebusqueda = false;
    Serial1.print("Flag de busqueda - ");
  }
  
  else if ((cm <= 5) & (avance == true)){
    digitalWrite (dirservo1,LOW);     
    digitalWrite (dirservo2,HIGH);   
 
    analogWrite (pinServo1,vel4);         //Motor Izquierda, 255
    analogWrite (pinServo2,vel8);        //Motor Derecha, 0
    contador1 = 0;
    Serial1.print("Avance <5 cm - ");
  }

  else if ((cm <= 50) & (avance == false)){
    avance = true;
    busqueda = false;
    Serial1.print("Flag de avance - ");
  }
    
  else if ((cm > 50) & (busqueda == true)){
    digitalWrite (dirservo1,HIGH);
    digitalWrite (dirservo2,HIGH);

    analogWrite(pinServo1,vel11);
    analogWrite(pinServo2,vel11);
    Serial1.print("Busqueda - ");
  }

  else if (avance == true){ 
    digitalWrite (dirservo1,LOW);     
    digitalWrite (dirservo2,HIGH);   
 
    analogWrite (pinServo1,vel4);         //Motor Izquierda, 255
    analogWrite (pinServo2,vel8);        //Motor Derecha, 0    
    contador1 ++;
    Serial1.print("Avance <50 cm - ");
    Serial1.println(contador1);
  }
    
}


void serial(){
  Serial1.print("Distancia: ");
  Serial1.print(cm);
  Serial1.print(" - CNY Centro Izquierda y Centro Derecha: ");
  Serial1.print(CNY_CI);
  Serial1.print(", ");
  Serial1.print(CNY_CD);
  Serial1.print(" - Contador reset avance: ");
  Serial1.print(contador1);
  Serial1.println();
  return;
}

The main reason i put this code is to show how here CNY reading and distance measure works fine together. This issue is destroying my brain more than ussual.

Thanks for your attention.

I just realized that most of the variables are in my native language, if you prefere it i can translate them to be easier to understand.

I don't know the answer but I can suggest what I would do in this circumstance.

  1. Comment out all existing Serial.print statements (as you know it all works)
  2. Add new Serial.print statements that show when the bot is reading the ultrasonic measurement (start/end)
  3. Add new ---"---- when the bot is calculating the line following algorithm.

Now, when the bot moves off course whilst trying to also work out whether something is in front of it, do the serial print statements show you that logically it is doing what it should or not? That is, your serial.print statements show you that it is calculating the line measurement as well as the distance in front of it?

If so, then I guess you might have some blocking statements in there (even without your knowledge) that prevent the loop running as fast as it needs to or there is something wrong in your code.

There is an article on this forum about doing several things 'simultaneously' which might help you too (search for it at the very top of this forum). In essence, your loop should be running as fast as it can whilst calling non-blocking methods to do what needs to be done along the way.

As you have both parts working independently I don't think there is a big problem other than program design (possibly) so do search for that link. This might be it!