#include <Servo.h>
unsigned long previousMillis = 0;
const unsigned long interval = 20; // 10 or 20 milliseconds are common values
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
Servo servo5;
float position = 90.0; // Position of the 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();
// Check if it's time to update the servo
if (currentMillis - previousMillis >= interval)
{
previousMillis = currentMillis;
//------------------------------------------
// 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;
float distance = fabs(90.0 - position); // Distance from the center
step = (90.0 - distance) / 90.0; // Calculate step size based on position
step = 0.2 + (pow(step, 1) * 3); // Brake PWR 0.1=High - traveling time - speed
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);
}
}