Turn LEDs on while routine is running (they're all off)

Greetings,

I’m quite new to C/C++ programming, but i had the basics on college and am self-taught in VBA… My question is probably very simple but i can’t see the solution.

I’m building a line follower robot, and i have a few routines for it to check the line and any nearby object (it stands still for five seconds when the ultrasonic sensor [attached pointing sideways] identify something less then 20cm, and then move on).

What i cant do is to make it show what routine he’s doing atm with a led. They work fine with a test code, so it’s not hardware or anything like.

I set up 3 diferent coloured leds and hooked’em to pins 9, 10 and 11.

What i had in mind was to set the desired led pin high (and all others led pins low) before doing the said routine, that way only one led relative to the running routine would be lit at any given momment… I even tried to light do this inside the routine, but it doesnt show any result.

Here’s the code, sorry for variables in portuguese… if you will i can translate’em.

int E1 = 5;     //M1 Speed Control
int E2 = 6;     //M2 Speed Control
int M1 = 8;    //M1 Direction Control //motores da direita
int M2 = 7;    //M1 Direction Control //motores da esquerda

int sensorLinhaDir = A1;
int sensorLinhaEsq = A2;
int led_amarelo =  9;
int led_vermelho = 10;
int led_verde = 11;
int P_trig = 12;
int P_echo = 13;

long duracao = 0;
long dist = 0;

int ValorCorte = 600;

byte laststeer;
const int velocidade0 = 0;
const int velocidade1 = 51;
const int velocidade2 = 102;
const int velocidade3 = 153;
const int velocidade4 = 204;
const int velocidade5 = 255;

int valorLinhaEsq, valorLinhaDir = 0;







long check = 0;
int now, lastrun = 0;
int stopduration = 5000;

void setup() {
    Serial.begin(9600);
    pinMode(5, OUTPUT); 
    pinMode(6, OUTPUT); 
    pinMode(7, OUTPUT); 
    pinMode(8, OUTPUT); 
    digitalWrite(M1,LOW);
    digitalWrite(M2,LOW); 

    pinMode(P_trig, OUTPUT);
    pinMode(P_echo, INPUT); 

    pinMode(led_amarelo, OUTPUT);
    pinMode(led_vermelho, OUTPUT);
    pinMode(led_verde, OUTPUT);
}

void loop() {

  check = sensor();
  if(check>20){
    acender_verde();
    andar();
    lastrun = millis();
  }
  else{
      now = millis();

      if(now - lastrun <= stopduration){
        parar();
      }
      else{
       andar();
      }
  }


}




void andar(){
  valorLinhaEsq = analogRead(sensorLinhaEsq);    
  valorLinhaDir = analogRead(sensorLinhaDir);
  
  digitalWrite(P_trig, LOW);
  delayMicroseconds(2);
  digitalWrite(P_trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(P_trig, LOW);
  duracao = pulseIn(P_echo, HIGH);
  dist = duracao / 58.2;

  //SEGUE RETO
  if((valorLinhaEsq > ValorCorte)&&(valorLinhaDir > ValorCorte)){
    acender_verde();
    analogWrite (E1,velocidade3);      
    analogWrite (E2,velocidade3); 
    digitalWrite(M1,HIGH);
    digitalWrite(M2,HIGH);
    laststeer = 'r';    
  }
  //AJUSTE ESQUERDA
  if((valorLinhaEsq < ValorCorte)&&(valorLinhaDir > ValorCorte)){
    acender_amarelo();
    analogWrite (E1,velocidade5);      
    analogWrite (E2,velocidade5);
    digitalWrite(M1,LOW);
    digitalWrite(M2,HIGH);   
    laststeer = 'e';   
  }
  //AJUSTE DIREITA
  if((valorLinhaEsq > ValorCorte)&&(valorLinhaDir < ValorCorte)){
    acender_amarelo();
    analogWrite (E1,velocidade5);      
    analogWrite (E2,velocidade5); 
    digitalWrite(M1,HIGH);
    digitalWrite(M2,LOW);    
    laststeer = 'd';
  }

  //AJUSTE FUGA DE ROTA
  if((valorLinhaEsq < ValorCorte)&&(valorLinhaDir < ValorCorte)){ 
    acender_vermelho();
    switch(laststeer){
      case 'e':
        analogWrite (E1,velocidade5);      
        analogWrite (E2,velocidade5); 
        digitalWrite(M1,HIGH);
        digitalWrite(M2,LOW);

      case 'd':
        analogWrite (E1,velocidade5);      
        analogWrite (E2,velocidade5);
        digitalWrite(M1,LOW);
        digitalWrite(M2,HIGH);

      case 'r':
        analogWrite (E1,velocidade3);      
        analogWrite (E2,velocidade3); 
        digitalWrite(M1,LOW);
        digitalWrite(M2,LOW);
      
    }
  }
  
}

void parar(){
    acender_vermelho();
    analogWrite (E1,velocidade0);      
    analogWrite (E2,velocidade0);
    digitalWrite(M1,LOW);
    digitalWrite(M2,LOW);
}

long sensor(){
  digitalWrite(P_trig, LOW);
  delayMicroseconds(2);
  digitalWrite(P_trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(P_trig, LOW);
  duracao = pulseIn(P_echo, HIGH);
  dist = duracao / 58.2;
  return dist;
}

void apagarLEDS(){
  digitalWrite(led_amarelo,LOW);
  digitalWrite(led_vermelho,LOW);
  digitalWrite(led_verde,LOW);
}

void acender_vermelho(){
  digitalWrite(led_amarelo,LOW);
  digitalWrite(led_vermelho,HIGH);
  digitalWrite(led_verde,LOW);
}

void acender_verde(){
  digitalWrite(led_amarelo,LOW);
  digitalWrite(led_vermelho,LOW);
  digitalWrite(led_verde,HIGH);
}

void acender_amarelo(){
  digitalWrite(led_amarelo,HIGH);
  digitalWrite(led_vermelho,LOW);
  digitalWrite(led_verde,LOW);
}

I’m using a motordrive, have removed the jumper (small-black-squarey-thing) from its pins (it has “vin” written next to it), and am supplying power through both inputs, if that concerns to the matter. I’ll try to put pics…

When it’s plugged to a PC and the battery power is OFF, it’ll have the Red LED on. The code responsible for turning the red led on is (because portuguese):

void acender_vermelho(){
  digitalWrite(led_amarelo,LOW);
  digitalWrite(led_vermelho,HIGH);
  digitalWrite(led_verde,LOW);
}

If i turn on the power, it’ll run on course as programmed, but no LED will be lit.

Why are you not using Serial to debug your code?

PaulS:
Why are you not using Serial to debug your code?

Because i’m Inexperienced and self-taught. Thanks for the tip, i’m adding prints right now.