Two servos at once with millis

Projects Programming
1

Trying to make servo-driven eyebrows on a face go up and down at different times together. I can only get them to go in sequence -- first the left one goes, then the right, then the left. I want them to go at the same time but with different timing.

I can get LEDs to work properly using millis but I can't get these darn servos to cooperate.

The 'Several Things at a Time' tutorial that gets recommended regularly has a servo, but only one and in a sweep motion. I can't make out how to apply that to this issue.

Any help would be appreciated

#include <Servo.h>
Servo leftServo;
Servo rightServo;

int leftServoUpPos = 130; //CCW
int leftServoDownPos = 90; //CW
int waitLeftServoUp = 555;
int waitLeftServoDown = 555;

int rightServoUpPos = 60; //CCW
int rightServoDownPos = 100; //CW
int waitRightServoUp = 555;
int waitRightServoDown = 555;

unsigned long previousLeftMillis = 0;
unsigned long previousRightMillis = 0;
const long leftInterval = 2000;   //activate left servo every 2 seconds
const long rightInterval = 3000;  //activate right servo every 3 seconds

unsigned long leftServoTimer;
unsigned long rightServoTimer;

void setup() {
}

void loop() {
  unsigned long currentLeftMillis = millis();
  if (currentLeftMillis - previousLeftMillis >= leftInterval) {
    previousLeftMillis = currentLeftMillis;
    leftServoMove();
  }
  unsigned long currentRightMillis = millis();
  if (currentRightMillis - previousRightMillis >= rightInterval) {
    previousRightMillis = currentRightMillis;
    rightServoMove();
  }
}

void leftServoMove() {
  leftServo.attach(9);
  leftServo.write(leftServoDownPos);
  delay(waitLeftServoDown);
  leftServo.write(leftServoUpPos);
  delay(waitLeftServoUp);
  leftServo.write(leftServoDownPos);
  delay(waitLeftServoDown);
  leftServoTimer = millis();
}

void rightServoMove() {
  rightServo.attach(10);
  rightServo.write(rightServoDownPos);
  delay(waitRightServoDown);
  rightServo.write(rightServoUpPos);
  delay(waitRightServoUp);
  rightServo.write(rightServoDownPos);
  delay(waitRightServoDown);
  rightServo.detach();
  rightServoTimer = millis();
}
2

Most of the time, you use millis() to replace delay(). Using delay() together will mess up using millis() as timing.

3

Duh! I can't believe I did that!! Thanks -- I just needed someone to see that because I know that, but just didn't see it! ...Something about not seeing the forest for the trees...

4

OK, I'm trying to replace those delays with millis but I am lost. I hope someone can help set me in the right direction (some sample code would be really helpful).

Here's my somewhat revised code:

#include <Servo.h>
Servo leftServo;
Servo rightServo;

int leftServoUpPos = 130; //CCW
int leftServoDownPos = 90; //CW
int waitLeftServoUp = 555;
int waitLeftServoDown = 555;

int rightServoUpPos = 60; //CCW
int rightServoDownPos = 100; //CW
int waitRightServoUp = 555;
int waitRightServoDown = 555;

unsigned long currentMillis = 0;    // stores the value of millis() in each iteration of loop()
unsigned long previousLeftMillis = 0;
unsigned long previousRightMillis = 0;
unsigned long previousLeftServoMillis = 0; // the time when the servo was last moved
const long leftServoInterval = 2000;   //activate left servo every 2 seconds
const long rightServoInterval = 3000;  //activate right servo every 3 seconds

unsigned long leftServoTimer;
unsigned long rightServoTimer;

void setup() {
}

void loop() {
  currentMillis = millis();
  leftServoMove();
  //rightServoMove();
}


void leftServoMove() {
  leftServo.attach(9);
  if (currentMillis - previousLeftServoMillis >= leftServoInterval) {
    previousLeftServoMillis = leftServoInterval;
    leftServo.write(leftServoDownPos);
    // delay(waitLeftServoDown);
    leftServo.write(leftServoUpPos);
    // delay(waitLeftServoUp);
    leftServo.write(leftServoDownPos);
    //  delay(waitLeftServoDown);
    leftServoTimer = millis();
  }
}


void rightServoMove() {
  rightServo.attach(10);
  rightServo.write(rightServoDownPos);
  // delay(waitRightServoDown);
  rightServo.write(rightServoUpPos);
  // delay(waitRightServoUp);
  rightServo.write(rightServoDownPos);
  // delay(waitRightServoDown);
  rightServo.detach();
  rightServoTimer = millis();
}
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.

6

Wow! That's some code! I really appreciate your effort and hope you pulled that from somewhere and didn't spend too much time on it.

But is that much complicated code really required to move two little servos?!

I'm going to want to vary the code for lots of little projects like this, so I'm going to need to understand it and be able to modify it. There's no way that would happen with the example you provided.

So thank you! But isn't there a much simpler solution?

7

callmebob:
So thank you! But isn't there a much simpler solution?

If you ignore the class definition (it needn’t change at all) the rest of the code is a breeze and very usable.

I have a library version that will abstract away all of the class Mumbo-Jumbo if you want.

I wrote that class for a project I did a while back and where I wanted a non-blocking extension to the servo class, much like you want.

8

You could use the servo function in Several Things at a Time and make a second copy of the function with a different name for your second servo.

...R

9

Bulldog and Robin are pointing you in the right direction.

Don’t look at the amount of source code, most of that bulk gets optimised away during compilation.

If you think of the eyebrows as ‘actions’, you may be able to setup your code as a state machine for each eyebrow servo.
Then simply setup the timing, and ‘start’ each eyebrow, and let the state machine take them through the required steps independently.

If you don’t need to vary the timing - even easier ! Just start when needed.
Of course you could also make it more complex - setting the rest positions, hence the ‘expression’ of each action sequence could be adjusted.

10

Thanks guys! I've gone with Robin's suggestion and tutorial and both eyebrows (servos) work!! They both move in the same direction and sweep too far, so now I just have to solve those two issues... wish me luck...

11

OK, I got both servos moving the amount I want them to move, but I need to reverse the right servo. I've tried for a few hours now and can't figure it out. Can someone help me to reverse it?

#include <Servo.h>

// --------CONSTANTS (won't change)---------------

const int onBoardLedPin   ;      // the pin numbers for the LEDs
const int led_A_Pin = 12;
const int led_B_Pin = 11;
const int buttonLed_Pin = 13;

const int buttonPin = 2; // the pin number for the button

const int onBoardLedInterval = 500; // number of millisecs between blinks
const int led_A_Interval = 2500;
const int led_B_Interval = 4500;

const int blinkDuration = 500; // number of millisecs that Led's are on - all three leds use this

const int buttonInterval = 300; // number of millisecs between button readings

const int leftServoPin = 9; // the pin number for the servo signal
const int rightServoPin = 10; // the pin number for the servo signal

const int leftServoMinDegrees = 90; // the limits to servo movement
const int leftServoMaxDegrees = 120;
const int rightServoMinDegrees = 90; // the limits to servo movement
const int rightServoMaxDegrees = 120;

Servo leftServo;
Servo rightServo;

//------------ VARIABLES (will change)---------------------

byte onBoardLedState = LOW;             // used to record whether the LEDs are on or off
byte led_A_State = LOW;           //   LOW = off
byte led_B_State = LOW;
byte buttonLed_State = LOW;


int leftServoPosition = 90;     // the current angle of the servo - starting at 90.
int leftServoSlowInterval = 50; // millisecs between servo moves
int leftServoFastInterval = 0;
int leftServoInterval = leftServoSlowInterval; // initial millisecs between servo moves
int leftServoDegrees = 1;       // amount servo moves at each step
// will be changed to negative value for movement in the other direction

int rightServoPosition = 90;     // the current angle of the servo - starting at 90.
int rightServoSlowInterval = 50; // millisecs between servo moves
int rightServoFastInterval = 50;
int rightServoInterval = rightServoSlowInterval; // initial millisecs between servo moves
int rightServoDegrees = 1;       // amount servo moves at each step
//    will be changed to negative value for movement in the other direction

unsigned long currentMillis = 0;    // stores the value of millis() in each
unsigned long leftPreviousServoMillis = 0; // the time when the servo was last moved
unsigned long rightPreviousServoMillis = 0; 

unsigned long previousOnBoardLedMillis = 0;   // will store last time the LED was updated
unsigned long previousLed_A_Millis = 0;
unsigned long previousLed_B_Millis = 0;

unsigned long previousButtonMillis = 0; // time when button press last checked


void setup() {

  pinMode(onBoardLedPin, OUTPUT);
  pinMode(led_A_Pin, OUTPUT);
  pinMode(led_B_Pin, OUTPUT);
  pinMode(buttonLed_Pin, OUTPUT);

  leftServo.write(leftServoPosition); // sets the initial position
  rightServo.write(rightServoPosition); // sets the initial position
  leftServo.attach(leftServoPin);
  rightServo.attach(rightServoPin);


  pinMode(buttonPin, INPUT_PULLUP);
}

void loop() {

  currentMillis = millis();   // capture the latest value of millis()
  //   this is equivalent to noting the time from a clock
  //   use the same time for all LED flashes to keep them synchronized

  readButton();               // call the functions that do the work
  updateOnBoardLedState();
  updateLed_A_State();
  updateLed_B_State();
  switchLeds();

  leftServoSweep();
  rightServoSweep();
}

void leftServoSweep() {    /////////////////////////////////////////////////////

  // nothing happens unless the interval has expired
  // the value of currentMillis was set in loop()

  if (currentMillis - leftPreviousServoMillis >= leftServoInterval) {
    // its time for another move
    leftPreviousServoMillis = leftPreviousServoMillis + leftServoInterval;  //increments leftServoInterval

    leftServoPosition = leftServoPosition + leftServoDegrees; // leftServoDegrees might be negative

    if ((leftServoPosition >= leftServoMaxDegrees) || (leftServoPosition <= leftServoMinDegrees))  {
      // if the servo is at either extreme change the sign of the degrees to make it move the other way
      leftServoDegrees = - leftServoDegrees; // reverse direction
      // and update the position to ensure it is within range
      leftServoPosition = leftServoPosition + leftServoDegrees;
    }

    // make the servo move to the next position
    leftServo.write(leftServoPosition);
    // and record the time when the move happened
  }
}

void rightServoSweep() {   //////////////////////////////////////

  // this is similar to the servo sweep example except that it uses millis() rather than delay()

  // nothing happens unless the interval has expired
  // the value of currentMillis was set in loop()

  if (currentMillis - rightPreviousServoMillis >= rightServoInterval) {
    // its time for another move
    rightPreviousServoMillis += rightServoInterval;

    rightServoPosition = rightServoPosition + rightServoDegrees; // servoDegrees might be negative

    if (rightServoPosition <= rightServoMinDegrees) {
      // when the servo gets to its minimum position change the interval to change the speed
      if (rightServoInterval == rightServoSlowInterval) {
        rightServoInterval = rightServoFastInterval;
      }
      else {
        rightServoInterval = rightServoSlowInterval;
      }
    }

    if ((rightServoPosition >= rightServoMaxDegrees) || (rightServoPosition <= rightServoMinDegrees))  {
      // if the servo is at either extreme change the sign of the degrees to make it move the other way
      rightServoDegrees = - rightServoDegrees; // reverse direction
      // and update the position to ensure it is within range
      rightServoPosition = rightServoPosition + rightServoDegrees;
    }

    // make the servo move to the next position
    rightServo.write(rightServoPosition);
    // and record the time when the move happened
  }
}

void updateOnBoardLedState() {

  if (onBoardLedState == LOW) {
    // if the Led is off, we must wait for the interval to expire before turning it on
    if (currentMillis - previousOnBoardLedMillis >= onBoardLedInterval) {
      // time is up, so change the state to HIGH
      onBoardLedState = HIGH;
      // and save the time when we made the change
      previousOnBoardLedMillis += onBoardLedInterval;
      // NOTE: The previous line could alternatively be
      //              previousOnBoardLedMillis = currentMillis
      //        which is the style used in the BlinkWithoutDelay example sketch
      //        Adding on the interval is a better way to ensure that succesive periods are identical

    }
  }
  else {  // i.e. if onBoardLedState is HIGH

    // if the Led is on, we must wait for the duration to expire before turning it off
    if (currentMillis - previousOnBoardLedMillis >= blinkDuration) {
      // time is up, so change the state to LOW
      onBoardLedState = LOW;
      // and save the time when we made the change
      previousOnBoardLedMillis += blinkDuration;
    }
  }
}

//========================================

void updateLed_A_State() {

  if (led_A_State == LOW) {
    if (currentMillis - previousLed_A_Millis >= led_A_Interval) {
      led_A_State = HIGH;
      previousLed_A_Millis += led_A_Interval;
    }
  }
  else {
    if (currentMillis - previousLed_A_Millis >= blinkDuration) {
      led_A_State = LOW;
      previousLed_A_Millis += blinkDuration;
    }
  }
}

//========================================

void updateLed_B_State() {

  if (led_B_State == LOW) {
    if (currentMillis - previousLed_B_Millis >= led_B_Interval) {
      led_B_State = HIGH;
      previousLed_B_Millis += led_B_Interval;
    }
  }
  else {
    if (currentMillis - previousLed_B_Millis >= blinkDuration) {
      led_B_State = LOW;
      previousLed_B_Millis += blinkDuration;
    }
  }
}

//========================================

void switchLeds() {
  // this is the code that actually switches the LEDs on and off

  digitalWrite(onBoardLedPin, onBoardLedState);
  digitalWrite(led_A_Pin, led_A_State);
  digitalWrite(led_B_Pin, led_B_State);
  digitalWrite(buttonLed_Pin, buttonLed_State);
}

//========================================

void readButton() {


  if (millis() - previousButtonMillis >= buttonInterval) {

    if (digitalRead(buttonPin) == LOW) {
      buttonLed_State = ! buttonLed_State; // this changes it to LOW if it was HIGH
      //   and to HIGH if it was LOW
      previousButtonMillis += buttonInterval;
    }
  }

}
12

The code looks a bit complicated, but since the servos move independently, you can keep separate direction flags, and toggle them when each servo reaches a limit...
Then use that flag to increment or decrement as appropriate during the run phase.

13

One way to reverse a servo is to use this
rightServo.write(180 - rightServoPosition)

14

vinceherman:
One way to reverse a servo is to use this
rightServo.write(180 - rightServoPosition)

I think that should be
leftServo.write(180 - rightServoPosition)

...R

15

Robin2:
I think that should be
leftServo.write(180 - rightServoPosition)

...R

yes, as demonstrated way back in post #4!

:wink:

16

180-x
is good for synchronous eyebrows... no disagreement, but my hints were in anticipation of asynchronous ‘emotive’ brows...,

17

Wow! That crazy little simple line of code... After pulling my hair out for three hours!! I envy you programmer wizards. Thank you guys for that! Sorry, BulldogLowell -- your suggested code was just way too sophisticated for me to have seen, and been able to use, that line...

I didn't know where to put it so just pasted it in at the top of my rightServoSweep() function and it worked! I was just about to give up on software and just use two Nanos (which may still be on my list for the next steps...)

But yes, lastchancename, I'm hoping for asynchronous eyebrows too. I tried using a flag as you suggested below but couldn't figure out how to make it work.

lastchancename:
The code looks a bit complicated, but since the servos move independently, you can keep separate direction flags, and toggle them when each servo reaches a limit...
Then use that flag to increment or decrement as appropriate during the run phase.

Now that I have them both going up and down together my next self-punishment will be to try to make them asynchronous, and only go up and down a couple of times.

Any help beforehand would surely be appreciated. (lastchancename, if your suggestion is the solution, could you explain a little more about how I would use the flag? -- I haven't had much luck with flags so far.

On a related, but different flag mission, I want to stop the ongoing sweep of the servos, so I set a 'count=1' flag at the top of setup() and set it to 0 at the bottom of one of the servo functions to just stop that servo, but that didn't work).

I'm sure I'll be back soon...

P.S. I hopefully didn't need to say it, but the code besides the servo stuff can be ignored. I just wanted to include all of the code (posting it was blocked several times saying I exceeded the 9,000 character limit, so I had to trim out some comments).

18

Well, decide how far you want each eyebrow to go, and how fast.
Then, you can start them together, or whoever you want.

For example you could -

  1. Have one go all the way up, and the other half way (quizzical)
    Then bring them down together.

  2. One up, one down, alternating (parlour trick) alternating a couple of times.

The flag simply says ‘starting from where I am (here), I want to move this far in this direction (there), but if you write the code differently, you can do this a completely different way.

Create a function that simply takes the target position.
Keep a static (i.e. persistent) current position - then the function simply manages the run from ‘here’ to ‘there’.

For best effect, use a millis() ticker to update the position steps, then you have control over speed, and other stuff can happen at the same time!