Servo doesn't move fully to 180 degrees


I have two boards linked per radio, one sending a value from a joystick and the other moving a servo according to the value.
But the servo doesn’t move when I move the joystick fast to one direction and hold it. For instance, if I move the joystick to the right and hold it in full right position, the servo will stop at say 40 degrees. If I move slightly the joystick, the servo move finaly to 180 degrees.
I checked the received value, it is allright. The code “myservo.write(radioValue);” is simply not applied when the joystick move fast. Any guess? :slight_smile:

// comment out one or other above

const int ledPin=13;            // number of the pin with onboard LED
const byte telegrammstart = 255; // start of 3-byte telegram

const int delayValue = 50;     // delay between 2 telegrams to send
const unsigned long unsignedLongAlmostMax = 4294000000; // close to overflow of millis
char radioModus = 'A';    // A: Joystick modus, B: 8-fly/pull, C: stable
int radioValue = 0;      // value to be transmitted

int incomingByte = 0;
int sensorValue = 0;  // variable to store the value coming from the sensor
unsigned long millisLast = 0; 

  #include <Servo.h> 
  Servo myservo;
  int servoPos = 0;
  int servoPin = 8;
  //The PID code comes next
  #include <PID_v1.h>
  //The code for the IMU+servo
  #include "Wire.h"
  #include "I2Cdev.h"
  #include "MPU6050.h"
  #include "Servo.h"
  MPU6050 mpu;
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  int Val, Target;
  //Define Variables we'll be connecting to
  double Setpoint, Input, Output;
  //Specify the links and initial tuning parameters
  PID myPID(&Input, &Output, &Setpoint, 1.5,0,0, REVERSE);

  const int sensorPin = A0;       // select the input pin for the potentiometer
  int modePin1 = 2;
  int modePin2 = 3;
  int modePin3 = 4;
  char modeSelect()
    if (digitalRead(modePin1)==LOW) return 'A';
    if (digitalRead(modePin2)==LOW) return 'B';
    if (digitalRead(modePin3)==LOW) return 'C';

void setup()
  pinMode(ledPin, OUTPUT);
  millisLast = 0;
    //initialize the variables we're linked to
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    Input = map(ay, -17000, 17000, 0, 255);   
    Setpoint = 90;
    //turn the PID on
    //Hereafter the code for the IMU+servo
    Serial.println("Initialize MPU");
    Serial.println(mpu.testConnection() ? "Connected" : "Connection failed");
    myPID.SetOutputLimits(-90, 90);
      pinMode(modePin1, INPUT_PULLUP);
      pinMode(modePin2, INPUT_PULLUP);
      pinMode(modePin3, INPUT_PULLUP);

void loop()
    // read a value and send a telegram
    if ((millis() >= millisLast + delayValue)) {
//  if ((millis() >= millisLast + delayValue)||(millis() < millisLast - unsignedLongAlmostMax)) {
      sensorValue = analogRead(sensorPin);
      //value for radioModus is to be evaluated, at present only joystick control
      sensorValue = map(sensorValue, 0, 1023, 0, 179);
      //start with telegram
      Serial.write(telegrammstart);  // start of telegram
      Serial.write(modeSelect());    // joystick-modus senden
      millisLast = millis();

  // ==============================================

  // see if there's incoming serial data sent
  if (Serial.available() > 2) {
    // minimum 3 bytes in buffer (sequence telegramstart, modus, value) 
    incomingByte =;
    if (incomingByte == telegrammstart) {
      radioModus =;
      radioValue =;
      #ifdef ISKITEVERSION
        if(radioModus=='A') {
          // joystick mode
          myservo.write(radioValue); // sets the servo position according to the scaled value 
        } else if (radioModus=='B') {
             //Hereafter the code for the IMU+servo 
              mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
              Val = map(ay, -17000, 17000, 1, 180);
              Input = (double)Val;
              Target = Output + Setpoint ;
              //Print some values for debug
              //Serial.print("Setpoint: ");
              //Serial.print("ay: ");
              //Serial.print(" Val: ");
              //Serial.print(" Intput: ");
              //Serial.print(" Output: ");
              //Serial.print(" Target: ");
        } else if (radioModus=='C') {
              myservo.write(180); // test to show modus

Can you post a version of the code with all the irrelevant lines removed - it will make it much easier to focus on the essentials.


char modeSelect()
    if (digitalRead(modePin1)==LOW) return 'A';
    if (digitalRead(modePin2)==LOW) return 'B';
    if (digitalRead(modePin3)==LOW) return 'C';

....and if none of them are LOW . . ?