Arduino programming: Advanced Servos

/, Programming Tutorials, tips/Arduino programming: Advanced Servos

For a while now I wanted to share what I learned in programming Arduino and AVR micro-controllers in general.  Most of the topics I want to discuss are above basics.  There are already plenty of basic tutorials online, so no need to reinvent bicycle.

However don’t be discouraged by “advanced” title. I will attempt to explain it in terms even a very beginner programmer can understand.

Let’s start with servos. I only started to play with servos about a month ago, and found how fun controlling them can be. But also challenging.

I won’t go into details about servo types, specs, uses, etc. but I do want to mention a few things about ways to control them.

There are essentially 3 ways to have full control of a servo.

  1. You can do it directly with Arduino or similar AVR microprocessor using PWM pins or a Servo library that allows you to use even non-PWM pins.
  2. An external PWM expansion board like Adafruit’s 16 channel servo controller
  3. A dedicated “smart” 32 channel servo controller board (i.e. RTRobot)

In this topic I will talk about first option, however let me talk just a little bit about options 2 and 3.

An external PWM controller is just an extension of Arduino’s own PWM ports.  These are usually controlled via I2C bus and can be chained together to control a huge number of servos.  These are great but a little more difficult to control and will rely on your micro-controller’s CPU.

A dedicated servo controller board usually have it’s own microprocessor (ARM), in many cases more powerful than Arduino.  These are fantastic boards, that can store servo movement macros that can be initiated by Arduino serial commands. If you are building a robot like hexapod, you can pretty much program movement sequences into controller’s memory and offload your Arduino to do other tasks.  Some even come with PS2 wireless controller capabilities!

But if you only have a handful of servos to control, doing it directly from AVR is probably the best choice.

Arduino comes with built in Servo library. This library can make almost any pin on your Arduino capable of driving a servo.  I really like that you don’t have to mess with PWM frequencies, and can just tell it to turn servo to a specific degree.

But there’s one problem. There’s no built-in way to control servo speed. Large high torque servos move pretty slow, but for small micro servos like MG90s this could be a problem as they move pretty fast.  So fast that parts attached to it could be flying off, or inertia damaging servo gears.

Servo library does come with a code that can move servo at different speeds, but it’s problematic because it’s “blocking”. Blocking function means that nothing else can run until it completes.   So you can see that you can’t drive more than one servo at a time because of that or do something else.

So let’s talk about writing code that will address updating multiple servos and letting your micro-controller do other things.

You can find complete sketch here.

Making a non-blocking function.

Simplest way to create a non-blocking function is to launch it as frequently as possible.  Every time it runs it checks two things:

  1. Is it needed to perform an action?
  2. Has enough time passed since last run to do it’s thing again?

In our servo code, we want to execute servo movement only when it’s needed.

if (!activeServo

[servoID]) return; // No servos need updating

That’s the first thing that function will check. If servo position needs to be updated, it will go to second check, but if it’s not need, it should quit as soon as possible to free up resources and let other things run.  So even tho I call this non-blocking function, it will still use some processor resources, but it will be a very small delay.

After first check passed, we will now check if it’s OK do execute main part or maybe it’s still too soon.  This part performed same function as delay() function except it doesn’t stop rest of the program.  That is also how you control servo speed, meaning it remembers last time servo moved, so it will compare that time with current time and if difference is less than specified it will not update servo, otherwise it will update servo and remember current time:

if ((unsigned long)millis()-lastUpdate > servoSpeed ) {// It’s time to execute
lastUpdate=millis();
myservo[servoID].write(currDeg[servoID]);// Move servo to current location

It is literally that simple.

Things do get a little bit complicated when you realize that this “update” function needs to constantly run in the main loop. So if there’s some other “blocking” code in there, there will be a problem.  To prevent this, we will need to make rest of the functions in the program “non-blocking”.  And of course there’s a better way.   Instead of using main loop to execute our “servo update” function, we can use built in Timers.  I did not explore this topic myself so I can’t tell you how to implement timers to execute code, but once I figure it out I will come back to it and explain.

Moving Servo Physically

When using servo library, servo can be moved to any angle by issuing command

myservo.write(DEGREE)

Where degree is number between 0 and 180.  This is fine when we want to move servo at maximum speed, however if we want to slow it down, we will need to move it gradually from current position to target position one degree at a time until we reach it. We also waiting for specified time between these moves. These one degree moves are made by our non-blocking “updateServo” function.

In order to do this, we require several global variables.  One of them “targetDeg” will contain our target degree number. Second one “currDeg” will get changed every time “updateServo” function runs.

This “currDeg” variable is little tricky.  It needs to be either incremented or decremented depending on which way servo is moving.

We can find out direction of the rotation by comparing current degree with target degree. If “currDeg” is larger than “targetDeg” we are moving clockwise and need to subtract 1 from “currDeg” after each move.
For example let’s say currently servo is at 90, and we need to move it 30.  90 is more than 30 so we are moving servo clockwise. First we move it to 89 (90-1), then we move it 88 (89-1) and so on until we reach 30.
Now let’s say we need to move it 45 .  Our currDeg=30.  30 < 45, so we are moving counter clock-wise and need to add 1 each time to currDeg.  So first we’ll move servo to 31 (30+1), then to 32 and so on until we reach 45.

Please note: When the program runs for the very first time, we have NO IDEA where server is at right now (unless it’s some fancy servo that provides location feedback with extra wire).  So when we initialize our program we will move it some default degree (usually 90) at maximum speed by issuing myservo.write(90)

After we reach target degree, we no longer want our updateServo function to do anything, so we must change activeServo variable to “false”.  Because update function checks this variable first, it will see that there’s nothing to do and quit.

Using arrays for variables

It is extremely convenient to use arrays to store variable values. Especially when we have multiple devices that can re-use same code, such as buttons, LEDs and yes you guessed it – Servos 🙂

For example we have 2 servos, one for panning and one for tilting camera.  We can create set of variables for each of those servos like:

#define PAN_SERVO 2  //specify Arduino pin attached to that servo
#define TILT_SERVO 4 //specify Arduino pin attached to that servo
int servoPIN1 = PAN_SERVO ;
int servoPIN2 = TILT_SERVO ;
boolean activeServo1=false
boolean activeServo2=false;
int currDeg1,currDeg2;
int targetDeg1,targetDeg2;
and so on…

We have to remember each of those variables and make to check or update them by name. Very difficult to do. And what if we have 10 servos? 32?  Imagine the mess!

But let’s see what happens if use arrays for those:

#define MAX_SERVOS 3 //number of servos. We now can do this becuase we use arrays (static)
#define PAN_SERVO_PIN 2  //specify Arduino pin attached to that servo (static)
#define TILT_SERVO_PIN 4 //specify Arduino pin attached to that servo (static)
#define ZOOM_SERVO_PIN 5 //We added additional servo (static)
int servoPIN[MAX_SERVOS]={PAN_SERVO_PIN, TILT_SERVO_PIN, ZOOM_SERVO_PIN};
boolean activeServo[MAX_SERVOS];
int currDeg[MAX_SERVOS];
int targetDeg[MAX_SERVOS];

Much easier to read, we can add more servos very easily. Only manual entry wold be to change MAX_SERVOS value, add extra “#define” statements and also specify them in “servoPIN” array.  Rest of the code can be automated.  For example, we will want to disable all servos in the begining:

for (int i=0; i<MAX_SERVOS;i++)
activeServo[i]=false;

We could’ve added 20 more servos and above for loop will remain the same! Neat, huh?

Of course you still need to remember which servo corresponds to which array element.  Or not, if you define them beforehead like:

#define PAN_SERVO 0
#define TILT_SERVO 1
#define ZOOM_SERVO 2

Moving servo virtually

What is this?  How can we move servo virtually?  Well, I’m just calling it “virtual” to differentiate from “physical” move function.

Physical move function is actually our “updateServo” function that performs single degree move of the servo.  Our “virtual” move function called “moveServo” is only telling “updateServo” function what to do. It gives update function a task.

UpdateServo function runs all the time, but only does something if moveServo function tells it to do something.  Thus “moveServo” only executes one time when needed.

For example we want to slowly move our TILT servo arm to 100 degrees. To do this we going to call moveServo function with parameters:

moveServo (TILT_SERVO, 100, 50)

  • TILT_SERVO is defined as 0 array element in beginning of our program
  • 100 is target degree mark
  • 50 is number of milliseconds to wait between single degree rotation. The bigger the number the slower servo will rotate.

Our “moveServo” function will “activate” required servo, and set it’s target angle and global speed.

void moveServo (char servoID, int newTarget, int newSpeed) {
if (activeServo[servoID]) return; // Servo is currently updating
activeServo[servoID]=true;
targetDeg[servoID]=newTarget;
servoSpeed = newSpeed;

Above code can be improved (at expense of increasing complexity) by checking for servo limits as sometimes you don’t want it to move past specific degree.  So before setting targetDeg[servoID] you will check if it’s above or below limit and change it to this limit instead ignoring newTarget value). Like this:

if (newTarget < servoLimit[servoID][0]) targetDeg[servoID]=servoLimit[servoID][0]; // Servo taraget is below MIN
else
if (newTarget > servoLimit[servoID][1]) targetDeg[servoID]=servoLimit[servoID][1]; // Servo Target is above MAX
else targetDeg[servoID]=newTarget;

Here servoLimit[servoID][0] is hardcoded MINIMUM angle for specific servo, and servoLimit[servoID][1] is MAXIMUM.

Another look at updateServo function

Now that we learned about moveServo function and arrays used by program we can finally look at all of the code in  updateServo function:

void updateServo(char servoID) {
if (!activeServo[servoID]) return; // No servos need updating
if (currDeg[servoID]==targetDeg[servoID]) { // Reached destination
activeServo[servoID]=false;
Serial.println (“Reached destination”);
return;
}
if ((unsigned long)millis()-lastUpdate > servoSpeed ) {// It’s time to execute
lastUpdate=millis();
myservo[servoID].write(currDeg[servoID]);// Move servo to current location
if (currDeg[servoID] > targetDeg[servoID]) { // Rotating CW
currDeg[servoID]–; // Decrement current location
} // Rotate CCW
else currDeg[servoID]++;
}
}

Function takes only one parameter, servoID.  This means that we actually have to call it from main loop as many times as we have servos. So if you have 3 servos you would call:

void loop () {
updateServo (TILT_SERVO);
updateServo (PAN_SERVO);
updateServo (ZOOM_SERVO);
}

Or use “for” loop instead.  Keep in mind that with large number of servos, you might run into performance issues (slow downs). Also you can modify updateServo function to actually poll ALL servos, thus eliminating a need for parameter and calling function multiple times from main loop. It won’t give you any speed increase, but will make code look nicer.

First line checks if current servo needs to be moved (active) or not (idle).

Next 4 lines check if server actually reached it’s final destination. If it did, there’s no need to execute rest of the code. Servo status will get changed to inactive and function will exit.

If we passed first 2 checks, next line will check if it’s time to move servo by a degree or not. If it’s too soon, function do nothing. Otherwise it will use logic to check which way to rotate servo and perform rotation by single degree. All required global variables will also get updated now and read next time updateServo function will run.

Payload

Having create all required functions to move serves is all nice and well, but how do we actually use it?

One might need to move servos to specific location under specific condition. For example if remote signal received, or maybe some time has passed.  These functions also need to be non-blocking to allow for serverUpdate to always run.  So let’s create one.  Let’s move our servos to random location every second.  For this we’ll create a “randomMove” function that will run in main loop constantly, but only does any work when sufficient time has passed since last time it executed. Just like our “updateServos” function!

// =========================================================
// — Sets servos to random position between Min and Max
// =========================================================
void randomMove(int servoID, int mMin, int mMax) {
if (activeServo[servoID]) return; // Servo is currently updating
if ((unsigned long)millis() – lastRandomMove > timeoutB4NextMove ) { // It’s time to execute
lastRandomMove=millis();  // Remember the time we executed
int rPos = random (mMin,mMax); // Generate random number in specified range
Serial.println (rPos); // Debug
moveServo (servoID, rPos, 10); // Enable servo movement
}
}

Let look closer.

First it is called with 3 parameters. First one is servoID, and last two define range of motion for our servo. This means that while servo will move to a random position, it can never go beyond set limits.

First line will check servo already moving. No need to do anything since it’s still in motion, so we going to exit.

Second line is check if sufficient time has passed since last time randomMove executed. In our case timeoutB4NextMove variable is set to 1000ms or 1 second.  So if it’s been longer than that, we going to perform main action which is to generate random number withing specified range, and enable servo to move to that location.

And finally we going to put all those actions inside main loop:

void loop() {
updateServo(PAN_SERVO);
updateServo(TILT_SERVO);
randomMove(TILT_SERVO,45,120);
randomMove(PAN_SERVO,45,120);
}

If you are doing something more useful, like listening commands from Remote Control, you will add that function to the main loop, or use interrupts to execute it.

One again,here’s the complete sketch  discussed in this post.

And finally, as I mentioned, using TIMER interrupts is a better alternative to constantly calling updateServo function from main loop, but keep in mind that Arduino has limited number of TIMERs and at least one of them already used by Servo library.  But if you are using Arduino Mega or some other AVR that has several extra interrupts, this would be preferred way.   Once I figure out how to do this, I will post an update to this tutorial.

2019-01-26T21:28:01+00:00December 19th, 2016|How To, Programming Tutorials, tips|0 Comments

Share This Story, Choose Your Platform!

Leave A Comment