// Controlling multiple servo motors,
// by updating them every 10ms or 20ms.
//
// Version 1, July 25, 2021 by Koepel, Public Domain.
// Version 2, October 7, 2023 by Koepel, Public Domain.
//   Added pinMode() to setup() for the two buttons.

#include <Servo.h>

unsigned long previousMillis;
const unsigned long interval = 10;   // 10 or 20 milliseconds are common values

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

int count700ms = 0;    // count to 700ms for first servo motor

float r = 0.0;         // the angle for the sinus in radian for second servo motor

int step = 0;          // the step of the angle for fourth servo motor
int pos = 90;          // posistion of fourth servo motor

float position = 90.0; // position of fifth servo motor
int direction = 1;     // direction for fifth servo motor

void setup() 
{
  pinMode(2, INPUT_PULLUP);
  pinMode(3, INPUT_PULLUP);
  servo1.attach(8);
  servo2.attach(7);
  servo3.attach(6);
  servo4.attach(5);
  servo5.attach(4);
}

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

  if( currentMillis - previousMillis >= interval)
  {
    previousMillis = currentMillis;

    // ------------------------------------------
    // Servo motor 1
    // Move to a random position every 700ms
    // ------------------------------------------
    count700ms++;
    if( count700ms >= (700 / interval))
    {
      count700ms = 0;
      int angle1 = random( 0, 181);    // 0...180
      servo1.write( angle1);
    }

    // ------------------------------------------
    // Servo motor 2
    // A smooth sinus sweep
    // The speed is controlled by the first potentiometer
    // ------------------------------------------
    float s = sin( r);               // The sine from the angle in radian
    float t = (90.0 * s) + 90.0;     // convert -1...1 to 0...180
    int angle2 = int( t);            // convert it to integer for the servo angle
    servo2.write( angle2);

    int potentiometer1 = analogRead( A0);
    float r_increment = 0.001 + (0.00005 * float(potentiometer1));

    r += r_increment;
    if( r >= 2.0 * M_PI)
    {
      r -= 2.0 * M_PI;
    }

    // ------------------------------------------
    // Servo motor 3
    // Angle controlled by the second potentiometer
    // ------------------------------------------
    int potentiometer2 = analogRead( A1);
    int angle3 = map( potentiometer2, 0, 1023, 0, 180);
    servo3.write( angle3);

    // ------------------------------------------
    // Servo motor 4
    // Controlled by the buttons, will continue to 0 or 180.
    // ------------------------------------------
    bool up = false;
    if( digitalRead( 3) == LOW)
      up = true;
    bool down = false;
    if( digitalRead( 2) == LOW)
      down = true;

    if( step == 0 and up)
      step = 1;
    else if( step == 0 and down)
      step = -1;
    else if( step > 0 and down)
      step = -1;
    else if( step < 0 and up)
      step = +1;
    
    pos += step;
    if( pos <= 0)
    {
      pos = 0;
      step = 0;
    }
    else if( pos >= 180)
    {
      pos = 180;
      step = 0;
    }

    servo4.write( pos); 

    // ------------------------------------------
    // Servo motor 5
    // Using math to slow down the servo near the endpoints.
    // The position is a float variable to be able to move
    // really slow near the endpoints.
    // ------------------------------------------
    float step;
    step = (90.0 - fabs(90.0 - position)) / 90.0;
    step = 0.1 + (pow( step, 1.5) * 5);  // make a steep curve, with 0.1 as minimum
    
    if( direction > 0)
    {
      position += step;
      if( position >= 180.0)
      {
        position = 180.0;
        direction = -1;
      }
    }
    else
    {
      position -= step;
      if( position <= 0.0)
      {
        position = 0.0;
        direction = 1;
      }
    }
    int angle5 = int( position);
    servo5.write( angle5);
  }
}
uno:A5.2
uno:A4.2
uno:AREF
uno:GND.1
uno:13
uno:12
uno:11
uno:10
uno:9
uno:8
uno:7
uno:6
uno:5
uno:4
uno:3
uno:2
uno:1
uno:0
uno:IOREF
uno:RESET
uno:3.3V
uno:5V
uno:GND.2
uno:GND.3
uno:VIN
uno:A0
uno:A1
uno:A2
uno:A3
uno:A4
uno:A5
servo1:GND
servo1:V+
servo1:PWM
servo2:GND
servo2:V+
servo2:PWM
servo3:GND
servo3:V+
servo3:PWM
servo4:GND
servo4:V+
servo4:PWM
servo5:GND
servo5:V+
servo5:PWM
pot1:GND
pot1:SIG
pot1:VCC
pot2:GND
pot2:SIG
pot2:VCC
btn1:1.l
btn1:2.l
btn1:1.r
btn1:2.r
btn2:1.l
btn2:2.l
btn2:1.r
btn2:2.r
r1:1
r1:2
r2:1
r2:2