Two servos at once with millis

Projects Programming
5

this looks very complicated, and it is, but it allows you to move servos at the same time.

Instead of write() use moveTo(); to move all/both to the same destination, use moveAllTo()

take a look:

#include <Servo.h>

/******************* BEGIN CLASS DEFINITION *******************/

#define MOTION_SPEED_INTERVAL 10  // milliseconds
#define MAX_SERVO_INSTANCES 5

class Sweep : public Servo {
  public:
    Sweep(void(*_startCallback)(int), void(*_endCallback)(int));
    Sweep();
    void begin(int servo);
    void moveTo(int newPosition);
    static void moveAllTo(int newPosition);
    static void update();
    static void update(uint32_t now);
    static int getServoCount();
    int getPosition (void) const;

  private:
    enum  {
      ARM_STANDBY,
      ARM_MOVING,
    } motionState = ARM_MOVING;
    void(*startCallback)(int);
    void(*endCallback)(int);
    static Sweep* instanceAddress;
    static int instanceCount;
    int servoPin;
    uint32_t lastMotionMillis = 0;
    int currentPosition;
    int targetPosition;
    uint32_t sensedChangedMillis;
    static void setTargetPosition(Sweep* instance, int position);
};

int Sweep::instanceCount = 0;
Sweep* instances[MAX_SERVO_INSTANCES] = {nullptr};

Sweep::Sweep() {
  instances[instanceCount++] = this;
}

Sweep::Sweep(void(*_startCallback)(int), void(*_endCallback)(int)) {
  instances[instanceCount++] = this;
  startCallback = _startCallback;
  endCallback = _endCallback;
}

int Sweep::getServoCount() {
  return instanceCount;
}

void Sweep::begin(int servo) {
  servoPin = servo;
  pinMode(servoPin, OUTPUT);
  attach(servoPin);
  moveTo(0);
  delay(1000);
  detach();
}

void Sweep::setTargetPosition(Sweep* instance, int position) {
  instance->targetPosition = position;
}

void Sweep::moveTo(int newPosition) {
  if (newPosition > 180) {
    newPosition = 180;
  } else if (newPosition < 0) {
    newPosition = 0;
  }
  setTargetPosition(this, newPosition);
}

void Sweep::moveAllTo(int newPosition) {
  if (newPosition > 180) {
    newPosition = 180;
  } else if (newPosition < 0) {
    newPosition = 0;
  }
  for (auto i : instances)
  {
    setTargetPosition(i, newPosition);
  }
}

void Sweep::update() {
  update(millis());
}

void Sweep::update(uint32_t now) {
  for (auto srv : instances) {
    switch (srv->motionState) {
      case ARM_MOVING:
        if (now - srv->lastMotionMillis > MOTION_SPEED_INTERVAL) {
          if (srv->targetPosition == srv->currentPosition) {
            if (srv->attached()) {
              srv->detach();
              srv->motionState = ARM_STANDBY;
              if (srv->endCallback) {
                srv->endCallback(srv->getPosition());
              }
            }
          } else {
            if (!srv->attached()) {
              srv->attach(srv->servoPin);
            }
            if (srv->targetPosition > srv->currentPosition) {
              srv->currentPosition++;
              srv->write(srv->currentPosition);
            } else {
              srv->currentPosition--;
              srv->write(srv->currentPosition);
            }
          }
          srv->lastMotionMillis = now;
        }
        break;
      case ARM_STANDBY:
        if (srv->targetPosition != srv->currentPosition) {
          srv->motionState = ARM_MOVING;
          if (srv->startCallback) {
            srv->startCallback(srv->getPosition());
          }
        }
        break;
    }
  }
}

int Sweep::getPosition(void) const {
  return currentPosition;
}

/******************** END CLASS DEFINITION ********************/

void startMoving(int value);
void stoppedMoving(int value);

Sweep servo1(startMoving, stoppedMoving);  //callback functions to execute when the servo starts or stops moving.
Sweep servo2;  // callback functions not required!

void setup() {
  Serial.begin(9600);
  servo1.begin(4);
  servo2.begin(5);
  pinMode(13, OUTPUT);
  Serial.print("Total Attached Servos:\t");
  Serial.println(Sweep::getServoCount());
}

void loop() {
  Sweep::update();  // this is necessary

  
  if (Serial.available()) {
    int destination = Serial.parseInt();
    Serial.print(F("servo1 asked to move to: "));
    Serial.print(destination);
    Serial.print(F(" degrees\n"));
    servo1.moveTo(destination);
    servo2.moveTo(180 - destination);
  }
}

void startMoving(int value) {
  Serial.print(F("servo1 motion started at: "));
  Serial.print(value);
  Serial.print(F(" degrees\n"));
  digitalWrite(13, HIGH);
}

void stoppedMoving(int value) {
  Serial.print(F("servo1 motion completed at: "));
  Serial.print(value);
  Serial.print(F(" degrees\n"));
  digitalWrite(13, LOW);
}

Note: Test it by entering a value into the Serial Monitor between 0 and 180 inclusive.