#include <Servo.h>
// Define a structure to hold all data for a single servo
struct ServoMotor {
int pin; // The physical Arduino pin
int currentAngle; // The current position of the servo
int targetAngle; // The position the servo wants to move to
int stepSpeed; // How fast the servo moves (degrees per tick)
Servo servoObj; // The actual library object instances
};
// Create an array of 3 servos with their unique configuration data
ServoMotor myServos[] = {
// pin, currentAngle, targetAngle, stepSpeed, servoObj
{ 4, 0, 180, 2, Servo()}, // Servo 1 (e.g., Base)
{ 5, 0, 90, 1, Servo()}, // Servo 2 (e.g., Shoulder)
{ 6, 0, 95, 3, Servo()}, // Servo 3 (e.g., Gripper)
{ 7, 0, 90, 3, Servo()}, // Servo 4 (e.g., Gripper)
{ 8, 0, 90, 3, Servo()}, // Servo 5 (e.g., Gripper)
{9, 0, 180, 2, Servo()}, // Servo 6 (e.g., Base)
{10, 0, 90, 1, Servo()}, // Servo 7 (e.g., Shoulder)
{11, 0, 95, 3, Servo()}, // Servo 8 (e.g., Gripper)
{12, 0, 90, 3, Servo()} // Servo 9 (e.g., Gripper)
};
// Dynamically calculate the total number#include <Servo.h> of servos in the array
const int NUM_SERVOS = sizeof(myServos) / sizeof(myServos[0]);
void setup() {
// Loop through the array to initialize each servo
for (int i = 0; i < NUM_SERVOS; i++) {
myServos[i].servoObj.attach(myServos[i].pin);
myServos[i].servoObj.write(myServos[i].currentAngle); // Move to initial starting position
}
}
void loop() {
// Example 1: Move all servos to their targets smoothly simultaneously
moveServosSmoothly();
// Example 2: Change targets conditionally
if (millis() > 5000 && myServos[0].targetAngle == 180) {
myServos[0].targetAngle = 0; // Update only the first servo's target
myServos[1].targetAngle = 0; // Update only the second servo's target
myServos[2].targetAngle = 0;
myServos[3].targetAngle = 0;
myServos[4].targetAngle = 0;
myServos[5].targetAngle = 0;
myServos[6].targetAngle = 0;
myServos[7].targetAngle = 0;
myServos[8].targetAngle = 0;
}
delay(20); // Dictates the global refresh rate (~50Hz)
}
// A reusable function that sweeps all servos toward their targets non-blockingly
void moveServosSmoothly() {
for (int i = 0; i < NUM_SERVOS; i++) {
// If the servo hasn't reached its target, step it forward or backward
if (myServos[i].currentAngle < myServos[i].targetAngle) {
myServos[i].currentAngle += myServos[i].stepSpeed;
if (myServos[i].currentAngle > myServos[i].targetAngle) {
myServos[i].currentAngle = myServos[i].targetAngle; // Prevent overshoot
}
} else if (myServos[i].currentAngle > myServos[i].targetAngle) {
myServos[i].currentAngle -= myServos[i].stepSpeed;
if (myServos[i].currentAngle < myServos[i].targetAngle) {
myServos[i].currentAngle = myServos[i].targetAngle; // Prevent undershoot
}
}
// Write the new updated position to the physical servo hardware
myServos[i].servoObj.write(myServos[i].currentAngle);
}
}