Hi, I have a project where I want to control 80 SG90 microservos, using 5 pca9685 servo drivers. The goal of the project is for the servos to move simultanously in a sweeping motion, back and forth.
I have been successful with my code in getting all the servos to move. The problem is that the more servos that are added to the code the slower the speed. For example; when I increase from 1 servo to 16 servos the speed is doubled. When the code accounts for all 80 servos they move, but a lot slower than what I would expect them to compared with the speed when I only have 1 servo added. The speed is noticably slower and moves only a tiny bit faster if I decrease my Sweep_Step_Ms.
I have been using the Arduino Uno, but tried to switch to a Teensy 4.0 to see if the increased processing power would have an affect - it did not have a noticable affect.
The speed is not affected by how many servos are physically connected to the drivers. I am also using a 5V, 10A power source individually connected to each driver, so I dont think lack of power/amp is the problem.
I tried making a scemathic the best of my abbility, hopefully it is possible to read/understand.
And here is the code I have been using:
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <elapsedMillis.h>
#define SERVOS_PER_BOARD 16
#define NUM_BOARDS 5
#define TOTAL_SERVOS (SERVOS_PER_BOARD * NUM_BOARDS)
#define SERVO_MIN 200
#define SERVO_MAX 450
#define SERVO_FREQ 50 // in Hz
#define OSC_FREQ 27000000UL
#define SWEEP_STEP_MS 10 // milliseconds between each pulse step
#define SWEEP_PAUSE_MS 500 // milliseconds to pause between sweep directions
Adafruit_PWMServoDriver boards[NUM_BOARDS] = {
Adafruit_PWMServoDriver(0x40),
Adafruit_PWMServoDriver(0x41),
Adafruit_PWMServoDriver(0x42),
Adafruit_PWMServoDriver(0x43),
Adafruit_PWMServoDriver(0x44)
};
elapsedMillis sweepTimer;
elapsedMillis pauseTimer;
uint16_t currentPulse = SERVO_MAX; // Start at max position
bool sweepingDown = true; // true = MAX→MIN, false = MIN→MAX
bool inPause = false; // true while waiting between sweeps
const int BAUD_RATE = 9600;
const uint32_t I2C_CLOCK_FREQ = 400000;
bool initBoards();
void setServoPWM(uint8_t servoIndex, uint16_t pulselen);
// ---------------------------------------------------------------------------
// Board initialization
// ---------------------------------------------------------------------------
bool initBoards() {
for (uint8_t i = 0; i < NUM_BOARDS; i++) {
boards[i].begin();
boards[i].setOscillatorFrequency(OSC_FREQ);
boards[i].setPWMFreq(SERVO_FREQ);
}
return true;
}
void setServoPWM(uint8_t servoIndex, uint16_t pulselen) {
// Bounds guard: silently ignore out of range indices rather than sending garbage to a nonexistent board channel
if (servoIndex >= TOTAL_SERVOS) return;
uint8_t boardIndex = servoIndex / SERVOS_PER_BOARD;
uint8_t channel = servoIndex % SERVOS_PER_BOARD;
boards[boardIndex].setPWM(channel, 0, pulselen);
}
// ---------------------------------------------------------------------------
// Setup
// ---------------------------------------------------------------------------
void setup() {
Serial.begin(BAUD_RATE);
Wire.setClock(I2C_CLOCK_FREQ);
Serial.println(F("Initializing 80-servo controller..."));
if (!initBoards()) {
Serial.println(F("ERROR: Board initialization failed. Check wiring and I2C addresses."));
while (1)
; // Halt — no point continuing if drivers aren't responding
}
Serial.println(F("All boards initialized. Starting sweep."));
// Move all servos to the starting position before the loop begins, so they don't jump unpredictably
for (uint8_t s = 0; s < TOTAL_SERVOS; s++) {
setServoPWM(s, currentPulse);
}
//reset timer
sweepTimer = 0;
}
// ---------------------------------------------------------------------------
// Main loop
// ---------------------------------------------------------------------------
/**
* Sweeps all 80 servos back and forth between SERVO_MIN and SERVO_MAX, one pulse-width tick per SWEEP_STEP_MS interval, with a pause of SWEEP_PAUSE_MS between direction changes.
*/
void loop() {
// Pausing
if (inPause) {
if (pauseTimer >= SWEEP_PAUSE_MS) {
inPause = false;
sweepTimer = 0; // Reset sweep timer so next step starts cleanly
}
return; // Nothing else to do while pausing
}
// Sweeping down/up
if (sweepTimer >= SWEEP_STEP_MS) {
sweepTimer = 0;
// Apply the current PWM to every servo
for (uint8_t s = 0; s < TOTAL_SERVOS; s++) {
setServoPWM(s, currentPulse);
}
if (sweepingDown) {
// Moving MAX to MIN
if (currentPulse > SERVO_MIN) {
currentPulse--;
} else {
// Hit the bottom, flip direction and pause
Serial.println(F("Reached MIN. Reversing..."));
sweepingDown = false;
inPause = true;
pauseTimer = 0;
}
} else {
// Moving MIN to MAX
if (currentPulse < SERVO_MAX) {
currentPulse++;
} else {
// Hit the top, flip direction and pause
Serial.println(F("Reached MAX. Reversing..."));
sweepingDown = true;
inPause = true;
pauseTimer = 0;
}
}
}
}
I have also made and tried another code where I worked more from the Adafruit Pwm Servo driver "servo" example, which I can upload if it makes sense to. But I have met the same issue with that code.
I am a beginner, and this is my first post in this forum, all help is extremely appreciated! Thank you in advance.
