Variable speed & angle asynchronous servo control

Arduino program to provide complete, flexible control of servo motors.

Similar projects worth following
While there are many programs (call 'em sketches if you like) for the Arduino that use the Servo library to exercise a servo motor or two (sometimes more), they nearly all act the same: they move the servo(s) to a new position instantly, or at least as fast as possible. While useful, this behaviour is often not as desirable as it might be. Frequently, one would like the servo(s) to move at a slower speed, moving a given angle in a give time period. A few examples of this behaviour exist, but typically use the delay function and thus disallow independent movement of the servos or other asynchronous activities.

This project will present code that addresses the shortcomings of existing servo control schemes. Not only can desired movement angle and time be specified, but arrays can be used for control permitting coordinated group movements, as well as individual movements.

The first program (T2MultiServo1.ino) is pretty much a copy of the Adafruit code. All I did was to remove the Flasher class since I didn't need it. SIGNAL was changed to ISR since SIGNAL is now deprecated. I also added the external interrupt for the reset button. That code is part of the same Adafruit example. The purpose here was simply to see how the code worked and check out the interface board I built. Although the code is running on a Teensy 2, it will run equally well on an Arduino Uno or a Mega. It WILL NOT run on a Teensy 3 or other ARM-based Arduinos. I'll need to figure out more about the ARM timers and perhaps the interrupt structure. Future work!

Hardware Interface:
- Servos need independent power! I'm using a 6V, 1A wall wart. Ran 6 SG-5010 and 2 SG90 servos simultaneously for a few minutes and WW was just barely warm. I didn't try to find the limits of this setup. It worked for testing.
- I built a simple board to connect power and the control signals. The schematic and a picture are attached. Doesn't get much simpler.
- The two big caps are there to smooth out surges during servo motion.

Adding Control of Angle and Time

In T2MultiServo2.ino, I modified the Sweeper class to allow a move to a specified position at the programmed rate, then stop until directed to a new position. I also added a bit of code in loop() to exercise it. One perhaps interesting detail is the use of elapsedMillis objects to control servo position changes at independent times. The elapsedMillis class is just one more elegant, convenient implementation we can thank Paul Stoffregen for!

T2MultiServo3.ino adds programmable sweep time. Specifying the time allowed for a move to a new angle will be translated to the time interval between incremental moves. The only caveat, and one for which there is *no* checking, is that the move to a new position cannot happen faster than one degree per millisecond. So if we want to move 180 degrees say, in 100 milliseconds, it ain't gonna happen! Not yet anyhow. Since my concern is to move physical objects at reasonable rates, then this restriction is not an issue for me. But since I'm releasing the code to the wild, this problem must be dealt with. More Future Work. It would also be useful to be able to move less than 1 degree per time increment if very slow moves were desired. Right now, such slow moves will appear jerky, but at least they will happen.

Array Control: Enabling Group Moves

If you've followed the details so far, then T2MultiServo4.ino is your reward! It implements group moves of servo motors in a controlled manner. The program should allow you to implement your own design easily. Following is useful information for those who want to know more.

This final version of the Sweeper class requires no arguments to instantiate a Sweeper object. An array of Sweepers can now be created. This keeps the code for creating, initializing, and updating the servos nice and clean. Individual Sweeper objects (servos) are referred to by index. Indexing in this manner is very useful, as illustrated by the functions described below.

This version of the Sweeper class provides separate functions to initialize a move with destination and time, and to enable the move. Doing this allows several moves to be set up, then all started at the same time, for a group move. Now it is likely that this is not necessary in most cases, but I thought it would be nice to have and would eliminate worries caused by starting moves at staggered times. There is also a function that will produce the old behaviour if desired. Another useful function is resetTo() which moves a servo immediately to a desired position. This is most useful at startup to put all the servos into a known state. Note that the motors should be put in this position prior to powering down. Then there will be no sudden jerks as the program starts again.

The changes just described support a "group move" function. A group of servos may be easily defined (as an array), along with a corresponding...

Read more »


Complete version with examples. This is the one you want!

ino - 7.58 kB - 04/02/2017 at 20:23



The servo interface board I built.

JPEG Image - 149.50 kB - 03/12/2017 at 01:38



Time for a specific sweep can now be specified.

ino - 3.87 kB - 03/12/2017 at 01:04



Servo movement will now be to a specified angle.

ino - 3.39 kB - 03/12/2017 at 00:58



Simple Servo Interface board schematic.

Adobe Portable Document Format - 26.85 kB - 03/12/2017 at 00:48


View all 6 files

  • Class Improvements and Helper Functions

    doctek04/02/2017 at 19:37 0 comments

    OK! Except for some tiding up, the desired functions are working. Uh, sort of. Now it's time to make the Sweeper class easier to use and to add some helper functions to do interesting things - like group moves perhaps. For those playing along at home, this will be the useful version!

    Step 1: Fix the Sweeper class so that no arguments are required to instantiate a Sweeper object. Why? Because an array of Sweepers can now be created. This keeps the code for creating, initializing, and updating the servos nice and clean.

    Step 2: Add some useful class functions. The code to initialize a move with destination and time is now separate from the function that enables the move. Doing this allows several moves to be set up, then all started at the same time, for a group move. Now it is likely that this is not necessary in most cases, but I thought it would be nice to have and would eliminate worries caused by starting moves at staggered times.

    Step 3: Create a "group move" function. The idea here is to allow a group of servos to be easily defined (as an array), along with a corresponding array of target angles and an allowed time for the move.

    Step 4: Time the worst case operation. Suppose all the Sweepers need to move on the same tick. Will there be time or will there be a timing conflict?

    Still coming: video of a simple demo. Bonus: Lego link: Gear and Mount.

  • Modifications for More Control

    doctek03/12/2017 at 00:57 0 comments

    The first (Adafruit) version of the code simply swept two servos back and forth over their full range at independent rates. While that makes a nice demo, it's not really what I wanted. So time to start hacking the code. As a first step, I wanted a servo to move to a specified position at the programmed rate, then stop until directed to a new position. A few simple changes to the Sweeper class, a bit of code in loop() to excerise it, and it was working. The results are shown in T2MultiServo2.ino.

    If we can specify the desired position to sweep to, what else do we need? That's right, programmable sweep time. The Sweeper class functions need to know the time interval between incremental moves. But that's not really what I wanted to specify. I'd like to specify the time for the full move. So that's what I implemented! The code is in T2MultiServo3.ino. Again, the change is pretty straight-forward.

  • Inspiration and First Steps

    doctek03/12/2017 at 00:38 0 comments

    The seminal idea for this project came from a discussion at an Arduino Interest Group meeting in San Diego. One person mentioned that he had a kinetic sculpture and wanted to animate it with servo motors. He asked if anyone knew how to control the servos with an Aduino. We discussed his project needs and I said I'd see if I could find any useful info. When I started googling, I realized the problem didn't really have a good solution that I could find. Reason enough to see what I might come up with. Goal: Control 9 (min) to 12 (limit of Servo library) stepper motors independently to sweep to a desired position in a given time. Allow other functions to run simultaneously, if possible. Use minimal external components.

    While I found no complete solution, I did discover a huge step in the right direction! Bill Earl at Adafruit shows how to create an Arduino program to sweep two servos back and forth (0 to 180 degrees) at a programmed rate. His program is interrupt driven so the sweep rates are independent. This struck me as the perfect place to start!

View all 3 project logs

Enjoy this project?



doctek wrote 07/23/2017 at 04:25 point

Amazing and ambitious projects! I like the idea of using paper structurally to make robot bodies and limbs.

Your approach to servo control is also interesting. Continually sending commands from a central controller is a good way to partition motion control and allow multiple units for motion and sensing. Since I don't plan such a complex control system, I used the interrupt scheme to keep the loop() open for other tasks without impacting timing of the servo actions. I don't like trying to coordinate synchronous actions with asynchronous communications if I can avoid it. Too many "moving parts" makes me crazy! I think the control structures I use could be constructed on the fly from input commands, but I haven't tried that yet.

To deal with the problem of initial position and motion limits, I've written a program that allows one servo at a time to be addressed and exercised. I've considered the idea of putting the results into EEPROM, but so far that hasn't been needed. More future work?

  Are you sure? yes | no

Morning.Star wrote 07/23/2017 at 11:20 point

Interestingly enough, I got the idea looking at two consecutive servo signals on my scope and noticed how the interrupt system interleaved the PWM to produce what I was seeing. My first thought was to modify the interrupt code to pull in the sequence from the buffer directly, but I couldnt see how to sync that with an unknown number of servos and had to let the interrupts handle the switching. I built a structure to interface with the buffer and pinched it off in loop() because thats all the chip had to do; demux asynchronous commands into a synchronous buffer. Decal's supercontroller would be a multithreaded nightmare if I tried to put it on one die, which is why its built the way it is. AIMos is fully multi-threaded and the semaphore made my brain hurt as it was, trying to pipeline it for serial meant I had to implement a command structure because even at 115200bps the lag between servos 1 and 12 starting is noticeable, more audibly than visibly but still... Complex motions like accelerations are easy enough with one servo, but sending 12 parallel streams over serial proved notchy, so I abstracted them into commands that control a parallel structure which runs a lot faster than the serial does and is a lot smoother as a result. The problem is with the serial link, it really needs to be parallel and faster for an AI to control many servos over it. ;-)

Any thoughts on the problem I'm having with the ADC on my chips? They behave like digital ports, reading either 0 or 1023 unless floating, then they read variable and react to my finger touching the pin. It's really p*ing me off...

  Are you sure? yes | no

doctek wrote 07/25/2017 at 00:46 point

Your need to have an unknown and changeable number of servos connected kind of kills my scheme - although you could just design for a maximum of 12 servos, I think. But if what you have works, then why try to fix it?

Regarding your ADC, is that on the AVR? I don't see the code to run it implemented in the program you pointed to. Did I miss that part? I'll be glad to take a look at it if you like. I've used the AVR's ADC many times.

  Are you sure? yes | no

Morning.Star wrote 07/26/2017 at 06:25 point

I'll log the trouble I had when I cool off lol. I discovered that both of my Unos have failed, on one the 117 regulator is dead, and it has another unknown problem that prevents the ADC working. The other one has a broken trace somewhere but I never noticed til now as the digitals work fine. I also have a Mega2560 that the FTDI failed on, it wont talk to USB anymore but works otherwise. And then I also discovered the new IDE's Plotter crashes Inkscape on a multi-monitor setup and lost a bunch of foot template drawings. <pumps shotgun>

Chips work just great in my board... Thanks for answering tho. :-)

  Are you sure? yes | no

Morning.Star wrote 07/22/2017 at 09:27 point

Interesting approach. :-) I did something similar which I'm now reusing in Decal ( I called it a Servo Sequencer... What it does is accept a packet over serial that contains an angle and a speed, plus a start time in ticks and then execute the motion synchronously out of a rolling buffer containing the positions of all the servos. The code is documented in #AIMOS and posted in #Cardware if you're interested how I went about it.

  Are you sure? yes | no

Similar Projects

Does this project spark your interest?

Become a member to follow this project and never miss any updates