Servo Trajectory Controller for Arduino

Servo motors are used in almost all hobby robotics projects, as they allow you to control the position of joints without too much effort. Using the core “Servo” library which comes with Arduino, it is easy to command the motor to move to a specific position/angle. However, controlling the speed and acceleration of the movement is a bit more difficult. In this post, I describe how servo motors work, and different ways in which they can be controlled. To make it easier to precisely control the motion of servo motors, I have created a “Trajectory” library which can be used in conjunction with the “Servo” library to control the motion of servo motors. A full description of how to use this controller is included in this post, along with an example sketch to show how it can be used in practice. The full library and example sketch can be downloaded from my GitHub page.

Contents:

1. How do Servo Motors Work?
2. Controlling Hobby Servo Motors
3. Using my Trajectory Controller
4. Example Sketch
5. Timing

1. How do Servo Motors Work?

A servo motor is any type of actuator which uses sensor feedback to control the position, velocity or acceleration of the output. This type of system is commonly used in industrial automation, and there are a wide variety of different sensors, motors and control algorithms to choose from. However, in hobby electronics the term “servo motor” usually refers to a specific type of motor commonly used in small robots and radio-controlled vehicles. The image above shows what these types of motor look like; a regular servo motor and a micro servo motor are shown.

Hobby servo motors usually consist of a small DC motor, a gearbox, a controller chip and a potentiometer (rotary variable resistor) connected directly to the output shaft. As the output shaft turns, the resistance of the potentiometer changes which allows the controller to determine how far the motor has moved. When you send a new position command to the servo motor (usually in the form of a PWM signal), the controller moves the motor until the error between the target position and the current position is zero.

Most servo motors can only turn up to 180°. This limitation is caused by the potentiometer within the servo, which cannot rotate much more than this without breaking. It is possible to buy “continuous rotation” servo motors, but these aren’t quite the same; the name is actually a bit misleading. In continuous rotation servo motors, the potentiometer is removed so that the output shaft can rotate more than 180°. However, this also removes the motor’s ability to figure out how far it has turned. It therefore acts more like a normal DC motor with a speed controller (h-bridge), and shouldn’t actually be called “servo” since it isn’t using any sensor feedback to control its position.

2. Controlling Hobby Servo Motors

When you send a new position command to the servo motor, it quickly jumps to the target position at maximum speed (see Example 1). While this is very simple to program, it is only useful in a small number of applications where the speed of the motion doesn’t matter. For example, if you are trying to control a humanoid robot, you want to joints to move slowly and smoothly, rather than creating a robot which wildly flails its arms around! To get around this limitation, most programs split the movement down into small steps, which get sent to the servo motor individually over time. This is how the standard “Sweep” servo example sketch works (see Example 2). The code splits the movement into smaller but equal steps, making the servo move at a constant slower speed.

Example 1: Raw servo commands

Example 2: Sweep servo commands

However, when I was building my Wall-E robot, this was not quite good enough. Joints don’t usually move by just jumping suddenly to a certain velocity, and then stopping dead when the target is reached. There should be a gradual acceleration as the movement starts, and a slow deceleration as the joint approaches its target position. This motivated me to write my own Arduino class, which makes performing these types of movements simple!

3. Using my Trajectory Controller

In this section, all of the different functions of the class are described individually. If you are impatient and want to see how to use it all together in practice, you can jump straight to the “Example Sketch” section! To download the full trajectory controller library, please visit my GitHub repository.

a. Instantiating the Class

First of all, make sure that the file “trajectory.h” is contained within the same folder as the Arduino sketch you want to use the controller in. To tell the sketch that you want to use the class, you need to include it at the top of the code:

#include "trajectory.h"

Next, the class needs to be instantiated. In this step, there are several parameters which you can provide, which determine how the controller will function. If you are happy with using some of the default values, you can omit parameters from the right side of the instantiation statement… An example of how it works is shown below. “Units” refers to the unit type used to measure the position of the system. For servo motors, this would usually be measured in degrees, but the class can actually work in any units (meters, radians, feet etc.). The “Threshold” is the difference between the current position and target position at which the controller turns off.

/**
 * FORMAT: Trajectory(float maxVelocity, float acceleration, float deceleration, float threshold)
 * @param Maximum Velocity (units/second) - default = 100
 * @param Acceleration (units/second^2) - default = 50
 * @param Deceleration (units/second^2) - default = same as acceleration
 * @param Threshold (units) - default = 0.1
 */
// For example:
Trajectory servoTrajectory(20, 15, 12.5, 0.01);

// If the default threshold of 0.1 doesn't need to be changed:
Trajectory servoTrajectory(20, 15, 12.5);

// If you want the acceleration and deceleration to be the same:
Trajectory servoTrajectory(20, 15);

b. Setting a new Target

The class has three main ways in which it can be controlled:

i. Position – when a new position target is provided, the system accelerates at a constant rate until a maximum velocity is reached. Then once it approaches the target, it decelerates and comes to a stop just on the target position:

// FORMAT: void setTargetPos(float newPosition)
// For example, move to the 180° position:
servoTrajectory.setTargetPos(180);

ii. Position and Time – it is also possible to provide a target position and the amount of time the system should take to reach that position. While the acceleration and deceleration stay the same, the maximum velocity is reduced so that the system takes just the right amount of time to reach the target (it took me a while to figure out the formula for that! If you are interested to see how it works, check out this interactive graph I made). Depending on the maximum acceleration and velocity, it may not be possible for the system to reach the target position within the specified time. In these cases, the function notifies this by returning “False” rather than “True”, and it reverts to the normal position-only control.

// FORMAT: bool setTargetPos(float newPosition, float timeSeconds)
// For example, move to 180° over 2 seconds:
bool status = servoTrajectory.setTargetPos(180, 2));
if (status) Serial.println("Command Successful");
else Serial.println("Unable to reach target within specified time");

iii. Velocity – Finally, it is also possible to control just the velocity of the system. This is not applicable to normal hobby servo motors which can only rotate 180°, but it can be used for continuous-rotation motors with encoders. When a new velocity value is provided, the system accelerates/decelerates at a constant rate from the initial velocity until the target speed is reached.

// FORMAT: void setTargetVel(float newVelocity)
// For example, set velocity to 50°/s:
servoTrajectory.setTargetVel(50);

c. Using the Output from the Controller

To use the controller to control the position and velocity of a servo motor, the “update” function needs to be called at regular intervals in order to recalculate all the values. This new position value then can be sent to the servo motor. The control system inside most hobby analogue servo motors updates at a frequency of 60Hz, so updating the velocity controller class at a frequency equal to or slightly higher than this will give the best results. An example of how to use the “update” function is shown below. In the example, the variable “nextTime” is used to ensure that the update function is only run once every 1/60th of a second, to achieve a frequency of 60Hz. The new angle value is obtained directly from the update function, and is then rounded to the nearest integer before being sent to the servo motor:

#define TIMER_FREQ 60
unsigned long nextTime = millis();

void loop() {

	// Run this code once every 16ms (~60Hz)
	if (nextTime <= millis()) {
		nextTime = millis() + (1000 / TIMER_FREQ);

		// Update the controller and get the new position value
		float newAngle = servoTrajectory.update();
 
		// Send the new value to the servo motor
		myservo.write(round(newAngle));
	}
}

d. Other Functions

i. Current Position/Velocity: The current position and velocity of the controller can be read, along with the value of the current target. There is also a function with which the current position of the system can be changed:

// Get the current position and velocity
float currentPosition = servoTrajectory.getPos();
float currentVelocity = servoTrajectory.getVel();

// To change the current position to 22.5°
servoTrajectory.setPos(22.5);

// To reset the position back to 0°, the input can be left empty
servoTrajectory.setPos();

// To read the current target position/velocity value
float currentTarget = servoTrajectory.getTarget();

ii. Update class settings: The class provides some functions with which you can read and update the current velocity, acceleration and deceleration settings of the controller:

// For example, setting the maximum velocity to 40.5°/s,
// acceleration to 10.6°/s^2 and deceleration to 23°/s^2
servoTrajectory.setMaxVel(40.5);
servoTrajectory.setAcc(10.6);
servoTrajectory.setDec(23);

// To read the current settings for velocity, acceleration and deceleration
float maxVelocity = servoTrajectory.getMaxVel();
float acceleration = servoTrajectory.getAcc();
float deceleration = servoTrajectory.getDec();

iii. Reset controller: The controller uses an integrated timer which calculates the time difference since the “update” function was last called. If the controller is turned on again after the “update” function has not been used for a while, the timer needs to be reset. The reset function should be called once at the end of the “setup” function. The current position of the controller gets set back to 0 by default, but this can be changed by providing the desired position to the function:

// To reset the controller, resetting the current position to 0°
servoTrajectory.reset();

// To reset the controller and change the position to 22.5°
servoTrajectory.reset(22.5);

// To reset the controller, but keep the position the same as it was
servoTrajectory.reset(servoTrajectory.getPos());

4. Example Sketch

I have written a small example sketch for my library, which shows how the class can be used in practice. In the example, the aim is to smoothly move a servo motor back and forth between the 20° and 180° position. The maximum velocity is set to 60°/second, the acceleration to 40°/s² and the deceleration to 34°/s². The first two moves use the basic “setTargetPos” function to rotate the servo to the target position as quickly as possible. The last two move specify both the target position and time to the class, causing the servo to move at a slower speed. The servo position is recalculated and updated at a rate of 100Hz (once every 10 milliseconds):

/* * * * * * * * * * * * * * * * * * * * * * *
 * EXAMPLE SKETCH FOR THE SERVO TRAJECTORY CONTROLLER CLASS
 *
 * Code by: Simon Bluett
 * Version: 1.2
 * Copyright (C) 2020, MIT License
 * * * * * * * * * * * * * * * * * * * * * * */


// --- Include the library ---
// Make sure that the file "trajectory.h" is included in 
// the same folder as the Arduino sketch
#include "trajectory.h"
#include <Servo.h>


// --- Instantiate the class ---
/**
 * If you want the acceleration and deceleration to be the same
 * FORMAT: Trajectory(max velocity, acceleration)
 */
//Trajectory servoTrajectory(60, 40);

/**
 * If the acceleration and deceleration are different
 * FORMAT: Trajectory(max velocity, acceleration, deceleration)
 */
Trajectory servoTrajectory(60, 40, 34);

/**
 * By default the dynamics controller turns off when it is within 0.1 units
 * of the target position. This threshold value can be changed in the declaration
 * FORMAT: Dynamics(max velocity, acceleration, deceleration, threshold)
 */
//Trajectory servoTrajectory(60, 40, 34, 0.05);

Servo myservo;


// --- Define global variables ---
// The controller will be updated at a rate of 100Hz
#define UPDATE_FREQUENCY 100
#define UPDATE_TIME (1000 / UPDATE_FREQUENCY)
unsigned long updateTimer = 0;
int moveNumber = 0;


/* * * * * * * * * * * * * * * * * * * * * * *
 * SETUP
 * * * * * * * * * * * * * * * * * * * * * * */
void setup() {

	Serial.begin(115200);
	Serial.println("Starting program");

	// Attaches the servo on pin 9 to the servo object
	myservo.attach(9);

	// We want the servo to start at an angle of 20°
	myservo.write(20);

	// By default the controller starts at 0, so we need to
	// set the starting angle as well
	servoTrajectory.reset(20);

	/**
	 * If we suddenly decide we want to change the maximum velocity to 30°/s,
	 * the acceleration to 15°/s^2 and deceleration to 5.3°/s^2
	 */
	//servoTrajectory.setMaxVel(30);
	//servoTrajectory.setAcc(15);
	//servoTrajectory.setDec(5.3);

	/**
	 * To read what the current velocity and acceleration settings are
	 */
	//float maxVelocity = servoTrajectory.getMaxVel();
	//float acceleration = servoTrajectory.getAcc();
	//float deceleration = servoTrajectory.getDec();
}


/* * * * * * * * * * * * * * * * * * * * * * *
 * NEW MOVEMENT COMMANDS
 * * * * * * * * * * * * * * * * * * * * * * */
void nextMove() {
	switch (moveNumber) {
		case 0:
			// First we move to the 180° position as fast as possible
			servoTrajectory.setTargetPos(180);
			break;

		case 1:
			// Then move back to 20° as fast as possible
			servoTrajectory.setTargetPos(20);
			break;

		case 2:
			// Next move to 180°, but over the course of 5 seconds
			servoTrajectory.setTargetPos(180, 5);
			break;

		case 3:
			// Finally back to 20°, taking 8.5 seconds
			servoTrajectory.setTargetPos(20, 8.5);
			break;

		default:
			// If all other moves have completed, stop the program
			Serial.println("All moves completed");
			while(1) {}
	}

	moveNumber++;
}


/* * * * * * * * * * * * * * * * * * * * * * *
 * LOOP
 * * * * * * * * * * * * * * * * * * * * * * */
void loop() {

	// Update the servo position at regular intervals
	if (updateTimer <= millis()) {
		if (updateTimer <= millis() - UPDATE_TIME) updateTimer = millis() + UPDATE_TIME;
		else updateTimer += UPDATE_TIME;

		// Update the controller
		float currentAngle = servoTrajectory.update();

		// Set the new servo position; the function only takes integer numbers
		myservo.write(round(currentAngle));

		/**
		 * For more precise servo control, you could use writeMicroseconds.
		 * The min and max PWM pulse widths which correspond to the 0° and 180°
		 * positions needs to be inserted for MIN_PWM and MAX_PWM.
		 */
		//myservo.writeMicroseconds(map(currentAngle, 0, 180, MIN_PWM, MAX_PWM));

		// Output the target position, along with the current position and velocity
		Serial.print("Target: ");
		Serial.print(servoTrajectory.getTarget());
		Serial.print(", Angle: ");
		Serial.print(servoTrajectory.getPos());
		Serial.print(", Velocity: ");
		Serial.println(servoTrajectory.getVel());

		// Only once the servo has reached the desired position, complete the next move
		if (servoTrajectory.ready()) {
			nextMove();
		}
	}
}

The output position, velocity and acceleration of the system for each of the four moves is shown in the diagram below. As you can see, the position stops and starts smoothly. This type of motion is called a “trapezoidal velocity” trajectory, which comes from the shape seen in the “Velocity” graph. You may notice the there is sudden drop in the velocity just as the system reaches the target position. This is caused by the position “threshold” defined in the class, which turns off the controller when the system comes within a certain distance of the target position (by default this is set to 0.1).

5. Timing

The class is relatively fast, making it useful for controlling systems which need a high refresh rate. The table below records the approximate times taken by a variety of micro-controllers to run the main functions of the class. The results show that using an Arduino Uno at an update rate of 100Hz, the class should be able to control up to 30 servo motors before slowing down. The Arduino Uno and Pro Mini both use the same ATmega 328P micro-processor, which explains why their speed is identical:

Function	Arduino Uno	Arduino 101	Pro Mini
update()	0.164 ms	0.014 ms	0.164 ms
targetPos(x)	0.008 ms	0.002 ms	0.008 ms
targetPos(x,t)	0.28 ms	0.40 ms	0.28 ms
targetVel(v)	0.01 ms	0.005 ms	0.01 ms