Is there a better Servo Library for Teensy

[raflyer]

I need to smooth out the movement of my servo's I found a neat library called VarSpeedServo but it won't work with a Teensy as it changes timers that aren't the same as in a Teensy.
Are there any other options?

My servo's should be smooth but they tend to jump from one position to the next like it is missing data. Maybe my sketch is to busy? I attached it below but it may not make sense to anyone other Paul or Jorg as it is X-Plane specific.

Example The radioAlt servo which is tied to the Radio Altitude dataref, jumps 100ft or more each movement. The dataref is changing smoothly with no jumps but the servo has No smooth movements at all. Like it misses part of the incoming data? To much going on in loop maybe?

// Code for Course Dev Bar and GS + GS Flag on HSI, and VSI, Airspeed, Egt Gauge
#include <Servo.h>
#include <NonLinearGauge.h>
#include <AccelStepper.h>

const int homeButton = 11;
const int ledPin = 13;
const int PitchFD = A14;

byte hBval;
int val = 0;

AccelStepper stepper1(1, 0, 1);
AccelStepper stepper2(1, 6, 5);
AccelStepper stepper3(1, 2, 3);

// create servo object to control servo's
Servo airspeedServo;
Servo pitchfdServo;
Servo radioAltServo;
Servo myservo; // Coarse Altitude output

// Teensy specific variables
FlightSimFloat asp; //Air Speed gauge
FlightSimFloat ra; //Radio Altitude
FlightSimInteger toFrom;
FlightSimFloat adiroll;
FlightSimFloat adipitch;
FlightSimFloat FdPitch;
FlightSimInteger batt;

//FlightSimFloat gsFlag;
FlightSimFloat crsBar;  //-2.0 to 2.0 2 dot deflection
FlightSimFloat gsBar;  //-2.0 to 2.0 2 dot deflection
FlightSimFloat egt1; // in degrees celcius
FlightSimFloat Altitude;  //Alitude in Feet MSL
// Nonlinear Gauge setup for servo's
float raSet[] = {0, 500, 700, 1000, 1200, 1500, 2000, 2500, 2600}; //Gauge Face position
float raDeg[] = {35, 66, 75, 86, 92, 98, 106, 113, 118}; //Servo degrees
float aspFPM[] = {0, 60, 107, 164, 184, 205, 255, 357, 403}; //Gauge Face position
float aspDeg[] = {27, 27, 47, 72, 81, 90, 112, 158, 180}; //Servo degrees

NonLinearGauge airspeed {9, aspDeg, aspFPM};
NonLinearGauge radioAlt {9, raDeg, raSet};

#define CourseDeviation 10
#define GsDeviation 9
//#define Egt1Gauge 3

float cur_pos; // For stepper ADI Roll
float cur_pos2; // For stepper ADI Pitch
float cur_pos3; // Altimeter stepper

// setup runs once
void setup() {
  analogWriteResolution(12);
  setupCourseDeviation();
  setupGsDeviation();
  //setupEgt1Gauge();
  airspeedServo.attach(4);
  pitchfdServo.attach(20);
  radioAltServo.attach(23);
  myservo.attach(21);  

  stepper1.setMaxSpeed(700);
  stepper1.setAcceleration(700);
  stepper2.setMaxSpeed(1000);
  stepper2.setAcceleration(1000);
  stepper3.setMaxSpeed(500);
  stepper3.setAcceleration(300);

  toFrom      = XPlaneRef("sim/cockpit2/radios/indicators/nav1_flag_from_to_pilot"); //Nav-To-From indication, nav1, pilot, 0 is flag, 1 is to, 2 is from."
  asp         = XPlaneRef("sim/cockpit2/gauges/indicators/airspeed_kts_pilot");
  asp         = constrain(asp, 60, 400);
  adiroll     = XPlaneRef("sim/cockpit2/gauges/indicators/roll_electric_deg_pilot");
  adipitch    = XPlaneRef("sim/cockpit2/gauges/indicators/pitch_electric_deg_pilot");
  batt        = XPlaneRef("sim/cockpit2/electrical/battery_on[0]");
  Altitude    = XPlaneRef("sim/cockpit2/gauges/indicators/altitude_ft_pilot");
  FdPitch     = XPlaneRef("sim/cockpit2/autopilot/flight_director_pitch_deg");
  ra          = XPlaneRef("sim/cockpit2/gauges/indicators/radio_altimeter_height_ft_pilot");
  pinMode(5, OUTPUT);
  //pinMode(19, OUTPUT);
  //pinMode(PitchFD, OUTPUT);
  pinMode(8, OUTPUT);
  pinMode(7, OUTPUT);
  pinMode(homeButton, INPUT_PULLUP);
  pinMode(ledPin, OUTPUT);
  stepperHome(); //runs routine to home motor

}

// loop runs repetitively, as long as power is on
void loop() {
  FlightSim.update(); // causes X-Plane's changes to be received
     
  cur_pos = adiroll * 13.33;
  stepper1.moveTo(cur_pos);
  stepper1.run();  //ToPosition

  cur_pos2 = adipitch * -40;
  stepper2.moveTo(cur_pos2);
   stepper2.run();  //ToPosition

  cur_pos3 = Altitude * -3.549; //-3.551
  stepper3.moveTo(cur_pos3);
  stepper3.run();  //ToPosition
 
  ra = constrain(ra, 0 , 2600);
  
  airspeedServo.write(airspeed.getAngle(asp));
  radioAltServo.write(radioAlt.getAngle(ra));
  
  val = Altitude;
  val = map(val, 0, 40000, 85, 5);     // 42000 lue between 0 and 180)
  val = constrain(val, 5, 85 );
  myservo.write(val);                  // sets the servo position according to the scaled value

  
  int val = FdPitch - adipitch;
  val = map(val, -10, 10, 85, 130);
  val = constrain(val, 80, 140);
  pitchfdServo.write(val);
  

  updateCourseDeviation();
  updateGsDeviation();
  //updateEgt1Gauge();
}

    void setupCourseDeviation() {
    crsBar = XPlaneRef("sim/cockpit/radios/nav1_hdef_dot");
    pinMode(CourseDeviation, OUTPUT);  // PWM out
  }

  void updateCourseDeviation() {
    // crsBar -2.0 to 2.0 2 dot deflection
    int dev = (crsBar * 127); // dev = Deviation of course bar
    dev = map(dev, -254, 254, 0, 4095);
    analogWrite(CourseDeviation, dev);
  }

  void setupGsDeviation() {
    gsBar = XPlaneRef("sim/cockpit/radios/nav1_vdef_dot");
    pinMode(GsDeviation, OUTPUT);  // PWM out
  }

  void updateGsDeviation() {
    // crsBar -2.0 to 2.0 2 dot deflection
    int gsdev = (gsBar * 127); // dev = Deviation of course bar
    gsdev = map(gsdev, -254, 254, 0, 4095);
    analogWrite(GsDeviation, gsdev);
  }
   
 
   void stepperHome() { //this routine should run the motor
    hBval = digitalRead(homeButton);
    while (hBval == LOW)
    {
      //backwards slowly till it hits the switch and stops
      stepper1.moveTo(-2600);
      stepper1.run();
      digitalWrite(ledPin, HIGH); //indicates it's doing something
      hBval = digitalRead(homeButton);
      //      Serial.print("HomeButton is");
      //      Serial.println(hBval);
    }
    digitalWrite(ledPin, LOW); //indicates it's doing something
    stepper1.setCurrentPosition(0); //should set motor position to zero and go back to main routine
    //    Serial.print("Done with stepper Home");
  }



//void setupEgt1Gauge() {
//  egt1 = XPlaneRef("sim/cockpit2/engine/indicators/EGT_deg_C[0]");
//  pinMode(Egt1Gauge, OUTPUT);
//}

//void updateEgt1Gauge() {
//  int i;
//  i = (int) egt1;
//  i = map(i, 0, 600, 0, 3200);
//  analogWrite(Egt1Gauge, i);
//}