Last Updated: 25/04/2024
#84 PCA9685 controlling multiple servos direction and speed with millis()
Components >> #84 PCA9685 controlling multiple servos direction and speed with millis()
PCA9685 controlling multiple servos direction and speed with millis()
This tutorial shows how to control multiple servos all moving at different speeds and in different directions using millis() for control so no code getting blocked by delay().
Servo Circuit
Arduino Pins
A5 to PCA9685 SCL
A4 to PCA9685 SDA
WARNING: The 5v power supply to the PCA9685 needs to be seperate to the Arduino. An Arduino cannot provide the required power.
Wiring:for a few common boards
SDA/SCL connections
Arduino UNO: A4 (SDA), A5 (SCL) :
Arduino Mega 2560: 20 (SDA), 21 (SCL) :
ESP32: 21(SDA), 22 (SCL)
For other Arduino boards see:https://www.arduino.cc/en/reference/wire
Example 1: PCA9685MillisV1.ino
Click to Download code: PCA9685MillisV1.ino
This is the sample sketch from the tutorial
/* PCA9685MillisV1
25/04/2024
Control PCA9685 Servo movement with non blocking code.
Arduino UNo
Pins
Arduino UNO: A4 (SDA), A5 (SCL)
*/
#include "Wire.h"
#include "Adafruit_PWMServoDriver.h"
//Create Adafruit_PWMServoDriver object: servoBoard
Adafruit_PWMServoDriver servoBoard = Adafruit_PWMServoDriver();
//settings for PCA9685 defaults for servos
#define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates
unsigned long currentMillis; //stores current board time in milliseconds
unsigned long resetMillis; // keeps track of next time to check if servos have finished moving
int resetTimer = 2000; //checks every 2 seconds
unsigned long servoMillis[16];
//definitions to make servoData[] easier to work with
#define SERVOTARGET 0
#define SERVOCURRENT 1
#define SERVOSPEED 2
unsigned int servoData[16][3]; //target angle, current angle, speed
void setup() {
Serial.begin(9600);
Serial.println("PCA9685MillisV1");
servoBoard.begin();
servoBoard.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates
}
void loop() {
currentMillis = millis();
servoControl(); //servo control function
//This code just checks if the servo has reached it's destination
//If it has it sets a random destination and speed.
if (currentMillis - resetMillis > resetTimer) { //checks every 2 seconds
resetMillis = currentMillis;
//check to see if servo has finished movement and if it has give it a new destination value and speed
for (int q = 0; q < 16; q++) {
if (servoData[q][SERVOTARGET] == servoData[q][SERVOCURRENT]) { //has the servo reached it's final position
//if yes set a new position and speed
servoData[q][SERVOTARGET] = random(0, 179); //set new position to move to
servoData[q][SERVOSPEED] = random(0, 70); //set new position to move to
Serial.print("Servo: ");
Serial.print(q);
Serial.print(" Tgt: ");
Serial.print(servoData[q][SERVOTARGET]);
Serial.print(" Speed: ");
Serial.println(servoData[q][SERVOSPEED]);
}
}
Serial.println(" ");
}
/* NOTE on changing servo target and speed.
Changing the value of servoData[q][SERVOTARGET] at any time will cause the servo to instantly start moving to the new
setting even if it has not arrived at the last destination.
changing the value of servoData[q][SERVOSPEED] will change the current servo speed. This could be used for
servo acceleration deceleration as required.
to find out how many steps are left find the difference between servoData[q][SERVOTARGET] and servoData[q][SERVOCURRENT]
*/
}
//this function deals with moving all the servos at the correct speed.
void servoControl() {
int sendPos;
for (int q = 0; q < 16; q++) { //cycle through all the boards
if (servoData[q][SERVOTARGET] != servoData[q][SERVOCURRENT]) { //does this servo need to move?
if (currentMillis - servoMillis[q] >= servoData[q][SERVOSPEED]) { //is it time to move
servoMillis[q] = currentMillis; //reset this servos timer
if (servoData[q][SERVOTARGET] > 179) {
servoData[q][SERVOTARGET] = 179;
} //limit check in case someone tried to move servo more than 179 degrees
if (servoData[q][SERVOCURRENT] < servoData[q][SERVOTARGET]) { //work out the direction servo needs to move in
servoData[q][SERVOCURRENT]++;
} else {
servoData[q][SERVOCURRENT]--;
}
sendPos = map(servoData[q][SERVOCURRENT], 0, 179, USMIN, USMAX);
servoBoard.writeMicroseconds(q, sendPos);
}
}
}
}
Example 2: PCA9685MillisV2.ino
Click to Download code: PCA9685MillisV2.ino
This version moves all the servos together at the same speed and direction
/* PCA9685MillisV2
25/04/2024
Control PCA9685 Servo movement with non blocking code.
All servos moving servos together
Arduino UNo
Pins
Arduino UNO: A4 (SDA), A5 (SCL)
*/
#include "Wire.h"
#include "Adafruit_PWMServoDriver.h"
//Create Adafruit_PWMServoDriver object: servoBoard
Adafruit_PWMServoDriver servoBoard = Adafruit_PWMServoDriver();
//settings for PCA9685 defaults for servos
#define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates
unsigned long currentMillis; //stores current board time in milliseconds
unsigned long resetMillis; // keeps track of next time to check if servos have finished moving
int resetTimer = 2000; //checks every 2 seconds
unsigned long servoMillis[16];
//definitions to make servoData[] easier to work with
#define SERVOTARGET 0
#define SERVOCURRENT 1
#define SERVOSPEED 2
unsigned int servoData[16][3]; //target angle, current angle, speed
int dir;
void setup() {
Serial.begin(9600);
Serial.println("PCA9685MillisV2");
servoBoard.begin();
servoBoard.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates
}
void loop() {
currentMillis = millis();
servoControl(); //servo control function
//This code just checks if the servo has reached it's destination
//If it has it sets a random destination and speed.
if (currentMillis - resetMillis > resetTimer) { //checks every 2 seconds
resetMillis = currentMillis;
//check to see if servo has finished movement and if it has give it a new destination value and speed
if (dir > 0) {
dir = 0;
}else{
dir = 1;
}
for (int q = 0; q < 16; q++) {
if (servoData[q][SERVOTARGET] == servoData[q][SERVOCURRENT]) { //has the servo reached it's final position
//if yes set a new position and speed
if (dir > 0) {
servoData[q][SERVOTARGET] = 10; //set new position to move to
servoData[q][SERVOSPEED] = 15; //set new position to move to
} else {
servoData[q][SERVOTARGET] = 160; //set new position to move to
servoData[q][SERVOSPEED] = 15; //set new position to move to
}
//servoData[q][SERVOTARGET] = random(0, 179); //set new position to move to
//servoData[q][SERVOSPEED] = random(0, 70); //set new position to move to
Serial.print("Servo: ");
Serial.print(q);
Serial.print(" Tgt: ");
Serial.print(servoData[q][SERVOTARGET]);
Serial.print(" Speed: ");
Serial.println(servoData[q][SERVOSPEED]);
}
}
Serial.println(" ");
}
/* NOTE on changing servo target and speed.
Changing the value of servoData[q][SERVOTARGET] at any time will cause the servo to instantly start moving to the new
setting even if it has not arrived at the last destination.
changing the value of servoData[q][SERVOSPEED] will change the current servo speed. This could be used for
servo acceleration deceleration as required.
to find out how many steps are left find the difference between servoData[q][SERVOTARGET] and servoData[q][SERVOCURRENT]
*/
}
//this function deals with moving all the servos at the correct speed.
void servoControl() {
int sendPos;
for (int q = 0; q < 16; q++) { //cycle through all the boards
if (servoData[q][SERVOTARGET] != servoData[q][SERVOCURRENT]) { //does this servo need to move?
if (currentMillis - servoMillis[q] >= servoData[q][SERVOSPEED]) { //is it time to move
servoMillis[q] = currentMillis; //reset this servos timer
if (servoData[q][SERVOTARGET] > 179) {
servoData[q][SERVOTARGET] = 179;
} //limit check in case someone tried to move servo more than 179 degrees
if (servoData[q][SERVOCURRENT] < servoData[q][SERVOTARGET]) { //work out the direction servo needs to move in
servoData[q][SERVOCURRENT]++;
} else {
servoData[q][SERVOCURRENT]--;
}
sendPos = map(servoData[q][SERVOCURRENT], 0, 179, USMIN, USMAX);
servoBoard.writeMicroseconds(q, sendPos);
}
}
}
}
Additional Resource Links
map() function 19/03/2024
Lesson 11: Arrays 28/08/2021
State Machine Examples 08/03/2023
Comments
To ask a question please comment on YouTube or email the address in this image:
and use #84 PCA9685 controlling multiple servos direction and speed with millis() as a reference.