Ramping servo motor taking longer than calculated

Question

I'm learning how to code a "BOE Shield bot" with a partner at my university (first year). To clarify for those that do not know: a BOE Shield bot is a small robot equipped with 2 servo motors, a battery pack and an Arduino Uno. If you google the name you'll find plenty of pictures.

We were tasked to code a program that would make the robot accelerate the continuous rotational servo motors according to a given velocity-time graph in RPMs and seconds. We wrote the code, followed a pretty logical method, and passed the assignment. However one thing, and I spent days on trying to fix it, is still confusing me. The code is ahead, I've tried to make it as readible as possible. The RPM to PWM conversion for the motors is:
PWM = 2 \times RPM + 1500

#include <Servo.h>
class motor {
public:
  double Speed = 0;  //The actual speed of the servo in RPM
public:
  Servo servo;
//returns true if the desired speed "newSpeed" is reached, otherwise increments the current speed and returns false.
  //increment is the calculated speed increase based on the differance in time (interval) and the acceleration which is constant.
  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);   //vänster -2 * speedLeft + 1500
  servoRight.servo.attach(10);  //höger 2 * speedLeft + 1500
}
//This nested array contains instructions for both servo motors: the desired speed, how long it should take to accelerate to it and how long
//the instruction duration is. The first instruction starts from the second row. The first row is skipped and only used for calculations.
//The columns are in the following order: {instruction length in ms, speed of the left motor in RPM, speed of right motor in RPM, acceleration time}.
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 },
};
//time stuff
unsigned long currentMillis;
unsigned long previousMillis = 0;
unsigned long lastInstructionMillis = 0;
const double interval = 20;
//i is the index for the instructions.
int i = 0;
bool updateL = false;
bool updateR = false;
//the speed increments for each servo. Calculated in the loop.
double incrementL;
double incrementR;
void loop() {
  //This is to help keep track of the time so we can calculate the increments. It essentially means the speeds are only updated ever 20ms.
  currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
//if the desired speed is reached, stop updating. This is just to optimize code.
if (updateL)
  updateL = !servoLeft.accel(incrementL, instructions[i][1]);

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

}
//this runs only once after each instruction.
  if (currentMillis - lastInstructionMillis >= instructions[i][0]) {
    lastInstructionMillis += instructions[i][0];
    i++;
//exit program after all instructions have been completed.
if (i == 7)
  exit(0);

//the increments are calculated like such: increment = acceleration * interval
//where constant acceleration = (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);
//the actual ms signlas sent to the servos.
  servoLeft.writeMS(2 * servoLeft.Speed + 1500);
  servoRight.writeMS(-2 * servoRight.Speed + 1500); 
}

For some reason the acceleration is taking longer than specified in the instructions. For example, the first instruction specifies that both servos will accelerate from 0 to 25 RPM within 500ms. But printing out the speeds and time to the serial monitor shows that it actually takes around 860ms. That is if interval is set to 20. If it's set to 50, it takes 560ms.

Suspected causes were:

1. Void loop taking longer than the interval

Not the case as measuring it with micros() proved it never took longer than 0.26ms; this was the done with complete program as well.

2. Rounding errors

Not the case as switching pretty much all the number types to double still didn't fix anything. And double is more than enough precise for this program.

3. millis() not being accurate enough

I don't believe this is the case. I researched how accurate it actually is, and while it's nowhere near perfect, it should be accurate enough for the program. I tried using micros() instead and that lead to the same result, I do not know if that means something.

Other than that I have no idea what the cause could be. The code itself runs fine, and the robot actually seems to be following the instructions perfectly with no errors. It's just that the speeds and accelerations aren't accurate for some reason.

Answer 1

There are two things going on here.

You are printing too much to the serial port. After the first second (between 1000 and 9999 ms), each line you print is 20 bytes, including the CRLF terminator. At 9600 b/s, each bytes takes 1.04 ms (1 start bit, 8 data bites and 1 stop bit). This is about 20.8 ms for the whole line, which is slightly longer than interval. You should either print less often (say, every other loop iteration) or faster. Just moving to the next standard speed (19200 b/s) will be enough to ensure you do not miss any interval. You can go faster though.

The second issue is the way you keep track of previousMillis:

if (currentMillis - previousMillis >= interval) {
  previousMillis = currentMillis;
  // ...
}

This is prone to accumulating errors. The test will most likely not happen exactly at previousMillis + interval, mostly because of the serial port. You are a little bit late and, by updating previousMillis this way, these errors are cumulative. You can avoid this by updating previousMillis as:

previousMillis += interval;

You will still have timing errors, but these will manifest themselves in the form of jitter, not as a systematic timing drift.

Interestingly, you are updating lastInstructionMillis the right way!

---

Edit: Here are some extra comments about your question and your sketch, not necessarily related to the problem you are seeing.

You wrote:

measuring [loop()] with micros() proved it never took longer than 0.26ms.

It seems you measured incorrectly. Maybe you measured a version of the program that was not printing so much to the serial port. Or maybe you looked only at the first iterations of the loop, where Serial.println() is fast because the output buffer hasn't filled yet.

The following code is fragile:

bool accel(double increment, double newSpeed) {
  if (abs(Speed - newSpeed) < 0.1) {
    return true;
  }
  // ...
}

The value 0.1 is arbitrary, and the “right choice” is dependent on the program timing. I suggest something more robust, like:

bool accel(double increment, double newSpeed) {
  if (increment > 0) {
    Speed = min(Speed + increment, newSpeed);
  } else {
    Speed = max(Speed + increment, newSpeed);
  }
  return Speed == newSpeed;
}

Note that, although it is generally recommended to not test floating point numbers for exact equality, in this case it is safe, because min() and max() return exactly one of their arguments.

The optimization consisting of calling motor::accel() only during the acceleration phase is futile: this methods takes an very tiny fraction of the loop() time. In the same vein, there would be no harm in calling Servo::writeMicroseconds() on every loop iteration, even if some of the calls turn out to be useless.

The code would be more readable if instructions was an array of struct rather than an array of arrays: this way the fields of each instruction could have evocative names rather than numeric indices.

You are using too much floating point here. Most of the data fields, including all the contents of instructions, could be integers of suitable length.