/ServoEasing

Arduino library to enable smooth servo movement.

Primary LanguageC++GNU General Public License v3.0GPL-3.0

ServoEasing

A library for smooth servo movements.
It uses the standard Arduino Servo library and therefore has its restrictions regarding pins and platform support.

Version 3.1.1 - work in progress

Badge License: GPLv3     Badge Version     Badge Commits since latest     Badge Build Status     Badge Hit Counter

Stand With Ukraine

Available as Arduino library "ServoEasing".

Button Install     Button API     Button Changelog


YouTube video of ServoEasing in action

Demonstration of different servo easings

Table of content


Servo easing library for Arduino

Its purpose is to interpolate the movement between two servo positions set by software.
If your servo control data is e.g. generated by an joystick or other "slow" changing inputs and therefore does not change suddenly or does not jump, you most likely do not need this library!, you may consider to use a digital low pass or simple EMA filters to smooth your values used to control the servos.
ServoEasing works with the Arduino Servo library as well as with PCA9685 servo expanders. The expander in turn requires the Arduino Wire library or a compatible one and is bound to their restrictions.
For ESP32 you need to install the Arduino ESP32Servo library.

If you require only one or two servos, you may want to use the included LightweightServo library (only for AVR), instead of the Arduino Servo library. The LightweightServo library uses the internal Timer1 with no software overhead and therefore has no problems with servo twitching or interrupt blocking libraries like SoftwareSerial, Adafruit_NeoPixel and DmxSimple.
For instructions how to enable these alternatives, see Compile options / macros.


Features

  • Linear and 9 other ease movements are provided.
  • All servos can move synchronized or independently.
  • Non blocking movements are implemented by the startEaseTo* functions by using a timer. This functions are not available for all platforms.
  • Degree values >= 400 is taken as microsecond values for the servo pulse to allow fine-grained control.
  • Float angels are supported to allow fine-grained servo control comparable to using microseconds.
  • User-specified callback function at "servo arrived" enables movement control independent of main loop.
  • Stop and resume of servo movement.
  • A trim value can be set for any servo. Its value is internally added to each requested position.
  • Reverse operation of servo is possible e.g. if it is mounted head down.
  • Constraints for minimum and maximum servo degree can be specified. Trim and reverse are applied after constraint processing.
  • Allow to specify an arbitrary mapping between degrees and microseconds by attach(int aPin, int aMicrosecondsForServoLowDegree, int aMicrosecondsForServoHighDegree, int aServoLowDegree, int aServoHighDegree).
  • Servo speed can be specified in degree per second or milliseconds for the complete move.
  • Multiple servo handling by *ForAllServos() functions like setDegreeForAllServos(3, 135, 135, 135).
  • All ServoEasing objects are accessible by using the ServoEasing::ServoEasingArray[].
  • Easy implementation of a move list - see ConsecutiveEasingsWithCallback example.

List of easing functions

Linear     Quadratic     Cubic     Quartic

Sine     Circular     Back     Elastic     Bounce

Precision     Dummy     User defined

  • Precision is like linear, but if descending, add a 5 degree negative bounce in the last 20 % of the movement time. So the target position is always approached from below. This enables it to taken out the slack/backlash of any hardware moved by the servo.
  • Dummy is used for delays in callback handler.

All easing functions can be used in the following variants:

All ease functions are called internally with the value: PercentageOfCompletion / 100 resulting in values from 0 and 1.

  • In (start the function with 0 and go to 1) for PRECISION, do a bounce if approaching from above (go in to origin), else linear.
  • Out (start the function with 1 and go to 0) for PRECISION, do a bounce if approaching from below (go out from origin), else linear.
  • InOut (start the function with 0 go to 1 and back to 0)
  • Bouncing (start with OUT, then return with IN to start degree) e.g. Bouncing of the Sine function results in the upper (positive) half of the sine.

All easing types (starting in flavor IN_OUT, then IN, OUT and BOUNCE) in one plot.
Since the values are computed in a fixed 20 ms raster, the last degree increment or decrement in an easing may be much smaller than the increment/decrement before, resulting in some small discontinuities between adjacent movements.
Arduino Plotter Output for Linear->Quadratic->Cubic->Quartic->Sine-Circular->Back->Elastic

Constraints

To restrict servo movements to a fixed range, you can specify constraints with setMinMaxConstraint(int aMinDegreeOrMicrosecond, int aMaxDegreeOrMicrosecond).
Arduino Plotter Output with constraints at 5 degree and 175 degree activated. Arduino Plotter Output with constraints at 5 degree and 175 degree activated


API

For floating point constants, use the notation of 123.456f with trailing f (for a floating point constant) to avoid compiler errors. A Doxygen documentation of the sources is available here.

Usage

See also the examples here.

void setup() {
    Servo1.attach(SERVO1_PIN, 45);
}
void loop() {
    Servo1.setEasingType(EASE_CUBIC_IN_OUT); // EASE_LINEAR is default
    Servo1.easeTo(135, 40);                                 // Blocking call
    Servo1.startEaseTo(45, 40, START_UPDATE_BY_INTERRUPT);  // Non blocking call
    // Now the servo is moving to the end position independently of your program.
    delay(5000);
}

Just call myServo.startEaseTo() instead of myServo.write() and you are done. Or if you want to wait (blocking) until servo has arrived, use myServo.easeTo().

  • Do not forget to initially set the start position for the Servo, since the library has no knowledge about your servos initial position and therefore starts at 0 degree at the first move, which may be undesirable.
    Setting the start position of the servo can be done as the second parameter to myServo.attach(int aPin, int aInitialDegree) or by calling myServo.write(int aDegree),
  • And do not forget to initially set the moving speed (as degrees per second) with myServo.setSpeed() or as second parameter to startEaseTo() or easeTo(). Otherwise the Servo will start with the speed of 5 degrees per second, to indicate that speed was not set.

Multiple servo handling

You can handle multiple servos simultaneously by special functions like writeAllServos(), setSpeedForAllServos(), setDegreeForAllServos(), setEaseToDForAllServos(), synchronizeAndEaseToArrayPositions(), updateAndWaitForAllServosToStop() and much more.


Comparison between Quadratic, Cubic and Sine easings.

Arduino Serial Plotter result of the SymmetricEasing example. Arduino plot


Useful resources


Resolution of servo positioning

  • The standard range of 544 to 2400 µs per 180 degree results in an timing of around 10 µs per degree.
  • The Arduino Servo library on AVR uses an prescaler of 8 at 16 MHz clock resulting in a resolution of 0.5 µs.
  • The PCA9685 expander has a resolution of 4.88 µs per step (@ 20 ms interval) resulting in a resolution of 0.5 degree. Digital Servos have a deadband of approximately 5 µs / 0.5 degree which means, that you will see a stuttering movement if the moving speed is slow. If you control them with a PCA9685 expander it may get worse, since one step of 4.88 µs can be within the deadband, so it takes 2 steps to move the servo from its current position.

Speed of servo positioning

These values are measured with the SpeedTest example.

These are the fastest values for my SG90 servos at 5 volt (4.2 volt with servo active).

Degree Duration Speed
180 400 ms 450 degree per second
90 300 ms 300 degree per second
45 180 ms 250 degree per second
30 150 ms 200 degree per second
20 130 ms 150 degree per second
10 80 ms 125 degree per second

Values for the MG90Sservos servos at 5 volt (4.2 volt with servo active).

Degree Duration Speed
180 330 ms 540 degree per second
90 220 ms 410 degree per second
45 115 ms 390 degree per second

Why *.hpp instead of *.cpp?

Every *.cpp file is compiled separately by a call of the compiler exclusively for this cpp file. These calls are managed by the IDE / make system. In the Arduino IDE the calls are executed when you click on Verify or Upload.
And now our problem with Arduino is: How to set compile options for all *.cpp files, especially for libraries used?
IDE's like Sloeber or PlatformIO support this by allowing to specify a set of options per project. They add these options at each compiler call e.g. -DTRACE.
But Arduino lacks this feature. So the workaround is not to compile all sources separately, but to concatenate them to one huge source file by including them in your source. This is done by e.g. #include "ServoEasing.hpp".
But why not #include "ServoEasing.cpp"?
Try it and you will see tons of errors, because each function of the *.cpp file is now compiled twice, first by compiling the huge file and second by compiling the *.cpp file separately, like described above. So using the extension cpp is not longer possible, and one solution is to use hpp as extension, to show that it is an included *.cpp file. Every other extension e.g. cinclude would do, but hpp seems to be common sense.

Using the new *.hpp files

In order to support compile options more easily, the line #include <ServoEasing.h> must be changed to #include <ServoEasing.hpp> in your main program (aka *.ino file with setup() and loop()).

In all other files you must use #include <ServoEasing.h>, to prevent multiple definitions linker errors:

If you forget to include ServoEasing.hpp, you will see errors like Simple.ino:57: undefined reference to ServoEasing::attach(int, int).

Ensure that all macros in your main program are defined before any #include <ServoEasing.hpp>.
The following macros will definitely be overridden with default values otherwise:

  • MAX_EASING_SERVOS
  • REFRESH_INTERVAL
  • USE_PCA9685_SERVO_EXPANDER

Compile options / macros for this library

To customize the library to different requirements, there are some compile options / macros available.
These macros must be defined in your program before the line #include <ServoEasing.hpp> to take effect.
Modify them by enabling / disabling them, or change the values if applicable.

Name Default value Description
USE_PCA9685_SERVO_EXPANDER disabled Enables the use of the PCA9685 I2C expander chip/board.
USE_SOFT_I2C_MASTER disabled Saves up to 1756 bytes program memory and 218 bytes RAM for PCA9685 I2C communication compared with Arduino Wire.
USE_SERVO_LIB disabled Use of PCA9685 normally disables use of regular servo library. You can force additional using of regular servo library by defining USE_SERVO_LIB. See below.
PROVIDE_ONLY_LINEAR_MOVEMENT disabled Disables all but LINEAR movement. Saves up to 1540 bytes program memory.
DISABLE_COMPLEX_FUNCTIONS disabled Disables the SINE, CIRCULAR, BACK, ELASTIC, BOUNCE and PRECISION easings. Saves up to 1850 bytes program memory.
MAX_EASING_SERVOS 12, 16(for PCA9685) Saves 4 byte RAM per servo. If this value is smaller than the amount of servos declared, attach() will return error and other library functions will not work as expected.
Of course all AllServos() functions and isOneServoMoving() can't work correctly!
DISABLE_MICROS_AS_DEGREE_PARAMETER disabled Disables passing also microsecond values as (target angle) parameter (see OneServo example). Saves up to 128 bytes program memory.
DISABLE_MIN_AND_MAX_CONSTRAINTS disabled Disables servo movement constraints. Saves 4 bytes RAM per servo but strangely enough no program memory.
DISABLE_PAUSE_RESUME disabled Disables pause and resume functionality. Saves 5 bytes RAM per servo.
PRINT_FOR_SERIAL_PLOTTER disabled Generate serial output for Arduino Plotter (Ctrl-Shift-L).
DEBUG disabled Generates lots of lovely debug output for this library.
USE_LEIGHTWEIGHT_SERVO_LIB disabled Available only for ATmega328. Supports only servos at pin 9 and 10. Makes the servo pulse generating immune to other libraries blocking interrupts for a longer time like SoftwareSerial, Adafruit_NeoPixel and DmxSimple. See below. Saves up to 742 bytes program memory and 42 bytes RAM.

Using PCA9685 16-Channel Servo Expander

Using the expander makes the servo pulse generating immune to other libraries blocking interrupts for a longer time like SoftwareSerial, Adafruit_NeoPixel and DmxSimple.
To enable the use of the expander, activate the line #define USE_PCA9685_SERVO_EXPANDER before #include <ServoEasing.hpp>.
In expander mode, timer1 is only required for the startEaseTo* functions and not for the blocking easeTo* functions, since no servo signal must be generated by it.

The pin number parameter of the attach function determines the port number of the PCA9685 and can be in the range from 0 to 15.

Be aware that the PCA9685 expander is reset at the first attach() and initialized at every further attach().
To control simultaneously servos with the Arduino Servo library i.e. servos which are directly connected to the Arduino board, activate the line #define USE_SERVO_LIB.
In this case you should attach the expander servos first in order to initialize the expander board correctly. And as long as no servo using the Arduino Servo library is attached, the expander servos will not move, which should not be a problem since you normally attach all servos in setup().
Resolution of the is PCA9685 signal is approximately 0.5 degree.

On the ESP32 the I2C library is only capable to run at 100 kHz, because it interferes with the Ticker / Timer library used. Even with 100 kHz clock we have some dropouts / NAK's because of sending address again instead of first data.
Since the raw transmission time of 32 Servo positions is 17.4 µs @ 100 kHz, not more than 2 expander boards can be connected to one I2C bus on an ESP32 board, if all servos should move simultaneously.
If you do not use any timer in your program you can increase speed up to 800 kHz. Maybe you have to attach 2 x 2.2 kΩ pullup resistors to the I2C lines to have it working reliably.


Using the included Lightweight Servo library for ATmega328

Using the Lightweight Servo Library reduces sketch size and makes the servo pulse generating immune to other libraries blocking interrupts for a longer time like SoftwareSerial, Adafruit_NeoPixel and DmxSimple.
Up to 2 servos are supported by this library and they must be physically attached to pin 9 and/or 10 of the Arduino board.
To enable it, activate the line #define USE_LEIGHTWEIGHT_SERVO_LIB before the line #include "LightweightServo.hpp" like it is done in the TwoServos example.
If you do not use the Arduino IDE, take care that Arduino Servo library sources are not compiled / included in the project.


Handling multiple servos with the internal ServoEasingArray

The ServoEasing library provides two arrays to ease the handling of multiple servos.

  • ServoEasing *ServoEasing::ServoEasingArray[MAX_EASING_SERVOS]
  • float ServoEasing::ServoEasingNextPositionArray[MAX_EASING_SERVOS]

Every ServoEasing object is appended to the ServoEasingArray by the attach() function. Only the order of the attach() statements determines the position in the array. So you can access your servo, which you attached secondly, also by ServoEasing::ServoEasingArray[1]->setEaseTo(135) as it is done here.
There are also many other *AllServos* functions like stopAllServos().

To move multiple servos synchronized, you can fill up the ServoEasing::ServoEasingNextPositionArray with the desired positions and then use e.g. the function setEaseToForAllServos(uint_fast16_t aDegreesPerSecond). Another example can be found here.
The Quadruped example makes heavy use of the *AllServos* functions.

If you detach a servo and then attach another one, the latter will get the index of the former detached one.


Description of examples


WOKWI online examples


Servo utilities


Building breadboard servo adapter

Converting a 10 pin double row pin header with 21 mm pin length to a breadboard servo adapter. Side view Top view


Internals

The API accepts only degree (except for write() and writeMicrosecondsOrUnits()) but internally only microseconds (or units (= 4.88 µs) if using PCA9685 expander) and not degree are used to speed up things. Other expander or servo libraries can therefore easily be used.


Supported Arduino architectures

Every Arduino architecture with a Servo library will work without any modifications in blocking mode.
Non blocking behavior can always be achieved manually by calling update() in a loop - see last movement in Simple example.
Interrupt based movement (movement without calling update() manually in a loop) is supported for the following Arduino architectures:
avr, megaavr, sam, samd, esp8266, esp32, stm32, STM32F1 and apollo3.


Timer usage for interrupt based movement

On AVR Timer1 is used for the Arduino Servo library. To have non blocking easing functions its unused Channel B is used to generate an interrupt 100 µs before the end of the 20 ms Arduino Servo refresh period. This interrupt then updates all servo values for the next refresh period.

Platform Timer Library providing the timer
avr Timer1 Servo.h
ATmega Timer5 Servo.h
megaavr TCA0
sam ID_TC8 (TC2 channel 2)
samd TC5
esp8266 + esp32 Ticker Ticker.h
stm32 TIM3 HardwareTimer.h
STM32F1 3 or 7 HardwareTimer.h
Teensy IntervalTimer
apollo3 timer 3 segment A
Mbed mbed::Ticker Ticker.h
[RP2040 / Pi Pico](https://github.com/earlephilhower/arduino-pico default alarm pool time.h

Adding a new platform / board

If timer support is available for a platform the library can be ported by adding code for the Timer20ms like is was done for ESP and STM.
To add a new platform, the following steps have to be performed:

  1. If the new platform has an Arduino compatible Servo library, fine, otherwise include the one required for this platform like it is done for ESP32 here.
  2. You need a 20ms interrupt source providing the functions enableServoEasingInterrupt() and (optional) disableServoEasingInterrupt(). Extend these functions with code for the new platform. Place includes and timer definitions at top of ServoEasing.hpp.
  3. If your interrupt source requires an ISR (Interrupt Service Routine) place it after disableServoEasingInterrupt() where all the other ISR are located.
  4. To test the new platform, you may want to enable TRACE output by activating the line #define TRACE in ServoEasing.hpp and enabling interrupt timing feedback by activating the line #define MEASURE_SERVO_EASING_INTERRUPT_TIMING in ServoEasing.hpp.
  5. If it works for you, please issue a Pull Request, to share your efforts with the community.

Good luck!


Troubleshooting

If you see strange behavior, you can open the library file ServoEasing.hpp and activate the line #define TRACE or #define DEBUG. This will print internal information visible in the Arduino Serial Monitor which may help finding the reason for it.


Revision History

Version 3.1.0

  • SAMD51 support by Lutz Aumüller.
  • Added support to pause and resume and DISABLE_PAUSE_RESUME.
  • Fixed some bugs for PCA9685 expander introduced in 3.0.0.
  • Feather Huzzah support with the help of Danner Claflin.
  • Added ENABLE_EXTERNAL_SERVO_TIMER_HANDLER macro.

Version 3.0.0

  • Added target reached callback functionality, to enable multiple movements without loop control.
  • Changed ENABLE_MICROS_AS_DEGREE_PARAMETER to DISABLE_MICROS_AS_DEGREE_PARAMETER thus enabling micros as parameter by default.
  • Fixed some bugs for micros as parameter.
  • Changed constants for easing types.
  • Additional parameter aUserDataPointer for user easing function.
  • New easing type PRECISION.
  • New function printEasingType().
  • Easing functions are converted to static member functions now.
  • Easing types can be disabled individually.
  • Improved PCA9685 handling / support for SoftI2CMaster.
  • Changed default for parameter doWrite for setTrim() from false to true.
  • Added min and max constraints for servo write() and DISABLE_MIN_AND_MAX_CONSTRAINTS.

Version 2.4.1

  • RP2040 support.

Version 2.4.0

  • New attach() functions with initial degree parameter to be written immediately. This replaces the attach() and write() combination at setup.
  • Renamed ServoEasing.cpp to ServoEasing.hpp and LightweightServo.cpp to LightweightServo.hpp.

Version 2.3.4

  • ENABLE_MICROS_AS_DEGREE_PARAMETER also available for PCA9685 expander.
  • Moved sServoArrayMaxIndex, sServoNextPositionArray and sServoArray to ServoEasing::sServoArrayMaxIndex, ServoEasing::ServoEasingNextPositionArray and ServoEasing::ServoEasingArray.
  • Support for Apollo3 2.x core.
  • Fixed ESP8266 pin definitions.

Version 2.3.3

  • Added compile option ENABLE_MICROS_AS_DEGREE_PARAMETER to allow usage of microseconds instead of degree as function arguments for all functions using degrees as argument.
  • Improved LightweightServo API.

Version 2.3.2

  • Removed blocking wait for ATmega32U4 Serial in examples.
  • Improved output for Arduino Serial Plotter.

Version 2.3.1

  • Fixed wrong timer selection for STM32F1xx / ARDUINO_ARCH_STM32.
  • Documentation.

Version 2.3.0

  • Fixed EASE_LINEAR formula bug introduced with 2.0.0 for 32 bit CPU's. Thanks to drifkind.
  • Added stop(), continueWithInterrupts() and continueWithoutInterrupts() functions.

Version 2.2.0

  • ATmega4809 (Uno WiFi Rev 2, Nano Every) support.
  • Corrected position of macro for MAX_EASING_SERVOS.

Version 2.1.1

  • Fixed bug in detach of first servo.

Version 2.1.0

  • Added support of Teensy boards.

Version 2.0.0

  • PCA9685_Expander and standard Servos can be controlled simultaneously by defining USE_SERVO_LIB.
  • Changed some types to _fast types
  • Standardize pins for all examples

Version 1.6.1

  • Fix bug for Arduino SAMD boards.

Version 1.6.0

  • Added support of Apollo3 boards.
  • Print library version in examples.

Version 1.5.2

  • More examples using areInterruptsActive().
  • Added support of Arduino SAMD boards.

Version 1.5.1

  • Added support for STM32 cores of Arduino Board manager. Seen in the Arduino IDE as "Generic STM32F1 series" from STM32 Boards.
  • Inserted missing Wire.begin() in setup of PCA9685_Expander example.
  • In isMovingAndCallYield() yield() only called/required for an ESP8266.
  • New function areInterruptsActive(), especially for ESP32.

Version 1.5.0

  • Use type Print * instead of Stream *.
  • New LightweightServoExample.
  • Added function delayAndUpdateAndWaitForAllServosToStop().
  • Added Arduino Due support by using timer 8.
  • New PCA9685_ExpanderFor32Servos example.

Version 1.4.3

  • Improved detach() handling.
  • Initialize variables explicitly to 0 in constructor. On an ESP8266 they were NOT initialized to 0 😞.

Version 1.4.2

  • Improved INVALID_SERVO handling.
  • Speed 0 (not initialized) handling.
  • Fixed bug in ThreeServos example.

Version 1.4.1

  • Improved documentation and definitions for continuous rotating servo. Thanks to Eebel!
  • Improved support and documentation for generating Arduino Serial Plotter output.
  • Support of STM32F1 / BluePill boards.

Version 1.4.0

  • setTrim has additional parameter doWrite which is default false in contrast to older versions, where a write was always performed.
  • New attach( aPin, aMicrosecondsForServoLowDegree, aMicrosecondsForServoHighDegree, aServoLowDegree, aServoHighDegree) function for arbitrary mapping of servo degree to servo pulse width.
  • Order of Servos in sServoArray[] now depends from order of calling attach() and not from order of declaration.
  • New example for continuous rotating servo.

Version 1.3.1

  • Added detach() function.

Version 1.3.0

  • Added ESP32 support by using ESP32Servo.h and Ticker.h instead of Servo.h timer interrupts.
  • Changed degree parameter and values from uint8_t to integer to support operating a servo from -90 to + 90 degree with 90 degree trim.
  • RobotArmControl + QuadrupedControl examples refactored.
  • Changed "while" to "for" loops to avoid a gcc 7.3.0 atmel6.3.1 bug.
  • Extended SpeedTest example. Now also able to change the width of the refresh period.

Version 1.2

  • Added ESP8266 support by using Ticker instead of timer interrupts for ESP.
  • AsymetricEasing example overhauled.

Version 1.1.0

  • Corrected sine, circular, back and elastic IN functions.
  • easeTo() and write() store their degree parameter now also in sServoNextPositionArray.
  • added setSpeed(), getSpeed(), setSpeedForAllServos() and added ease* functions without speed parameter.
  • added getEndMicrosecondsOrUnits(), getDeltaMicrosecondsOrUnits().
  • added setDegreeForAllServos(uint8_t aNumberOfValues, va_list * aDegreeValues),setDegreeForAllServos(uint8_t aNumberOfValues, ...).
  • added compile switch PROVIDE_ONLY_LINEAR_MOVEMENT to save additional 1500 bytes program memory if enabled.
  • added convenience function clipDegreeSpecial().

Version 1.0.0

Initial Arduino library version

CI

Since Travis CI is slow (5 times slower 17:43 vs. 3:15 minutes), the library examples are now tested with GitHub Actions for this boards.

Requests for modifications / extensions

Please write me a PM including your motivation/problem if you need a modification or an extension.

If you find this library useful, please give it a star.