Problema pulsanti led millis

International Italiano Software
28 
#include <SR04.h>
#define TRIG_PIN D6 // ingresso A0 di arduino nano utilizzato come ingresso digitale
#define ECHO_PIN D7 // ingresso A1 di arduino nano utilizzato come ingresso digitale

SR04 sr04 = SR04(ECHO_PIN, TRIG_PIN); //inseriamo un oggetto di tipo SRO4, lo chiamiamo sr04 e gli diamo come parametri i pin utilizzati per TRIG ed ECHO

#define LATCH_PIN D3                            // Arduino UNO conection on Pin #4  = Latch of Display Module
#define CLK_PIN D2                               // Arduino UNO conection on Pin #7  = Clock of Display Module
#define DATA_PIN D4                              // Arduino UNO conection on Pin #8  = Data of Display Module

// Ingresso sensori IR E18-D80NK
#define SensMezzo D1
#define SensFine  D5

//led sensori IR E18-D80NK
#define LedYellowSens D0
#define LedRedSens D8



int N[10] = {0b00111111, 0b00000110, 0b01011011, 0b01001111, 0b01100110, 0b01101101, 0b01111101, 0b00000111, 0b01111111, 0b01101111};

int dg[4] = {0b11111101, 0b11111011, 0b11110111, 0b11101111};

long t = 0;
long debounce_delay = 2000;

int stato = 1;

// variabili lampeggio led
unsigned long ritardo;
int led = 0;

int pellet;

void setup() {
  pinMode(LATCH_PIN, OUTPUT);
  pinMode(CLK_PIN, OUTPUT);
  pinMode(DATA_PIN, OUTPUT);
  pinMode (LedYellowSens, OUTPUT);
  pinMode (LedRedSens, OUTPUT);
  pinMode (SensMezzo, INPUT);
  pinMode (SensFine, INPUT);
  Serial.begin(115200);
}

void loop() {

  static unsigned long timer = millis();

  if (millis() - timer >= 1000) { //sfruttando la funzione millis facciamo in modo che ogni 1000 ms dal reset della scheda
    timer += 1000; //(o dall'avvio) avvenga una lettura del sensore di distanza
    pellet = sr04.Distance();
    Serial.println(pellet);


   int migliaia = pellet / 1000;
    int centinaia = pellet / 100 % 10;
    int decine = pellet % 100 / 10;
    int unita = pellet % 10;
   
   displayChars(2,N[migliaia]);
   displayChars(3,N[centinaia]);
   displayChars(2,N[decine]);
   displayChars(3,N[unita]);
   
   
  }


  if ((millis() - t) > debounce_delay) { // ritardo lettura sensori
    int sens1 = digitalRead(SensMezzo); //sensore a ultrasuoni posto a metà della cassa del pellet
    int sens2 = digitalRead(SensFine);  //sensore a ultrasuoni posto alla fine della cassa del pellet
    t = millis();


    switch (stato) { // se i sensori sono tutti e 2 attivi i led sono spenti
      case 1:
        digitalWrite(LedYellowSens, HIGH);
        digitalWrite(LedRedSens, HIGH );
        //cambio di stato se:
        if (sens1 && !sens2) stato = 2; // se il primo sens1 e spento e il sens2 e attivo si sposta allo stato 2
        break;

      case 2: // il led giallo e attivo e il led rosso è spento
        digitalWrite(LedYellowSens, LOW);
        digitalWrite(LedRedSens, HIGH);
        //cambio di stato se:
        if (sens1 && sens2) stato = 3;    // se i sensori sono tutti e 2 spenti si passa allo stato 3
        if (!sens2 && !sens1) stato = 1;  // se i sensori sono tutti e 2 attivi i led sono spenti e si ritorna allo stato 1
        break;

      case 3: // il led giallo e spento e il led rosso e acceso lampeggiante
        digitalWrite(LedYellowSens, HIGH);
        if (millis() - ritardo > 500) {
          led = !led; // il valore stato cambia ogni volta che l'if è vero
          ritardo = millis(); // azzero il valvore di ritardo
        }

        digitalWrite(LedRedSens, led); // accendo e spengo il led in funzionae di "stato"

        if (sens1 && !sens2) stato = 2; //se il primo sens1 e spento e il sens2 e attivo si sposta allo stato 2

        break;
    }
  }
}



void displayChars(int num, int disp) {                    // Chars Display Routine

 int k, charsM = 0;

  charsM = num;

  
  


  digitalWrite(LATCH_PIN, LOW);
   shiftOut(DATA_PIN, CLK_PIN, MSBFIRST,  disp>>256 );       // Set Digits (0-1-2-3)
  shiftOut(DATA_PIN, CLK_PIN, MSBFIRST, charsM);           // Display number
   digitalWrite(LATCH_PIN, HIGH);

}

sto provando a far visualizzare questo sketch su un display4 cifre 7 segmenti ma ottengo solo il risultato delle unità riportato su tutte le cifre. dove sbaglio?