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

// Include servo library
#include <Servo.h>
Servo myservo;

void setup() {
    myservo.attach(9);
}

void loop() {
    // Move to 180°
    myservo.write(180);  
    delay(2700);

    // Move back to 0°
    myservo.write(0);
    delay(2700);
}

Example 2: Sweep servo commands

#include <Servo.h>
Servo myservo;
int pos = 0; 

void setup() {
    myservo.attach(9);
}

void loop() {
    // Move to 180° in steps of 1°
    for (pos = 0; pos <= 180; pos += 1) {
        myservo.write(pos);
        delay(15);
    }
    // Move back to 0° in steps of 1°
    for (pos = 180; pos >= 0; pos -= 1) {
        myservo.write(pos);
        delay(15);
    }
}

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:

How to connect a battery to the Arduino

When working with sensitive circuits and micro-controllers, batteries can be a tricky thing to get right. It is not as simple as just buying a battery with the same voltage as what the micro-controller requires; the issue with batteries is that their voltage does not remain constant. The voltage stated on the battery usually tells you the approximate voltage when it is at 50% charge. The voltage tends to be slightly higher when fully charged, and drops below this value as it is drained. Battery power level indicators work by analysing this voltage and then calculating how much power is left.

Most Arduino compatible micro-controllers have several different ways in which they can be powered. Through the (1) USB jack, the (2) Power Jack, the (3) 5V Pin, and the (4) Vin Pin. Many Arduino’s also have a 3.3V pin, however this is only a voltage output and cannot be used to power the board. The voltage input (Vin) pin and power jack are both connected to the same power regulation circuitry, which can take in voltages between 7-12V and convert them into stable 5V used by the micro-controller. If you use the 5V pin to provide power, this regulation circuitry is bypassed and feeds the chip directly. This means that if the voltage provided to the 5V pin deviates by as little as ±0.1v, you could potentially fry the board.

As the voltage of a battery varies with use, I would always connect it through the Vin pin or the power jack. This allows the power regulation circuitry to deal with any variances in the voltage and prevent the battery from frying the board.

However, frying the board isn’t the only danger when using a battery! As the battery is drained and its voltage drops below the micro-controller’s minimum voltage requirement, a brown-out can occur. Brown-outs happen when the micro-controller partially shuts-down due to a power shortage. These usually cause the controller to restart, but they could also potentially damage the controller. Some batteries (especially LiPos) also are damaged if they are over-discharged and the voltage drops too low. This makes it important to monitor the voltage of the battery during use, so that we know when the battery level is too low and it needs to be recharged.

Protecting your Battery from being Over-discharged

The best way to prevent a brown-out is to make sure that the voltage being supplied is always above the minimum required voltage for the controller. If the battery falls below this minimum voltage, the micro-controller should display a warning message, or switch itself off. A nice way of dealing with this is to make the micro-controller monitor its own power supply by using one of the analogue input pins. If the voltage of the battery is less than 5V, it can be connected directly into the analogue pin of an Arduino. Otherwise, we will have to use a potential divider circuit in order to reduce the voltage to a safe level.

For my second version of the self-balancing robot, I am using a 11.1V lithium quad-copter battery to power a Galileo Gen2 Board and two 12V motors. As the voltage of the battery is greater than 5V, I used the following circuit in order to monitor the battery as it is being used:

When two resistors are connected in series like this, with one end connected to a certain voltage and the other to ground, the voltage at the junction of the two resistors is determined by the ratio of the two resistances. The formula is as follows, where (Vin) is the input voltage, (R1) and (R2) the resistances of the first and second resistor, and (Vdiv) the voltage at the junction between both resistors:

The ratio of the resistors should be chosen, so that the voltage at the junction (Vdiv) is below 5V when the battery is at its maximum voltage. Since the Arduino measures the lower voltage (Vdiv) at the junction of both resistors, this formula can be rearranged to calculate the voltage of the battery (Vin):

The code I use to monitor the battery percentage on the Arduino is shown below. The same formula is used on line 22 to calculate the actual battery voltage. For the 11.1V LiPo battery I used, the maximum voltage at full charge was 12.6V. The minimum voltage below which it should no longer be used was 10.2V.

// Pin number of the analog input being used for the reading
#define ANALOG_PIN 2

// Values for the two resistors used in the potential divider
float resistorOne = 100000;
float resistorTwo = 47000;

float vRef = 5;               // Reference voltage - usually 5v or 3.3v
float maxVoltage = 12.6;      // The voltage of the battery when fully charged
float minVoltage = 10.2;      // Voltage below which battery isn't safe to use

void setup() {
    // Initialize serial communication
    Serial.begin(9600);
}

void loop() {
    // Calculate voltage at the analog pin
    float voltage = analogRead(ANALOG_PIN) * vRef / 1024.0;

    // Calculate the battery voltage
    voltage = voltage * (resistorOne / resistorTwo + 1);

    // Calculate the battery percentage
    float batteryPercentage = (voltage - minVoltage) / (maxVoltage - minVoltage) * 100;

    // Limit the percentage value to be between 0 to 100%
    if (batteryPercentage > 100) batteryPercentage = 100;
    else if (batteryPercentage < 0) batteryPercentage = 0;

    // Output the calculated values to the serial monitor
    Serial.print(voltage); Serial.print("V, ");
    Serial.print(batteryPercentage); Serial.println("%");
    delay(1000);
}

The serial output I got from the system then looked like this:

11.07V, 36%
11.29V, 45%
11.29V, 45%
11.29V, 45%

In my test, the actual voltage of the battery according to the Lithium battery charger was 11.3V, so the value recorded by the micro-controller was relatively accurate.

Here is a quick summary of the points I discussed above:

1. Never connect a battery directly into the 5v or 3.3v pins of the micro-controller. These inputs require the voltage to be properly regulated.

2. Always ensure that the battery is within  the controller’s specified voltage input range. For the Arduino Uno this is 7-12V.

3. Avoid brown-outs and battery over-discharge by monitoring the voltage of the battery and switching off the system if it drops below the minimum voltage.

4. When monitoring the voltage, take caution that the voltage being measured does not exceed the reference voltage (5V). Reduce the voltage by using a potential divider circuit.

Updated: 24th May 2019 – Reformatted post

Updated: 28th May 2020 – Added extra detail about the potential divider circuit