How to Independently run positional and continuous servos using millis()?

Question

I am setting up 3 servos (2 position servos and 1 '360' continuous rotational servo) with Arduino. but I failed to run these independently. It runs one after another. I wish to run it in loop independent to each other. Here is the code which I use to run positional servo and CR (360) servo. default servo positions are 90. *CR 360 servo should be running independently 90-60-120-90 (in loop) *Pos servo should be running one after another (in loop)

To run positional servos (tilt/pan):

//tilt servo
    for (pos = 90; pos >= 60; pos -= 1)     //90 ~ 60
  {
    tiltservo.write(pos);
    delay(30);
  }

  for (pos = 60; pos <= 120; pos += 1)      //60 ~ 120
  {
    tiltservo.write(pos);
    delay(30);
  }

  for (pos = 120; pos >= 90; pos -= 1)       //120 ~ 90
  {
    tiltservo.write(pos);
    delay(30);
  }

//pan servo
for (pos = 90; pos >= 60; pos -= 1)          //90 ~ 60
  {        
    panservo.write(pos);
    delay(30);
  }

  for (pos = 60; pos <= 120; pos += 1)       //60 ~ 120
  {        
    panservo.write(pos);
    delay(30);
  }

  for (pos = 120; pos >= 90; pos -= 1)       //120 ~ 90
  {        
    panservo.write(pos);
    delay(30);
  }

To run CR 360 servo:

 legservo.writeMicroseconds(1600);      //CW
 delay(5000);                           //run for 5s
 legservo.writeMicroseconds(1300);      //CCW
 delay(5000);                           //run for 5s 

Tried this to independently run CR servos, but how to put some wait (3s-4s) in between FW and RW.

if (millisNow - lastMillis >= 10000) 
{ 
lastMillis = millisNow; 
if (crservoState == 0) 
 { 
   legservo1.writeMicroseconds(2150); //FW
   crservoState = 1; 
 } 
  else 
 { 
   legservo1.writeMicroseconds(850);  //RW
   crservoState = 0; 
 } 
} 

Trial code after suggestions:

uint32_t milliSNow = millis();

  if (milliSNow - lastTimeServoRotate >= 1000)
  {
    if (crservoState == 0)
    {
      if (milliSNow - lastTimeServoRotate >= 10000)
      {
        legservo1.writeMicroseconds(2150);    //FW run
        crservoState = 2;
        lastTimeServoRotate = milliSNow;
      }
    }

    if (crservoState == 1)
    {
      if (milliSNow - lastTimeServoRotate >= 10000)
      {
        legservo1.writeMicroseconds(850);     //RW run
        crservoState = 3;
        lastTimeServoRotate = milliSNow;
      }
    }

    if (crservoState == 2)
    {
      if (milliSNow - lastTimeServoRotate >= 3000)
      {
        legservo1.writeMicroseconds(1500);    //FW STOP
        crservoState = 1;
        lastTimeServoRotate = milliSNow;
      }
    }

    if (crservoState == 3)
    {
      if (milliSNow - lastTimeServoRotate >= 3000)
      {
        legservo1.writeMicroseconds(1500);     //RW STOP
        crservoState = 0;
        lastTimeServoRotate = milliSNow;
      }
    }
  }

Answer 1

This is covered in the Blink Without Delay Arduino tutorial. Let me paraphrase the main part of the example code here:

uint8_t ledState = LOW;
uint32_t lastTimeLedChanged = 0;

void loop() {
    uint32_t now = millis();
    if (now - lastTimeLedChanged >= 1000) {

        // Save the last time we updated the LED.
        lastTimeLedChanged = now;

        // Compute the new state of the LED.
        if (ledState == LOW) {
            ledState = HIGH;
        } else {  // ledState is HIGH
            ledState = LOW;
        }

        // Set the LED to the computed state.
        digitalWrite(LED_BUILTIN, ledState);
    }
}

This will make the LED cycle between its two possible states at the desired rate. You can apply this principle as-is to your continuous rotation servo.

For the positional servos, the same principle can still be applied, but there is a catch. Whereas both the LED and the CR servo cycle between two states, the pair of positional servos goes through many states (92 240 if I counted correctly). You will thus need:

• more variables to store the current state of the servo pair
• more code to compute the new desired state
• more code to update the state

In order to figure out how to represent the state, you have to look at your current delay()-based code, see how many calls to delay() are performed on each full cycle, and figure out how to identify each of these calls. Each of these delays corresponds to one state of your system. I would say that you need one variable to know in which of the for loops you are, and another variable to know the last position you wrote to the servo in that loop. I will call each of the original for loops a “phase”. There are two six such phases:

• phase 0: tilt: 90° → 60°
• phase 1: tilt: 60° → 120°
• phase 2: tilt: 120° → 90°
• phase 3: pan: 90° → 60°
• phase 4: pan: 60° → 120°
• phase 5: pan: 120° → 90°

Note there were only two for loops in the original question, but it was then edited to include six loops, thus I edited my answer.

An then you will need to store the time of the last update, thus:

int phase = 0;  // which of the six phases we are in
int pos = 90;   // last position written to a servo
uint32_t lastTimeServoMoved = 0;

Once you have the proper variables to describe the state, updating it is not hard. If you have reached the end of a phase, move to the next one. Then update the position by one step. Thus:

void loop() {
    uint32_t now = millis();
    if (now - lastTimeServoMoved >= 30) {

        // Save the last time we updated a positional servo.
        lastTimeServoMoved = now;

        // Compute the new state.        
        if (phase == 0 && pos == 60)
            phase = 1;
        else if (phase == 1 && pos == 120)
            phase = 2;
        else if (phase == 2 && pos == 90)
            phase = 3;
        else if (phase == 3 && pos == 60)
            phase = 4;
        else if (phase == 4 && pos == 120)
            phase = 5;
        else if (phase == 5 && pos == 90)
            phase = 0;
        if (phase == 1 || phase == 4)
            pos += 1;
        else
            pos -= 1;

        // Set the servo to the computed state.
        if (phase < 3)
            tiltservo.write(pos);
        else
            panservo.write(pos);
    }
}

I personally do not like this kind of repetitive code. If you are comfortable with data structures, I would recommend using an array of structs to store all the information describing the moves that are done in each phase:

struct {
    Servo *servo;   // which servo to move
    int step;       // steps by which it moves
    int final_pos;  // final angular position
} moves[] = {
    { &tiltservo, -1, 60 },   // tilt:  90 ->  60
    { &tiltservo, +1, 120 },  // tilt:  60 -> 120
    { &tiltservo, -1, 90 },   // tilt: 120 ->  90
    { &panservo,  -1, 60 },   // pan:   90 ->  60
    { &panservo,  +1, 120 },  // pan:   60 -> 120
    { &panservo,  -1, 90 }    // pan:  120 ->  90
};

Then the actual code becomes simpler:

void loop() {
    uint32_t now = millis();
    if (now - lastTimeServoMoved >= 30) {

        // Save the last time we updated a positional servo.
        lastTimeServoMoved = now;

        // Compute the new state.
        if (pos == moves[phase].final_pos) {
            phase +=1;
            if (phase == 6)
                phase = 0;
        }
        pos += moves[phase].step;

        // Set the servo to the computed state.
        moves[phase].servo->write(pos);
    }
}