Jitter when attaching/detaching multiple servos

[aa365]

I'm using setTargetPositionReachedHandler to detect when a servo has finished moving and to detach it. This is to stop the servo overheating when left in position for a long time. The code works fine with just one servo, but if there is more than 1 then then reattachment causes a jitter at the start of the movement. I can't really understand why this is happening and would appreciate some help debugging. I've tried building a queue of servos to detached (marked as volatile) and then detaching them from the main loop() instead of from the handler, but same result. I've also tried removing interrupts entirely and using updateAllServos() but still no joy. Any ideas where I'm going wrong?

Here is my code:

#include "ServoEasing.hpp"
#include "constants.h"
#include "utils.h"
#include <Arduino.h>

ServoEasing leftServo;
ServoEasing rightServo;
ServoEasing lowerServo;

ServoEasing *getServo(int pin) {
  if (pin == LOWER_SERVO_PIN) {
    return &lowerServo;
  } else if (pin == LEFT_SERVO_PIN) {
    return &leftServo;
  } else if (pin == RIGHT_SERVO_PIN) {
    return &rightServo;
  } else {
    return nullptr;
  }
}

void moveServo(int pin, int pos, int speed) {
  ServoEasing *servo = getServo(pin);
  if (servo->isMoving() || servo->getCurrentAngle() == pos) {
    return;
  }

  if (!servo->attached()) {
    servo->attach(pin, servo->getCurrentAngle());
  }
  servo->startEaseTo(pos, speed);
}

void open() {
  moveServo(LEFT_SERVO_PIN, LEFT_SERVO_OPEN_POSITION, SERVO_OPEN_SPEED);
  moveServo(RIGHT_SERVO_PIN, RIGHT_SERVO_OPEN_POSITION, SERVO_OPEN_SPEED);
  moveServo(LOWER_SERVO_PIN, LOWER_SERVO_OPEN_POSITION, SERVO_OPEN_SPEED);
}

void close() {
  moveServo(LOWER_SERVO_PIN, LOWER_SERVO_CLOSE_POSITION, SERVO_CLOSE_SPEED);
  moveServo(LEFT_SERVO_PIN, LEFT_SERVO_CLOSE_POSITION, SERVO_CLOSE_SPEED);
  moveServo(RIGHT_SERVO_PIN, RIGHT_SERVO_CLOSE_POSITION, SERVO_CLOSE_SPEED);
}

void monitorSerialInput() {
  if (Serial.available()) {
    char command = Serial.read();
    switch (command) {
    case 'o':
      open();
      break;
    case 'k':
      close();
      break;
    case '?':
      break;
    case 'x':
      ESP.restart();
      break;
    }
  }
}

void ServoTargetPositionReachedHandler(ServoEasing *servo) {
  if (servo->attached() && !servo->isMoving()) {
    servo->detach();
  }
}

void setupServo(int pin, int pos) {
  ServoEasing *servo = getServo(pin);
  servo->setTargetPositionReachedHandler(ServoTargetPositionReachedHandler);
  servo->setEasingType(EASE_CUBIC_IN_OUT);
  servo->attach(pin, pos);
}

void setup() {
  Serial.begin(115200);
  setupServo(LOWER_SERVO_PIN, LOWER_SERVO_OPEN_POSITION);
  setupServo(LEFT_SERVO_PIN, LEFT_SERVO_OPEN_POSITION);
  setupServo(RIGHT_SERVO_PIN, RIGHT_SERVO_OPEN_POSITION);
  Serial.println("Ready");
}

void loop() { monitorSerialInput(); }

[ArminJo]

You can enable TRACE and watch serial output if there is a jitter in the values sent to the servos.

#define TRACE
#include "ServoEasing.hpp"