Problema failsafe su scheda con atmega328 e nrf24l01

Buonasera,sono giorni che lavoro su una scheda per realizzare una ricevente per auto radiocomandata.La scheda funziona bene,il problema è sorto da quando stò cercando di implementare il failsafe per portare il servo ed il motore in posizione neutra in caso di perdita di segnale.Implementando la funzione millis per leggere il segnale vi è un problema(contrasto col timer).Non riesco proprio a risolvere,se cortesemente potreste darmi una mano ve ne sarei grato.Per farvi capire il codice nuovo che ho implementato vi metto nel commento del codice //nuovo!.Grazie

//Ricevitore 2.4ghz per automodello proporzionale.Autore:Ivan Pepe-(Giugno 2016)-Versione 1
//Aggiunto potenziometro digitale 64 step;
//H-bridge -avanti 100%-indietro 60%
//Aggiunta uscita accensione fari anteriori.
//Aggiunte luci Freno;
//Aggiunte luci Fari;
//Aggiunto config pwm;
//Aggiunto Frequency hopping;
//Aggiungere reverse servo???
//Aggiungere failsafe???
//Collegamenti del modulo RF24L01 ad Arduino:
// 1 - GND
// 2 - VCC 3.3V !!! NOT 5V
// 3 - CE to Arduino pin 9
// 4 - CSN to Arduino pin 10
// 5 - SCK to Arduino pin 13
// 6 - MOSI to Arduino pin 11
// 7 - MISO to Arduino pin 12
// 8 - UNUSED

#include <EEPROM.h>
#include <HCMotor.h>
#include <SPI.h>
#include <RF24.h>
#include <Servo.h>
#include <PWMFrequency.h>

int r; int y; int n;
Servo servo;
/* Pins used to drive the motors */
#define MOTOR_PINA 5
#define MOTOR_PINB 6
#define DEADZONE 20
#define POT_REV_MIN 0
#define POT_REV_MAX (250 - DEADZONE)
#define POT_FWD_MIN (251 + DEADZONE)
#define POT_FWD_MAX 510

#define LuciFreno 4

//FARI
#define LED 3          // Led collegato al pin digitale 3
int val = 0;           // Si userà val per conservare lo stato del pin di input
int vecchio_val = 0;   //Si userà vecchio_val per conservare lo stato del pin di input al passo precedente
int stato = 0;         //Ricorda lo stato in cui si trova il led,stato =0 led spento,stato =1 led acceso;

//Failsafe
unsigned long interval=5000; // Tempo attivazione Failsafe 5secondi//NUOVO!
unsigned long previousMillis = 0;//NUOVO!        

#define CE_PIN   9
#define CSN_PIN 10
/* Create an instance of the library */
HCMotor HCMotor;

// Radio pipe addresses for the 2 nodes to communicate.
const uint64_t pipes[2] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL };
int8_t hop = 0;
const uint8_t chan[] = {0x46,0x4B,0x50,0x53,0x56,0x59,0x5C,0x5F,0x62,0x65,0x69,0x6C,0x6F,0x73,0x78,0x7C}; //  hopping channels that  you listen on RX on the same channel.



RF24 radio(CE_PIN, CSN_PIN);

int joystick[3];



void setup(void)
{
  /* Initialise the library */
  HCMotor.Init();
  HCMotor.attach(0, DCMOTOR_H_BRIDGE, MOTOR_PINA, MOTOR_PINB);
  HCMotor.DutyCycle(0, 100);
  Serial.begin(9600);
  delay (1000);
  Serial.println("Nrf24L01 Receiver Starting");
  radio.begin();
  radio.setChannel(chan[hop]);
  //radio.setDataRate(RF24_2MBPS);
  //radio.setDataRate(RF24_1MBPS);
  radio.setDataRate(RF24_250KBPS);
  // optionally, increase the delay between retries & # of retries
  radio.setRetries(12, 12);
  // optionally, reduce the payload size.  seems to
  // improve reliability
  radio.setPayloadSize(8);
  radio.openWritingPipe(pipes[1]);
  radio.openReadingPipe(1, pipes[0]);
  radio.startListening();

  servo.attach(2);
  pinMode(3, OUTPUT);
  pinMode(LuciFreno, OUTPUT);
  //joystick[0] = 0; // Sterzo
  //joystick[1] = 0; // Motore
  //joystick[2] = 0; // Throttle
 
 // Motor PWM frequency
  setPWMPrescaler(MOTOR_PINA, 1); // 123Hz = 256,  492Hz = 64, 3936Hz = 8, 31488Hz = 1
  setPWMPrescaler(MOTOR_PINB, 1);

}

void receiveOrder() {
  unsigned long currentMillis = millis();//NUOVO!

  
  if ( radio.available() )
 
  {
    bool done = false;
    // while (!done)
   
      { 
      uint32_t time = micros();
      hop = (hop + 1) % 16; //16 hop channels need to be sent out at fixed intervals
      while (micros() - time > 8000); //wait sent packets at time interval 8000 us example different channel every 8ms
      // Fetch the payload, and see if this was the last one.
      //radio.read( &message, sizeof(unsigned long) );
      // Fetch the payload, and see if this was the last one.
      radio.read( joystick, sizeof(joystick) );
      
      if ((unsigned long)(currentMillis - previousMillis) >= interval) {//NUOVO!
      
      joystick[0] = 85; // Sterzo//NUOVO!
      joystick[1] = 250; // Motore//NUOVO!
      //joystick[2] = 0; // Throttle//NUOVO!
      previousMillis = millis();//NUOVO!
      }


      servo.write(joystick[0]);
      Serial.print("X = ");
      Serial.print(joystick[0]);
      int Speed, Pot;
      Pot = joystick[1];
      /* Is the pot in the reverse position ? */
      if (Pot >= POT_REV_MIN && Pot <= POT_REV_MAX)
      {
        HCMotor.Direction(0, REVERSE);
        Speed = map(Pot, POT_REV_MIN, POT_REV_MAX, 60, 10);

        /* Is the pot in the forward position ? */
      } else if (Pot >= POT_FWD_MIN && Pot <= POT_FWD_MAX)
      {
        HCMotor.Direction(0, FORWARD);
        Speed = map(Pot, POT_FWD_MIN, POT_FWD_MAX, 0, 100);

        /* Is the pot in the dead zone ? */
      } else
      {
        Speed = 0;
      }

      /* Set the on time of the duty cycle to match the position of the pot. */
      HCMotor.OnTime(0, Speed);

      if   ((Pot >= 170) && (Pot <= 245) )
      {
        digitalWrite(LuciFreno, HIGH);

      }
      else
      {
        digitalWrite(LuciFreno, LOW);

      }

      Serial.print("    Y = ");
      Serial.print(joystick[1]);

      Serial.print("    z = ");
      Serial.println(joystick[2]);

      val = joystick[2]; //legge il valore del pulsante sul joystick2
      //controlla se è accaduto qualcosa
      if ((val == HIGH) && (vecchio_val == LOW)) {
        stato = 1 - stato;
        delay(15); //attesa di 15 millisendi
      }

      vecchio_val = val; //ricordiamo il valore precedente di val

      if (stato == 1) {
        digitalWrite(3, HIGH); //accende il led
      }
      else {
        digitalWrite(3, LOW);//spegne il led
      }

      // First, stop listening so we can talk.
      radio.stopListening();
      // Send the final one back.

      //build_data_frame();
      // Take the time, and send it.  This will block until complete
      radio.write( &joystick, sizeof(joystick) );//  radio.write (joystick, sizeof(joystick) );
      //Now, resume listening so we catch the next packets
      radio.startListening();
    }
  }

  else
  {
    Serial.println("No radio available");
    delay(50);
  }
}
void loop(void)
{
  receiveOrder();
}

per cosa usi il potenziometro digitale? Ciao Uwe

Il potenziometro digitale serve per regolare il servo dello sterzo-centraggio ruote,ma è un chip a parte,non è questo che influisce sul cattivo funzionamento del programma.Secondo me il contrasto è con il frequency hopping.Oggi provo a disabilitare questa funzione ed implementare il singolo canale di trasmissione e vedo se riesco a risolvere.