When does loop() time affect servo performance?

The calculations in my loop -there are lots of them - take about 21000us ± 700us to complete. It seems they are the source of my chunky servo action and yet a manual delay of this same time does not produce this effect. Is it the variance in the loop time? I am using the <servo.h> library and perhaps it planned on servos getting info faster than every 22ms. or on a more regular basis. I would greatly appreciate some guidance to smooth out this action.

I couldn’t not include all the code (over 9000 characters), of the 6 servos, I have included two representatives. It runs as shown, the loop as shown here takes about 5600us ±250us and is still jittery. the pot input is A0 servo out is pin ~9

```
//this program takes in inital stewart platfrom parameters anlong with 6 inputs (Pitch, Roll, Yaw, TransX, TransY, TransZ) and calcualtes the appropriate angles for 6 servos.
//this is the library used to communicate with the servos
#include <Servo.h>
//this is a vareable that will be used to time how long the code takes to execute a loop. as I write this it takes 22,000ms
unsigned int Time = 0;
int Printing = 1;
//this section initalizes the servos as object which have access to the Servo.h library
Servo ServoA1;
Servo ServoA2;
//this section names the read pints for each Pot so that they can be used more easily
int const PotA1 = A0;
int const PotA2 = A1;
//these vareabels will take on the readings of the 6 pots and will be mapped to an appropriate input value.
int InputA1;
int InputA2;
//these are the inital parameters that define the structure of the stewart platform. they will stay constant
float BaseRad = 10;
float BaseEdge = 12.5;
float PlatRad = 9;
float PlatEdge = 10;
float PlatHeight = 6.8;
float ArmLen = 5;
float RodLen = sqrt(pow((PlatRad*cos(-asin((PlatEdge/2)/PlatRad)))-(BaseRad*cos(-asin((BaseEdge/2)/BaseRad))),2)+pow((PlatRad*sin(-asin((PlatEdge/2)/PlatRad)))-(-BaseEdge/2+ArmLen),2)+pow(PlatHeight,2));
//these are the 6 inputs. They are defined here but they will be recalcuated when the program runs
float Pitch = 0;
float Roll = 0;
float Yaw = 0;
float TransX = 0;
float TransY = 0;
float TransZ = 0;
//These are teh base points, they will stay constant though the process.
float BA1x = BaseRad*cos(-asin((BaseEdge/2)/BaseRad));
float BA1y = BaseRad*sin(-asin((BaseEdge/2)/BaseRad));
float BA1z = 0;
float BA2x = BaseRad*cos(asin((BaseEdge/2)/BaseRad));
float BA2y = BaseRad*sin(asin((BaseEdge/2)/BaseRad));
float BA2z = 0;
//These are the platform points, they will be calcuated in the loop and will be used in conjuction with the base points to determine the servo angles
float PA1x;
float PA1y;
float PA1z;
float PA2x;
float PA2y;
float PA2z;
//thse are the 6 servo angles
float SA1;
float SA2;
//this is the setup where the serial connection is established and the servos are initialized to their corresponding pins
void setup() {
ServoA1.attach(9,650,1850);
Serial.begin(9600);
}
//this is the sign function but I may not need this for my program to run
int sign(float val){
if(val<0)return -1;
if (val>=0)return 1;
}
//this funciton formats information that is shown on the serialmonitor. it displays the 6 angles and has three more outputs that can be used
void Print(){
if(Printing = 1){
Serial.print("pot: ");
Serial.print(InputA1,4);
Serial.print("\t");
Serial.print("Mapped Z: ");
Serial.println(map(InputA1, 0, 1023, 300, -300)*.01,4);
Serial.print("SA1: ");
Serial.print(SA1,4);
Serial.print("\t");
Serial.print("add 90: ");
Serial.println(SA1+90,4);
Serial.print("mapped: ");
Serial.print(map(SA1+90,0,180,2,131));
Serial.print("\t");
Serial.print("time: ");
Serial.println(Time);
}
}
//this is where all the servo angles are calcuated. They requrie that the platfom coordinates also be calcualted
void loop() {
//this starts the loop timer
Time = micros();
//collect all the potentiometer readings
InputA1 = analogRead(PotA1);
//map all the poentiometer readins to the corresponding input values
TransZ = map(InputA1, 0, 1023, 300, -300)*.01;
//this section uses the input values to calcuate the platform coordinates. organized into x y and z components.
PA1x = cos(radians(Yaw))*cos(radians(Roll))*PlatRad*cos(-asin((PlatEdge/2)/PlatRad))+(cos(radians(Yaw))*sin(radians(Roll))*sin(radians(Pitch))-sin(radians(Yaw))*cos(radians(Pitch)))*PlatRad*sin(-asin((PlatEdge/2)/PlatRad))+TransX;
PA2x = cos(radians(Yaw))*cos(radians(Roll))*PlatRad*cos(asin((PlatEdge/2)/PlatRad))+(cos(radians(Yaw))*sin(radians(Roll))*sin(radians(Pitch))-sin(radians(Yaw))*cos(radians(Pitch)))*PlatRad*sin(asin((PlatEdge/2)/PlatRad))+TransX;
PA1y = PlatRad*cos(-asin((PlatEdge/2)/PlatRad))*sin(radians(Yaw))*cos(radians(Roll))+PlatRad*sin(-asin((PlatEdge/2)/PlatRad))*(sin(radians(Yaw))*sin(radians(Roll))*sin(radians(Pitch))+cos(radians(Yaw))*cos(radians(Pitch)))+TransY;
PA2y = PlatRad*cos(asin((PlatEdge/2)/PlatRad))*sin(radians(Yaw))*cos(radians(Roll))+PlatRad*sin(asin((PlatEdge/2)/PlatRad))*(sin(radians(Yaw))*sin(radians(Roll))*sin(radians(Pitch))+cos(radians(Yaw))*cos(radians(Pitch)))+TransY;
PA1z = PlatRad*cos(-asin((PlatEdge/2)/PlatRad))*-sin(radians(Roll))+PlatRad*sin(-asin((PlatEdge/2)/PlatRad))*cos(radians(Roll))*sin(radians(Pitch))+PlatHeight+TransZ;
PA2z = PlatRad*cos(asin((PlatEdge/2)/PlatRad))*-sin(radians(Roll))+PlatRad*sin(asin((PlatEdge/2)/PlatRad))*cos(radians(Roll))*sin(radians(Pitch))+PlatHeight+TransZ;
//This section uses the all the existing parameters and coordinates to calcuate the servo angles
SA1 = degrees(asin(((pow(PA1x-BA1x,2)+pow(PA1y-BA1y,2)+pow(PA1z,2)+pow(ArmLen,2)-pow(RodLen,2))/(2*ArmLen))/sqrt(pow(cos(radians(90))*(PA1x-BA1x)+sin(radians(90))*(PA1y-BA1y),2)+pow(PA1z,2)))+atan(PA1z/(cos(radians(90))*(PA1x-BA1x)+sin(radians(90))*(PA1y-BA1y))))-90*sign(cos(radians(90))*(PA1x-BA1x)+sin(radians(90))*(PA1y-BA1y));
SA2 = degrees(-asin(((pow(PA2x-BA2x,2)+pow(PA2y-BA2y,2)+pow(PA2z,2)+pow(ArmLen,2)-pow(RodLen,2))/(2*ArmLen))/sqrt(pow(cos(radians(90))*(PA2x-BA2x)+sin(radians(90))*(PA2y-BA2y),2)+pow(PA2z,2)))+atan(PA2z/(cos(radians(90))*(PA2x-BA2x)+sin(radians(90))*(PA2y-BA2y))))-90*sign(cos(radians(90))*(PA2x-BA2x)+sin(radians(90))*(PA2y-BA2y))+180;
//this section directs the servos to their required positions
// Notes: this section probably needs to be tweaked
ServoA1.write(map(SA1+90,0,180,131,2));
//this stops the loop timer
Time = micros()-Time;
//the infomation for this cycle is printed ont he serial monitor using the Print() function which was defined earlier;
Print();
//this sets the delay so that you can reed the serial monitor information (since it loops so fast that its hard to read if you dont slow it down)
delay(20);
}
```