Ramping servo motor taking longer than calculated

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.

1. 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.

1. 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.

• Have you seen this? forum.arduino.cc/t/use-of-exit-0/210352 Dec 15, 2022 at 18:07
• Are you sure you are properly driving the motors and that they are not slipping?
– Gil
Dec 15, 2022 at 20:04
• From Arduino reference on double: "On the Uno and other ATMEGA based boards, this occupies 4 bytes. That is, the double implementation is exactly the same as the float, with no gain in precision." Dec 15, 2022 at 20:37
• When you call the accel() method of your class, you provide first the second and third element of the array describing the instruction. These are the target speeds according to your comment. Then as second parameter you are providing incrementL or incrementR. So first target speed, then increment. But the function definition has this the other way round: First the increment, then the new speed. Please try switching the parameters at either the function definition or the function calls. Does it work correctly then? Dec 15, 2022 at 20:44
• @VE7JRO I do not think that is the issue. While I am aware how "taboo" exit is, it is only used at the end of the program. It has no effect on anything else. Dec 15, 2022 at 22:34

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.

• I do not currently have access to the robot, it's with my partner. But I'm very excited to try out this solution. I think you may have found the problem. @chrisl already hinted toward the serial interface spamming with the short speed. All your other suggestions are appreciated and insightful as well. I will try to get my hands on the robot as quickly as I can. In the meantime I will accept your answer as I'm pretty sure you solved it. If not, I will get back to you. It's the end of of the semester so I might not be able to access the robot and I don't want the question left unanswered. Dec 16, 2022 at 12:15