// 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);
  }
}