It is easy enough to control multiple servo motors with the Arduino, but I wanted a way to have them all going to different positions and at different speeds simultaneously. For example I did not want to move Servo A to 77° and afterwards Servo B to 25°. I wanted both servos to move (or appear to move) at the same time.
(For the basics of controlling a servo with the Arduino there's a ton of stuff on the web, this link seems like a good article. Read that first before continuing with this post.)
I'd seen some articles on multitasking on the Arduino and I thought: "Aha, I can use a similar technique for driving multiple servos..."
The main action of an Arduino program takes place in a function called loop, which is called automatically again and again. In our case the loop looks like this:
At the head of the loop you capture the command. The command can come over the serial link from a program running on the PC, or maybe from functions which read sensors.
If a command comes in which says "move Servo A to 33°" the last block in the above loop will start to do that. But the next time round there could be a command which says to "move Servo B to 90°". And again that last block will move both servos towards their final position, so they appear to be moving simultaneously.
Of course once the servos have reached their positions they do not move until another command arrives for them.
What I also wanted to do was to have each servo have acceleration and deceleration. I do this by having a curve which specifies the speed with respect to time. Time being measured with 0 at the start of the movement. So here is what I mean graphically:
Notice that the curve above does not reach a speed of 0, else the servo might stop before it gets to its target. That last horizontal line guarantees that movement will continue even if it takes a long time to get to the target.
In the program I don't actually do a curve, I have some discreet steps, look at the comments inside the kSpeedCurves part. Each speed curve has an id, I use ids 'A', 'B' and 'C', which means you can move one servo following one speed curve, and another servo following a different curve. Some faster, some slower, some with more acceleration, some with less.
As far as the program is concerned the speed of the servo is actually simply the angular size of the step to take in each loop.
The command from the PC are not gathered together in one iteration of the loop. A character is read in each iteration until a 0 terminator arrives, then the command is interpreted. Remember that commands do not move the servo, but specify a target for a given servo. The servo moves towards that target. It may be interrupted by another command even before it gets to the target.
The format of the commands contain which servo and which curve to use and the degrees target to move to, see the comments about "VA123" in the sources.
Here is the code, it was written for a specific need of mine, but maybe you'll find something useful in there and can adapt it to you own requirements. Have fun!
// Moving multiple servos at different rates simultanesouly...
#include <Servo.h>
/***************************************************************************************************************/
typedef struct {
Servo* pServoObj ; // Which servo will be driven by this structure
int iServoPin ; // On which pin is the servo connected
double CurrPos ; // Where we are now
double TargetPos ; // Where we want to go
unsigned long imsRealStartTime ; // The time when we started going
char chCurveId ; // For example 'A' which curve in kSpeedCurves to use
} ServoCurver_t ;
/***************************************************************************************************************/
// Each one of these is an entry in the speed curve
typedef struct {
unsigned long iTimeOrPcent ; // x-axis in ms or in %
double Speed ; // y-axis, actually a delta angle to apply
} SpeedAt_t ;
// Here is a list of speed curves....
#define NUM_X_COORDS 4
typedef struct {
char chId ;
SpeedAt_t Curve [NUM_X_COORDS] ;
} CurverData_t ;
const CurverData_t kSpeedCurves[] = {
{
'A', // the id has to e unique inside this array
{
{ 0, 1.00}, // at 0 ms, the beginning of the move the delta to apply is 1 degrees
{ 500, 2.00}, // at 0.5s the delta to apply is 2 degrees
{ 2000, 1.50}, // at 2s the delta to apply is 1.5 degrees
{ 15000, 1.25}, // at 15s the delta to apply is 1.25 degrees
}
} ,
{
'B',
{
{ 0, 1.0},
{ 500, 3.75},
{ 7000, 1.5},
{10000, 0.5},
}
},
{
'C',
{
{ 0, 6.0},
{ 500, 5.0},
{ 900, 4.0},
{10000, 0.75},
}
},
};
const size_t ikTotalNumCurves = sizeof(kSpeedCurves)/sizeof(kSpeedCurves[0]) ;
/***************************************************************************************************************/
int LedPin = 13 ;
/***************************************************************************************************************/
// This stores communications from the PC...
int iSerialCharsRead = 0 ;
#define CMD_BUF_SIZE 100
char szCommand[CMD_BUF_SIZE] = "" ;
void ClearCmdBuffer ()
{
iSerialCharsRead = 0 ;
memset (szCommand,CMD_BUF_SIZE,0) ;
}
/***************************************************************************************************************/
// Create the servo objects
const int ikHeadVerticalPin = 3 ;
ServoCurver_t scHeadVertical ;
Servo srvHeadVertical;
const int ikHeadHorizontalPin = 10 ;
ServoCurver_t scHeadHorizontal ;
Servo srvHeadHorizontal;
Servo srvRightArm ;
const int ikRightArmPin = 5 ;
ServoCurver_t scRightArm ;
Servo srvLeftArm ;
const int ikLeftArmPin = 6 ;
ServoCurver_t scLeftArm ;
/***************************************************************************************************************/
// Look up which CurverData_t corresponds to the given id
const CurverData_t& GetCurveFromId (char chId)
{
for (size_t c = 0 ; c < ikTotalNumCurves ; ++c) {
if (kSpeedCurves[c].chId == chId) {
return kSpeedCurves[c] ;
}
}
Serial.print ("GetCurveFromId unknown id: ") ;
Serial.print (chId) ;
Serial.println() ;
// Return something
return kSpeedCurves[0] ;
}
/***************************************************************************************************************/
// First attempt no interpolation
// Calculates and returns the speed we must apply to the servo given its
// position and the speed curve it is using
// imsDeltaTime is how far into the speed curve the servo is
double GetSpeedFromTimeCurve (char chCurve, unsigned long imsDeltaTime)
{
// Get hold of that speed curve in an an easy to use way...
const CurverData_t& kThisTimeSpeedCurve = GetCurveFromId(chCurve) ;
if (imsDeltaTime == 0) {
// We are at the very beginning of the curve, return the first value
//Serial.print (" at time start") ;
//Serial.println (kThisTimeSpeedCurve.Curve[0].Speed) ;
return kThisTimeSpeedCurve.Curve[0].Speed ;
}
// Look at every section in the time speed curve...
for (int x = 0 ; x < NUM_X_COORDS-1 ; ++x) {
// Are we in the range x to x+1 in the graph?
const bool kbInRange = (imsDeltaTime >= kThisTimeSpeedCurve.Curve[x].iTimeOrPcent) && (imsDeltaTime < kThisTimeSpeedCurve.Curve[x+1].iTimeOrPcent) ;
if (kbInRange) {
const double kSpeed = kThisTimeSpeedCurve.Curve[x].Speed ;
return kSpeed ;
}
}
// if we get here we are beyond the end of the x-axis of the curve,
// Assume he curve carries on to infinity from the last value
const double kSpeed = kThisTimeSpeedCurve.Curve[NUM_X_COORDS-1].Speed ;
return kSpeed ;
}
/*****************************************************************************************************/
// Get direction required to move towards the target of this servo...
int GetSignForServoMovement (const ServoCurver_t& ServoCurver)
{
const double kOnTargetMargin = 1.0 ;
const double kError = abs (ServoCurver.CurrPos - ServoCurver.TargetPos) ;
if (kError < kOnTargetMargin ) {
// In this case the servo is already at the target and we don't need to
// do anything
return 0 ;
}
int iSign ; // In which direction to move the servo
if (ServoCurver.CurrPos < ServoCurver.TargetPos) {
// need to move in a positive direction to get to the target
iSign = +1 ;
} else if (ServoCurver.CurrPos > ServoCurver.TargetPos) {
// need to move in a negative direction to get to the target
iSign = -1 ;
} else {
// In this case the servo is already at the target and we don't need to
// do anything
iSign = 0 ;
}
return iSign ;
}
void MoveServoOnCurveByTime (ServoCurver_t& ServoCurver)
{
const int ikSign = GetSignForServoMovement (ServoCurver) ;
if (ikSign == 0) {
// Servo is at target, no movement to do
return ;
}
// Get the time now. unsigned long is important
const unsigned long imsNow = millis() ;
// Get the delta time, how far into the curve you are
unsigned long imsDelta = imsNow - ServoCurver.imsRealStartTime ;
// See what speed the curve gives us at this time.
const double kSpeed = GetSpeedFromTimeCurve (ServoCurver.chCurveId,imsDelta) ;
if (kSpeed > 0) {
// Update the current pos...
ServoCurver.CurrPos = ServoCurver.CurrPos + (ikSign*kSpeed) ;
} else if (kSpeed == 0) {
// Speed 0 means that time is beyond the end of the curve
// so we must be at the target position. Update vars and move servo
// to the final target position
ServoCurver.CurrPos = ServoCurver.TargetPos ;
}
// ...and the actual server
const int ikIntPos = int(round (ServoCurver.CurrPos)) ;
ServoCurver.pServoObj->write (ikIntPos) ;
}
/********************************************************************************************/
// This should be called everytime we need a new target, i.e every time a new command comes along
void SetServoCurveAndTarget (ServoCurver_t& ServoCurve, const char chCurveToUse, const double kNewTarget)
{
// ServoCurve.CurrPos should not be changed, the servo is where it is, wherever that
// may be, but we want to give it a new *target*.
ServoCurve.TargetPos = kNewTarget ;
ServoCurve.chCurveId = chCurveToUse ; // This is which graph to use
// Only one of these will be used, depends on what sort of curve it is
ServoCurve.imsRealStartTime = millis() ; // This is the start time of the command, now
}
// This should be called only once per servo in the setup
void InitServoCurverAndHardware (ServoCurver_t& ServoCurve, Servo* pServoObject, int const ikServoPin, char chCurveToUse)
{
// Set everything to 90° initially, servos in a halfway position
const double kDefaultPos = 90.0 ;
ServoCurve.CurrPos = kDefaultPos ; // this is where it is
SetServoCurveAndTarget (ServoCurve,chCurveToUse,kDefaultPos) ;
// Remember what hardware this ServoCurver is attached to...
ServoCurve.pServoObj = pServoObject ;
ServoCurve.iServoPin = ikServoPin ;
// Attach and set the position
ServoCurve.pServoObj->attach(ServoCurve.iServoPin) ;
ServoCurve.pServoObj->write (int(round(ServoCurve.CurrPos))) ;
}
/*********************************************************************************************************************************/
void setup() {
Serial.begin (9600) ;
InitServoCurverAndHardware (scHeadVertical,
&srvHeadVertical,
ikHeadVerticalPin,
'A') ;
InitServoCurverAndHardware (scHeadHorizontal,
&srvHeadHorizontal,
ikHeadHorizontalPin,
'A') ;
InitServoCurverAndHardware (scLeftArm,
&srvLeftArm,
ikLeftArmPin,
'A') ;
InitServoCurverAndHardware (scRightArm,
&srvRightArm,
ikRightArmPin,
'A') ;
pinMode(LedPin, OUTPUT);
ClearCmdBuffer () ;
}
unsigned long iTimeToMoveServos = 0 ;
const unsigned long imskServoMoveDelta = 200 ;
void loop() {
// Read a command character from the PC...
char c ;
bool bReadingSerial = false ;
if (Serial.available() > 0) {
// read next character
c = Serial.read();
bReadingSerial = true ;
// newline is end of command
if (c != '\n') {
// not a newline so still collecting chars from the serial
szCommand[iSerialCharsRead] = c;
iSerialCharsRead++;
digitalWrite(LedPin, LOW);
}
}
// If we have some characters and the last char recieved was a newline...
// ...we got a command, do it
// Commands set targets and let the servos get on with it
// Commands are in the form "VA123", "HB90"
// V is which servo
// A is which curve to use
// 123 is the target angle
if ((iSerialCharsRead > 0) && (c == '\n')) {
// We have a command...
szCommand[iSerialCharsRead] = 0 ; // zero terminate the command
Serial.println(szCommand);
// Gather data from the command string...
char chServo = szCommand[0] ; // 'H' or 'V' etc
char chCurve = szCommand[1] ; // 'A' for example
double kNewTarget = atof(szCommand+2) ;// Degrees position of new target
if ((kNewTarget >= 0.0) && (kNewTarget <= 180.0)) {
// Act on the command...
switch (chServo) {
case 'H':
// Horizontally turn the head
SetServoCurveAndTarget (scHeadHorizontal,chCurve,kNewTarget) ;
break ;
case 'V':
// Vertically nod the head
SetServoCurveAndTarget (scHeadVertical,chCurve,kNewTarget) ;
break ;
case 'R':
// Move the right arm...
SetServoCurveAndTarget (scRightArm,chCurve,kNewTarget) ;
break ;
case 'L':
// Move the leftt arm...
SetServoCurveAndTarget (scLeftArm,chCurve,kNewTarget) ;
break ;
default:
// Don't understand the command, ignore it
break ;
}
}
// get ready to read another command
ClearCmdBuffer () ;
}
// Whethere there has been a command or not the servos carry on moving till
// they get to their targets
if (!bReadingSerial) {
if (millis() > iTimeToMoveServos) {
MoveServoOnCurveByTime (scHeadVertical) ;
MoveServoOnCurveByTime (scHeadHorizontal) ;
MoveServoOnCurveByTime (scLeftArm) ;
MoveServoOnCurveByTime (scRightArm) ;
iTimeToMoveServos = millis() + imskServoMoveDelta ;
}
}
}
Note, the code above can oscillate for a while near the target position. This versione of MoveServoOnCurveByTime solves that problem, see kbSameSign...
static void MoveServoOnCurveByTime (CServoCurver& ServoCurver)
{
const int ikSign = GetSignForServoMovement (ServoCurver) ;
if (ikSign == 0) {
// Servo is at target, no movement to do
return ;
}
// Get the time now. unsigned long is important
const unsigned long imsNow = millis() ;
// Get the delta time, how far into the curve you are
unsigned long imsDelta = imsNow - ServoCurver.m_imsRealStartTime ;
// See what speed the curve gives us at this time.
const double kSpeed = GetSpeedFromTimeCurve (ServoCurver.m_chCurveId,imsDelta) ;
// Where are we now?
const double kCurrPos = ServoCurver.m_CurrPos ;
const double kCurrDelta = kCurrPos - ServoCurver.m_TargetPos ;
// Where would we move to?
const double kNewPos = ServoCurver.m_CurrPos + (ikSign*kSpeed) ;
const double kNewDelta = kNewPos - ServoCurver.m_TargetPos ;
// 2021-07-11 This change stops oscillations around the target
const bool kbBothNegative = (kNewDelta < 0.0) && (kCurrDelta < 0.0) ;
const bool kbBothPositive = (kNewDelta > 0.0) && (kCurrDelta > 0.0) ;
const bool kbSameSign = kbBothNegative || kbBothPositive ;
if (kbSameSign && (kSpeed > 0)) {
// Update the current pos...
ServoCurver.m_CurrPos = kNewPos ;
} else {
// Moving to the new position we would overshoot the target, so just move
// to the target
ServoCurver.m_CurrPos = ServoCurver.m_TargetPos ;
}
// Now move the actual servo
const int ikIntPos = int(round (ServoCurver.m_CurrPos)) ;
ServoCurver.m_ServoObj.write (ikIntPos) ;
}
