These 4 simple if statements executed each cycle (they were boolean checks that, if true, would execute the appropriate code.) Their presence introduced a delay of about. 0.25~.5 seconds to every cycle.
To save me guessing, can you say which 4 if statements?
This could waste a lot of time:
while(1) // Stay reading the joystick until it has fully been read 1 time
{
incoming_byte = Serial1.read(); // Read the byte and set it equal to incoming_byte
if (incoming_byte == delimiter) // If incoming byte is the delimiter
And this:
while(millis() < servo_enter_function_time + 1250) // Wait for 1250 ms
{
}