Problema nel controllare servo digitale con encoder

Ciao a tutti sto cercando di controllare un servo digitare jx con un rotary encoder. Questo è il data sheet:
Description:
Brand:JX
Item:PDI-6221MG 20KG Large Torque Digital Standard Servo
Dead band: 2μs
Working frequence: 1520μs / 330hz
Operating Speed (4.8V): 0.18 sec/60°
Operating Speed (6V): 0.16 sec/60°
Stall Torque (4.8V): 17.25 kg.cm (239.55oz/in)
Stall Torque (6V): 20.32 kg-cm (281.89 oz/in)
Dimensions: 40.5X20.2X38mm /1.59 X0.80X49 in
Weight: 62 g (2.18oz)
Connector Wire Length: JR 265 mm (10.43in)
Bearing: 2BB
Usage:For RC Models

È da un po’ che cerco di variare i parametri di ritardo temporale dal valore ottenuto dall’ encoder a quello che invio al servo ma appena giro un po’ più veloce la manovella il servo si blocca. Sapreste aiutarmi? Sto usando arduino uno come trasmettitore con encoder e nrf24l01 e come ricevitore arduino mega con attaccato il servo e altri sensori.

RICEVITORE

#include <Servo.h>

#include "Arduino.h"
#include <SPI.h>
#include <RF24.h>

#include<Wire.h>

Servo sterzo;

const int MPU_addr=0x68;  // I2C address of the MPU-6050
int16_t AcX;

int tempPin1 = 1;
int tempPin2 = 2;

int analogInput1 = 3;
float V1_bat = 0.0;
int value1 = 0;

int analogInput2 = 4;
float V2_bat = 0.0;
float vin2 = 0.0;
int value2 = 0;

int analogInput3 = 5;
float V3_bat = 0.0;
int value3 = 0;

int analogInput4 = 6;
float V_servo = 0.0;
float vin_servo = 0.0;
int value4 = 0;
float R1 = 100000.0; // PER ENTRAMBI SERVO E ARDUINO
float R2 = 1000000.0;

int analogInput5 = 7;
float V_arduino = 0.0;
float vin_arduino = 0.0;
int value5 = 0;

// This is just the way the RF24 library works:
// Hardware configuration: Set up nRF24L01 radio on SPI bus (pins 50, 51, 52, 53) plus pins 7 & 8
RF24 radio(7, 8);

byte addresses[][6] = {"1Node","2Node"};

// -----------------------------------------------------------------------------
// SETUP   SETUP   SETUP   SETUP   SETUP   SETUP   SETUP   SETUP   SETUP
// -----------------------------------------------------------------------------
void setup() {
  //ACCELEROMETRO
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

  //TENSIONI
  pinMode(analogInput1, INPUT);
  pinMode(analogInput2, INPUT);
  pinMode(analogInput3, INPUT);
//  pinMode(analogInput4, INPUT);
//  pinMode(analogInput5, INPUT);

  sterzo.attach(6, 545, 2550);
  sterzo.write(90);
  
  Serial.begin(9600);
  Serial.println("THIS IS THE RECEIVER CODE - YOU NEED THE OTHER ARDUINO TO TRANSMIT");

  // Initiate the radio object
  radio.begin();

  // Set the transmit power to lowest available to prevent power supply related issues
  radio.setPALevel(RF24_PA_MIN);

  // Set the speed of the transmission to the quickest available
  radio.setDataRate(RF24_2MBPS);

  // Use a channel unlikely to be used by Wifi, Microwave ovens etc
  radio.setChannel(124);

  // Open a writing and reading pipe on each radio, with opposite addresses
  radio.openWritingPipe(addresses[0]);
  radio.openReadingPipe(1, addresses[1]);

  // Start the radio listening for data
  radio.startListening();
}

// -----------------------------------------------------------------------------
// We are LISTENING on this device only (although we do transmit a response)
// -----------------------------------------------------------------------------
// LOOP      LOOP----------------------------LOOP-----------------LOOP

void loop() {

  //ACCELEROMETRO
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  float accelerazione=AcX;

  //TEMPERATURA
  int tempReading1 = analogRead(tempPin1);
  // This is OK
  double tempK1 = log(10000.0 * ((1024.0 / tempReading1 - 1)));
  tempK1 = 1 / (0.001129148 + (0.000234125 + (0.0000000876741 * tempK1 * tempK1 )) * tempK1 );       //  Temp Kelvin
  float temperatura1 = tempK1 - 273.15;            // Convert Kelvin to Celcius  

/*  //TEMPERATURA2
  int tempReading2 = analogRead(tempPin2);
  // This is OK
  double tempK2 = log(10000.0 * ((1024.0 / tempReading2 - 1)));
  tempK2 = 1 / (0.001129148 + (0.000234125 + (0.0000000876741 * tempK2 * tempK2 )) * tempK2 );       //  Temp Kelvin
  float temperatura2 = tempK2 - 273.15;            // Convert Kelvin to Celcius  
*/
  //TENSIONI-------INCERTEZZA 5mV
  //BATTERIA
  value1 = analogRead(analogInput1);
  V1_bat = (value1 * 5) / 1024.0;
  if(V1_bat<0.09){
    V1_bat = 0.0;
  }

  value2 = analogRead(analogInput2);
  V2_bat = (value2 * 5) / 1024.0;
  if(V2_bat<0.09){
    V2_bat = 0.0;
  }

  value3 = analogRead(analogInput3);
  V3_bat = (value3 * 5) / 1024.0;
  if(V3_bat<0.09){
    V3_bat = 0.0;
  }
  
/*  //SERVO
  value4 = analogRead(analogInput4);
  vin_servo = (value4 * 5) / 1024.0;
  V_servo = (vin_servo * (R1 + R2))/ R1;
  if(V_servo<0.09){
    V_servo = 0.0;
  }

  //ARDUINO
  value5 = analogRead(analogInput5);
  vin_arduino = (value5 * 5) / 1024.0;
  V_arduino = (vin_arduino * (R1 + R2))/ R1;
  if(V_arduino<0.09){
    V_arduino = 0.0;
  }
*/
  
  // This is what we receive from the other device (the transmitter)
  int dataTx[2];

  // Is there any data for us to get?
  if ( radio.available()) {

    // Go and read the data and put it into that variable
    while (radio.available()) {
      radio.read( &dataTx, sizeof(dataTx));
    }

    //SERVO ------ 180ms per 60°------>1°  3ms ///  1ms 0.33°
    int posServo = dataTx[0];
    unsigned long tempo = millis();
    static unsigned long ritardo=0;

    if( tempo - ritardo > 70)
    {
      sterzo.write(posServo);
      ritardo = tempo;
    }

    // No more data to get so send it back but add 1 first just for kicks
    // First, stop listening so we can talk
    radio.stopListening();
//PROVAAAAAAAAAAAAAAAAAAA
float temperatura2=0.0;
    float dataRx[]={accelerazione, temperatura1, temperatura2, V1_bat, V2_bat, V3_bat, V_servo, V_arduino};
    
    //MANDO TUTTO A TX
    radio.write( &dataRx, sizeof(dataRx) );

    // Now, resume listening so we catch the next packets.
    radio.startListening();

    // Tell the user what we sent back (the random numer + 1)
    Serial.print("Sent response ");
    Serial.println(dataTx[0]);
    Serial.println("  ");
    Serial.println(dataTx[1]);
    
  }
}