Серводвигатель с линейным изменением занимает больше времени, чем рассчитано

Я учусь программировать "бот BOE Shield" с партнером в моем университете (первый курс). Поясняю для тех, кто не знает: бот BOE Shield — это небольшой робот, оснащенный 2 серводвигателями, аккумулятором и Arduino Uno. Если вы погуглите название, то найдете много картинок.

Нам было поручено написать программу, которая заставляла бы робота ускорять вращающиеся серводвигатели непрерывного действия в соответствии с заданным графиком зависимости скорости от времени в об/мин и секундах. Мы написали код, следовали довольно логическому методу и сдали задание. Однако одна вещь, и я потратил дни, пытаясь это исправить, все еще сбивает меня с толку. Код впереди, я постарался сделать его максимально читабельным. Преобразование RPM в PWM для двигателей:
PWM = 2 \x RPM + 1500

#include <Servo.h>

class motor {
public:
  double Speed = 0;  //Реальная скорость сервопривода в об/мин
public:
  Servo servo;

  //возвращает true, если нужная скорость "newSpeed" достигается, в противном случае увеличивается текущая скорость и возвращается false.
  //инкремент - расчетное увеличение скорости, основанное на разнице во времени (интервале) и постоянном ускорении.
  bool accel(double increment, double newSpeed) {
    if (abs(Speed - newSpeed) < 0.1) {
      return true;
    }

    Speed += increment;
    return false;
  }

  void writeMS(double value) {
    servo.writeMicroseconds(value);
  }
};

motor servoLeft = motor();
motor servoRight = motor();

void setup() {
  Serial.begin(9600);
  servoLeft.servo.attach(11);   //венстер -2 * скорость влево + 1500
  servoRight.servo.attach(10);  //höger 2 * скорость влево + 1500
}

//Этот вложенный массив содержит инструкции для обоих серводвигателей: желаемую скорость, сколько времени нужно, чтобы разогнаться до нее и сколько времени
//длительность инструкции. Первая инструкция начинается со второй строки. Первая строка пропускается и используется только для расчетов.
//Столбцы расположены в следующем порядке: {длина инструкции в мс, скорость левого двигателя в об/мин, скорость правого двигателя в об/мин, время разгона}.
double instructions[7][4] = {
  { 0, 0, 0, 0 },
  { 3000, 25, 25, 500 },
  { 2550, 50, 10, 500 },
  { 3000, 25, 25, 500 },
  { 2850, 10, 50, 500 },
  { 3000, 25, 25, 500 },
  { 3000, 15, -15, 500 },
};

//время
unsigned long currentMillis;
unsigned long previousMillis = 0;
unsigned long lastInstructionMillis = 0;
const double interval = 20;

//i - это индекс инструкций.
int i = 0;
bool updateL = false;
bool updateR = false;

//увеличение скорости для каждого сервопривода. Рассчитывается в цикле.
double incrementL;
double incrementR;

void loop() {
  // Это помогает отслеживать время, чтобы мы могли вычислять приращения. По сути, это означает, что скорости обновляются каждые 20 мс.
  currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;

    //если нужная скорость достигнута, прекращаем обновление. Это просто для оптимизации кода.
    if (updateL)
      updateL = !servoLeft.accel(incrementL, instructions[i][1]);

    if (updateR)
      updateR = !servoRight.accel(incrementR, instructions[i][2]);
  }

  // это выполняется только один раз после каждой инструкции.
  if (currentMillis - lastInstructionMillis >= instructions[i][0]) {
    lastInstructionMillis += instructions[i][0];
    i++;

    // выходим из программы после выполнения всех инструкций.
    if (i == 7)
      exit(0);

    // приращения рассчитываются так: приращение = ускорение * интервал
    //где постоянное ускорение = (newSpeed - oldSpeed) / AccelerationTime
    incrementL = (instructions[i][1] - instructions[i - 1][1]) * interval / instructions[i][3];
    incrementR = (instructions[i][2] - instructions[i - 1][2]) * interval / instructions[i][3];

    updateL = true;
    updateR = true;
  }

  Serial.println((String)servoLeft.Speed + " " + (String)servoRight.Speed + " " + (String)currentMillis + " " + (String)i);

  // фактические сигналы мс, отправленные на сервоприводы.
  servoLeft.writeMS(2 * servoLeft.Speed + 1500);
  servoRight.writeMS(-2 * servoRight.Speed + 1500); 
}

По какой-то причине разгон идет дольше, чем указано в инструкции. Например, первая инструкция указывает, что оба сервопривода будут разгоняться с 0 до 25 об/мин в течение 500 мс. Но распечатка скорости и времени на последовательном мониторе показывает, что на самом деле это занимает около 860 мс. То есть, если интервал установлен на 20. Если он установлен на 50, это занимает 560 мс.

Предполагаемые причины:

1. Пустой цикл занимает больше времени, чем интервал

Это не так, так как измерение с помощью micros() показало, что это никогда не занимало больше 0,26 мс; это было сделано и с полной программой.

2. Ошибки округления

Это не тот случай, поскольку переключение практически всех типов чисел на double ничего не исправило. И double более чем достаточно точен для этой программы.

3. millis() недостаточно точен

Я не верю, что это так. Я исследовал, насколько он точен на самом деле, и хотя он далеко не идеален, он должен быть достаточно точным для программы. Вместо этого я попытался использовать micros(), и это привело к тому же результату, я не знаю, означает ли это что-то.

Кроме этого, я понятия не имею, в чем может быть причина. Сам код работает нормально, и кажется, что робот действительно следует инструкциям без ошибок. Просто скорость и ускорение почему-то неточны.