// For: https://forum.arduino.cc/t/how-do-i-code-the-motion-in-this-animation-using-4-servo-motors/1037603/

#include <Servo.h>

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;

float seconds = 0.0;                 // walk from 0 to 20 seconds
float maxSeconds = 20.0;             // maximum number of seconds for sequence    

unsigned long previousMillis;
const unsigned long interval = 20;  // interval for millis timer

void setup() 
{
  servo1.attach(1);
  servo2.attach(3);
  servo3.attach(5);
  servo4.attach(7);
  servo5.attach(9);

  // Set initial positions
  servo1.write(180);
  servo2.write(180);
  servo3.write(180);
  servo4.write(180);
  servo5.write(180);
}

void loop() 
{
  unsigned long currentMillis = millis();

  // Make a millis timer
  if (currentMillis - previousMillis >= interval)
  {
    previousMillis = currentMillis;

    // The seconds go from 0 to 20, then it restarts at 0, and so on.
    seconds += (float) interval / 1000.0;   // interval is in milliseconds
    if( seconds >= maxSeconds)              // start over again ?
      seconds = 0.0;

    // --------------------------------------------------
    // servo1
    // --------------------------------------------------
    if( seconds >= 1.0 and seconds < 3.0)
    {
      // From 1 to 3 seconds, servo1 from 180 to 80
      // time range is 2 seconds. time offset is 1 second
      // servo angle range is 100 degrees
      // 
      // From seconds to angle: 
      //   divide by time range, multiply by angle range, solve offset
      float pos = 180.0 - ((seconds - 1.0) / 2.0 * 100.0);
      servo1.write((int) pos);
    }
    else if( seconds >= 3.0 and seconds < 5.0)
    {
      // from 3 to 5 seconds servo 1 from 80 to 180
      float pos = 80.0 + ((seconds - 3.0) / 2.0 * 100.0);
      servo1.write((int) pos);
    }
    else if( seconds >= 16.0 and seconds < 18.0)
    {
      float pos = 180.0 - ((seconds - 16.0) / 2.0 * 100.0);
      servo1.write((int) pos);
    }
    else if( seconds >= 18.0 and seconds < 20.0)
    {
      float pos = 80.0 + ((seconds - 18.0) / 2.0 * 100.0);
      servo1.write((int) pos);
    }

    // --------------------------------------------------
    // servo2
    // --------------------------------------------------
  }
}