1. Series Battery Method

One of the simplest ways to create a dual power supply is by using two sets of batteries. The batteries are connected in series, so that the positive terminal of one battery is attached to the negative terminal of the second battery. When this middle connection is used as the ground reference for the circuit, you will be able to get a positive and negative voltage from the batteries, as shown in the circuit diagram below. For small and portable synthesisers, this is often done using two 9V batteries as I’ve demonstrated on a breadboard in the image below. Since the voltage of both batteries will drop as the power is drained, we also need to include voltage regulators which ensure a stable voltage is supplied to the synthesiser. In the image below, you can see that the batteries I am using are almost empty as the voltage measured by my multimeter is only -7.11V.

This method only works when one or both voltage sources are said to be “floating”. This means that the power source is not connected to any absolute reference voltage, such as a connection to Earth. All batteries are floating power sources, however wired power supplies often aren’t. For example, if the negative terminal of both voltage sources is connected to ground, then attaching the positive and negative terminals of both sources together will simply create a short-circuit; this is something I would encourage you to avoid!

• Benefits:
  • Very easy to implement and troubleshoot.
  • Relatively Portable.
  • Voltage can be increased by adding more batteries in series.
  • The battery life and maximum output current can be increased by adding more batteries in parallel.
• Disadvantages:
  • Batteries constantly need to be replaced!
  • The voltage of the batteries will drop as they run out (as seen in the image), so an additional power regulator IC will still be required.

2. Dual AC to DC Rectification

The electricity being supplied in the mains socket alternates from a positive to a negative voltage many times a second (230V 50Hz in Europe, 120V 60Hz in the US). What we want to do is reduce this voltage down to a lower and more manageable voltage, taking the positive half of the AC signal to supply the positive output and the negative half for the negative output. This process requires the following steps:

• Step down the high voltage being supplied by the mains to a lower voltage using a transformer.
• Rectify the AC signal into a positive and negative signal using diodes.
• Smooth out the voltage using capacitors.
• Generate a stable output voltage using power regulators.

a. Half-wave Rectifier Circuit

This is the power supply design I used in my synthesiser, and it is probably the most common design used by DIY synth builders. This design is often preferred to the Full-wave Rectifier as you can use a commercial wall plug transformer to convert the mains power down to 12V AC, which is used by the power supply. This means that your circuit does not directly come into contact with the mains power, making it a little bit safer to work with (but you still need to be careful!).

Important: You need to make sure that the wall plug transformer you use outputs 12V alternating current, and not 12V direct current. The 12V DC plugs are a lot more common, so it may take some searching to find the correct type of 12V AC plug. Also make sure that the plug you get is rated for a current of at least 1000mA or above, and that the mains voltage input rating is correct for the country you are in.

An example of a half-wave rectifier circuit is shown in Schematic 2 below. The circuit takes in a 12V AC signal from the wall plug, and converts it into a stable positive and negative 12V output. I have seen many variations of this circuit, using a wide variety of different capacitor values.

How does it work?

1. The circuit takes in a 12V alternating current signal from the wall plug transformer. 12V AC refers to the root mean square (RMS) value of the signal. This signal has a peak voltage of ±17V, as shown in the waveform diagram below.
2. The diode D1 only allows the positive half of the AC signal to pass through, while D2 lets negative voltages through. This process is known as half-wave or half-bridge rectification, since only half of the AC waveform is used to power each of the voltage outputs. As a result, each output can theoretically only output half of the power (and consequently current) delivered by the wall plug transformer. The peak voltage of the rectified signals is ±16.3V, since the diodes introduce a 0.7V drop into the circuit.
3. The capacitors smooth out the waveform, ensuring that a more continuous voltage is being supplied to the voltage regulators. The reasoning behind selecting this specific capacitance value is discussed in the next section.
4. The LM7812 and LM7912 voltage regulators ensure that the outputs of the power supply stay at a stable +12V and -12V respectively. If you want to get +15V and -15V outputs instead, you can use a 15V AC power plug and replace these with the LM7815 and LM7915 regulators. If you are putting together your own circuit, watch out as the input, output and ground pins are in a different order on the positive and negative voltage regulators.
5. Capacitors C3 and C4 are mainly included to improve the transient response of the power supply; the capacitor can provide brief bursts of high current when there are sudden changes in load being applied to the power supply. According to the data-sheet for the negative voltage regulator LM7912, for stability the capacitor C4 should be at least 1μF (using a tantalum capacitor) or 10μF (using an electrolytic capacitor). The higher value of 100μF was chosen to give an additional factor of safety over this minimum value.
6. The two LEDs are there to indicate that there is power at the outputs. Some negative power regulators also require a minimum load to be applied at the output before they start up, so the LEDs help to provide that load.
7. According to the data-sheet for LM7912, the diode D4 is required when large capacitors such as C10 are used at the input. The diode prevents momentary input short circuits, which can occur when the circuit is powered up or down. The LM7812 does not necessarily need this, but I put D6 in for good measure.
8. The data sheet for both LM7812 and LM7912 specify that D5 and D3 should be present to prevent latch-up problems. These components act as clamping diodes, helping to protect the regulators from reversed polarity on the outputs. If one regulator starts up before the other one, devices such as operational amplifiers (op amps) can latch up and cause a short circuit between both power rails. This can prevent the second regulator from starting up. The diodes (preferably Schottkey) prevent the positive output from going below -0.3V and the negative output going above 0.3V, allowing both regulators to start up and the latch up condition to stop.

How to choose the capacitor values?

Why are there two capacitors at the input of each power rail (C1 & C7, C2 & C10)? How were the values of these capacitors chosen? I’ve looked at several schematics for half-wave rectifiers and there seems to be a lot of variance in what the capacitance value should be.

Generally there is one small non-electrolytic capacitor close to the input of each power regulator, which helps to stabilise, filter and smooth the input (C1 and C2). Usually this is between 100nF to 1μF. Small capacitors (ceramic, polyester, tantalum etc.) tend to be better than larger electrolytic film capacitor at filtering out high-frequency noise from the signal.

Then there is a bank of large electrolytic capacitors connected in parallel (C7 and C10; more capacitors can be connected if required), ensuring that there is a relatively constant reservoir of power even when the AC input signal is in the opposite half of the wave and no new power is being supplied. These capacitors are good at removing low frequency noise and stabilising variances in the DC voltage. The total capacitance of this reservoir depends on the amount of load you expect to put on the power supply. Here is how to calculate how much capacitance you may need:

V_{DC}=\frac{16.3+14.5}{2}=15.4V

\rho_\%=\frac{15.4-14.5}{15.4}\times100=5.8\%

R_L=\frac{V_{DC}}{I_{DC}}=\frac{15.4}{1}=15.4\Omega

P_D = (V_{DC}-V_O )(I_{DC})\newline=(15.4-12)(1)=3.4W

C_s=\frac{1}{\rho _\%R_L}=\frac{1}{5.8\times 15.4}=0.011F

\text{If } \quad C_s=0.0047F \quad \text{then:}

R_L=\frac{1}{5.8\times 0.0047}=36.7\Omega

I_{DC}=\frac{15.4}{36.7}=0.42A

So to summarise… if we want to get a full 1A of output current from our power supply, the combined capacitance value at the input of the regulator needs to be at least 11,000µF.

b. Full-wave Rectifier Circuit

In a “full-bridge” or “full-wave” rectification circuit, both the positive and negative sections of the alternating waveform are used to power both outputs. This means that the circuit can theoretically drive twice the load compared to a half-bridge rectifier. As can be seen in Schematic 3, most of the circuit is identical to the half-bridge rectifier. The only differences are that two additional rectification diodes have been added, and a transformer with three outputs (called a “Center Tapped Transformer”) is used. The central output of the transformer is used as the ground reference, while the other two connections output an identical 12V AC signal, but out of phase by 180°. This means that when one of the outputs is in the positive section of the alternating waveform, the other is in the negative section and vice versa.

This type of circuit is often used in professional equipment, but is not used as much by DIY synthesiser builders. Center-tapped transformers are not available as a pre-packaged wall-plug, so you would need to wire your own. Since one end of the transformer is connected to mains power, building this circuit involves a bit more risk and should only be attempted if you have the right equipment and know what you are doing! When buying a transformer, make sure that the mains voltage input rating is correct for the country you are in.

How does it work?

1. The transformer takes the mains alternating signal and reduces the voltage, outputing two 12V alternating current signals which are out of phase by 180°.
2. The four diodes are used to separate the positive and negative sections of the alternating signal, directing the positive halves to the +12V regulator and the negative halves to the -12V regulator. Since both AC signals are out of phase, this results in a continuous supply of power for both polarities.
3. The rest of the circuit is identical to the “half-bridge rectifier”, so you can refer to my description above to see how it works and what each component is doing.

3. DC to DC Inverting Charge Pump

It is also possible to generate a dual 12V power supply from only one +12V DC power plug. This is useful since DC power plugs are a lot more common and therefore cheaper to buy. It is also easier to find 12V DC plugs which have a high current rating, allowing more synthesiser modules to be powered from the same supply. This type of power supply design is often seen in portable modular synthesiser kits, and small Eurorack-compatible power modules. Since the transformer and rectification circuitry (large capacitors) are all contained within the external plug, the footprint of the electronics used in this design can be made a lot smaller than in the Dual AC-DC rectification circuits.

a. How does it work?

In its most simple form, an inverting charge pump uses a “floating” capacitor to carry charge over from the +12V side to the -12V side of the circuit. The capacitor is charged up from the +12V input being provided by the wall plug. Once full, the capacitor is disconnected from +12V input and the positive lead is instead connected to ground. Since the charge (and therefore voltage drop) across the capacitor remains the same, this means the negative terminal of the capacitor is now at a voltage of -12V. The capacitor then begins to discharge and this is used to power the negative rail. In our power supply, this process of charging and discharging is repeated many times a second. Schematic 4 shows an equivalent circuit, demonstrating how this system works. In a real circuit, the switching of the capacitor is done using an IC chip.

1. Initially, switches S1 and S3 are closed while switches S2 and S4 are open. Capacitor C1 is connected to Vin and ground, causing the charge in the capacitor to increase.
2. After a certain interval, the switches S1 and S3 are opened up again while S2 and S4 are closed. The top leg of the capacitor is now connected to ground instead of Vin. Since the charge in the capacitor hasn’t changed, there is still the same voltage drop across the capacitor. As a result, a voltage of -Vin is present on the bottom leg of the capacitor.
3. This switching mechanism is continuously repeated, charging the capacitor C1 with the positive input voltage and de-charging it again on the inverted output. The capacitor is essentially pumping the charge from the positive input to the inverted output.
4. Capacitor C2 acts as a power buffer/storage, smoothing the voltage on the output and ensuring that a continuous supply is available at the inverted output.

b. Implementing it in practice

In the example circuit show in Schematic 5, we are using the LTC1144 chip made by Analog Devices to do the switching for out inverting charge pump. The capacitor C6 is used to invert the charge, while C5 acts as reservoir so that the negative output has a more stable output. The graphs show how the circuit reacts when it is started up. The current through the capacitor C6 alternates from positive to negative at regular intervals as it charges from the positive supply and de-charges into the negative output. The voltage of the negative output quickly decreases as the reservoir capacitor C5 is charged up, levelling out at -12V over time.

In the LTC1144 chip, the frequency of the switching signal can be increased or decreased by changing the value of the capacitor connected to the OSC input pin. Charge pumps can operate at a wide range of switching frequencies, usually ranging from 1kHz to as high as 200kHz.

Note: I haven’t had the chance to try this circuit out in practice, so the capacitor values in Schematic 5 will probably need to be altered to make it suitable for use as a synthesiser power supply. The circuit simulations were done in the free program “LTspice” made by Analog Devices.

Building a synthesiser rather than buying a pre-assembled one has several benefits; first of all, it is significantly cheaper! For example, a simple voltage-controlled oscillator (VCO) Eurorack module usually costs at least €100, while the components for a DIY version can often be sourced for less than €20. But more importantly, building a DIY synth helps you to understand exactly how each circuit works and how it influences the overall sound. Once you become familiar with how a circuit works, you can also begin adding your own extra features and quirks! There is quite a large DIY synth community where people show off and share their own designs. For me at least, I find that building my own synth is just as much fun as trying to make music with it once it is complete.

What is a Modular Synth?

Synthesisers are devices which can create sounds and music from electronic circuits. A sound can be created very simply by generating an oscillating electrical signal, which is varying at a frequency which we can hear (usually between 20 to 20,000Hz). By changing the rate at which this signal oscillates we control the pitch of the tone, while the volume can be controlled by adjusting the peak-to-peak voltage of the signal. Diagram 1 shows what changing the volume and pitch of an sinusoidal signal looks like. However, generating a basic signal is only the starting point when using a synth! By combining multiple signals and filtering them in weird and wonderful ways, it is possible to create some truly unique sounds and music.

Basic synthesisers have multiple functions hardwired together to give you a specific type of instrument and tone. Modular synths are special, because each unique musical operation is broken out into a separate device/module. For example, you could have one module to generate an initial waveform (oscillator), then one to change the tone of the sound (filter), followed by a module to control the volume (amplifier). Each module has an audio jack for every input, output and control signal, allowing you to automate and change the way in which the module operates. This allows you to hook up the modules in a large number of different ways! Diagram 2 shows one way in which 4 common modules can be linked together to produce music.

So what does the result sound like? I’ve attached an early video of my DIY modular synth below, showing a demo tune I put together to see if all of the modules were working. I have since added some extra modules to my synthesiser, which I will talk about in a later post. At the top of the box is an 8-step sequencer which is used to control the pitch and duration of each of the notes. On the left (blue) is a single oscillator which can produce three different types of waveforms (sinusoid, triangle, square), as demonstrated at the start of the video. Finally, next to this is an amplifier (grey) which can control the volume of each of these waveforms. It is interesting to hear all the different types of tones which this basic synthesiser can produce, without needing to introduce any filtering.

Up next in this Tutorial Series…

As I continue building and improving my own DIY synth, I hope to post several tutorials describing various electronic circuits commonly used to create music. Using a variety of diagrams and practical demonstrations, I will try to show how each synth module affects the audio signal. I will also try and model parts of the circuit mathematically, to illustrate the process and consideration made by the designers when developing the designs. Here is a list of the modules I have already built, and may discuss in my future tutorials:

• Dual +-12V Power Supply
• Voltage Controlled Oscillator (VCO)
• Voltage Controlled Amplifier (VCA)
• 8-step Sequencer
• Attack, Decay, Sustain and Release module (ADSR)
• Voltage Controlled Filter – Low Pass and High Pass (VCF)

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:

There are a multitude of micro-controllers available today, many of them advertised to be ‘Arduino Compatible’. Are these devices really worth considering, or should you just stick with genuine Arduinos? For the past few years I’ve had a chance to work with a good few of these devices, and I’ve discovered many advantages and disadvantages with each of them. Over the summer I got my hands on the Arduino 101; a relatively new Uno compatible micro-controller using Intel’s Curie module. Over the next couple of weeks I’ll be writing a number of tutorials about how to use some of Arduino 101’s features, but in this post I’ll look simply look at what it’s all about!

What makes Arduino 101 special?

What makes the Arduino 101 different than any of the other micro-controllers? Let’s look at a quick comparison between the Uno and the 101:

(Source – www.arduino.cc, Prices on 22nd Oct 2016 – www.sparkfun.com)

Overview

The two devices are physically identical; they both are the same size and have the same renowned ‘Uno’ header layout. The Arduino 101 definitely wins out in terms of features; it has more flash memory and better interrupt support. The only place where the 101 is lacking is the number of PWM pins.

Input/Output Pins & Interrupts

The Arduino 101 has the same number of Digital and Analog I/O pins as the Uno. While it has less PWM pins than the Uno, it more than makes up for it in terms of interrupt pins. Many people who are new to micro-controllers don’t know the importance of interrupts, but they are extremely useful. For example if you have three buttons connected to the controller (with each of them attached to an interrupt), you know that the program definitely will notice the button being pressed, no matter what it is doing.

A cool feature with the Arduino 101 is that it also supports a software timing interrupt. If you #include “CurieTimerOne.h” library, you can set up an interrupt which runs repeatedly at a set time interval. This is really useful if you want to process sensor data at a regular rate, without having to write your own time-keeping function.

Accelerometer, Gyroscope, and BLE

The best feature of the Arduino 101 is that it has integrated Gyroscope, Accelerometer, and Bluetooth Low Energy (BLE) modules! If you were to buy these features separately as breakouts for the Arduino Uno, it would definitely add another $30 to the total cost. Therefore if you have a project in mind which uses any of these features, I’d encourage you to get the 101!

The accelerometer and gyroscope sensors are not the most accurate or sensitive ones I have ever used, but with some calibration and data processing, they should be good enough to cover most electronics projects.

I’ve used the BLE to connect the Curie to an Android phone. It can be quite awkward to get the BLE to work properly (especially on the Android side), but it definitely is possible. BLE is good for low-power communication at short ranges and low transfer rates, but it is not suitable for transmitting a large amount of real-time data (trust me, I’ve tried!). The Arduino sketch supports a number of interrupts for each of the main BLE events, such as connecting/disconnecting to a host and receiving new data.

Neural Network

The Intel Curie module also has a Neural Network Pattern Matching Engine, which can be used on the Arduino 101. I’ve looked around online, and there is very little documentation about what it is and how to use it. I happen to be one of the few people who has used it, so I will definitely be writing a tutorial about it soon…

Advanced Features

The software support for the Curie module is still an ongoing process, and not all of the features have yet been catered for. The Intel Curie module itself is actually quite interesting. It has two processor cores; one of them is an Intel Quark processor core that hosts the USB and other system level duties, while the other is an ARC core that runs the user sketches. Advanced users can compile their own firmware for the Curie (based on Zephyr) which provides access to all of its features, low power states, and cores. The Open Developer Kit (ODK) is available on Intel’s software download page.

Getting Started with the Arduino 101

The Arduino 101 is fully compatible with the Arduino IDE, so that makes creating sketches and uploading them to the device very simple. To get started, simply follow these steps:

1. Download the Arduino IDE: https://www.arduino.cc/en/Main/Software
2. Unzip the application, and install it on your computer.
3. Once the IDE is open, you need to install the drivers for the 101. To do this navigate to: Tools->Board->Boards Manager
4. Scroll down in the window that appears until you find “Intel Curie Boards by Intel“. Click on it and install the latest version.
5. You should now be good to go; I’d recommend you try out the “Blink” example sketch to make sure that everything is working.
6. Some common issues are that you do not have the right board selected in the menu Tools->Boards, or that you don’t have the right comms port selected in Tools->Port.

Updated: 24th May 2019 – Reformatted post

Up until now we have looked at all of the individual topics behind self-balancing robots. In this final part of the tutorial, I’ll bring it all together and give you some guidelines to designing and assembling your own robot!

Designing the Robot

Weight Distribution: Self-balancing robots work on the principle of an inverted pendulum. This means that the system is most stable when all of the mass is positioned as high as possible. This seems to go against common sense; usually systems are more stable when they have a low centre of gravity. In this case keeping the mass on top increases the inertia of the system, meaning that the robot has more time to respond to changes in balance. Therefore my first recommendation is to place the heaviest objects, such as the battery, at the top of the robot.

Sensor Positioning: The positioning of the accelerometer/gyroscope module is also important. When I was demonstrating my balancing robot at the Dublin Maker Faire this year, I asked a number of people where they think the sensor should be positioned. Most guessed that it should be on top, as this is where it would record the largest amount of movement!

We actually want to avoid as much of this translational movement as possible, as we are only interested in the rotation of the robot. Therefore the sensor should be placed exactly on the axis of rotation, between both wheels. Placing the sensor further up on the frame introduces noise and jitter into the readings, and may cause a feedback loop (similar to the squeaking noise made when a microphone is too close to its own speaker).

Frame Design: For the rest of the frame, it is up to your own imagination what you want to do with it. I’ve included a couple of pictures and sketches below to help you come up with your own designs. Although I 3D printed two of my frames, I made my first prototype out of lollipop sticks (and it worked really well)!

Assembling the Frame

Putting together the frame and electronics is actually the easiest part of the project! Once you have your frame designed and ready to go, all you have to do is stick/screw all of the components together. Here is a schematic I made to help you with the wiring of the robot:

Combined Program

In the previous parts of the tutorial I included snippets of code to show you how each part of the self-balancing robot should work. Here I have compiled all of the parts together into one code that you can use and modify for your own robot. I included a horrendous amount of comments, so that the program is as easy to follow as possible!

Note: This code is programmed for the specific components I was using, such as an Arduino motor shield, and the MPU6050 Accel-Gyro module.

/* * * * * * * * * * * * * * * * * * * * * *
 * SELF-BALANCING ROBOT
 * =========================================
 *
 * Code by: Simon Bluett
 * Email: hello@chillibasket.com
 * Website: wired.chillibasket.com
 *
 * 7/10/15, Version 2.0
 *
 * Here are some hints when you try to use this code:
 *
 *  > Ensure pin-mapping is correct for your robot (line 54)
 *  > Ensure calibration values are correct for your sensor (line 181)
 *  > Uncomment (line 700) in order to see if your sensor is working
 *  > Play with your PID values on (line 93)
 *  > Ensure that your left & right motors aren't inverted (line 355)
 *  > Confirm whether you want the Pitch ypr[1] or Roll ypr[2] sensor readings!
 * * * * * * * * * * * * * * * * * * * * * */


/* * * * * * * * * * * * * * * * * * * * * *
 *  This Demo makes use of the I2Cdev and MPU6050 libraries, and the demonstration
 *  sketch written by (Jeff Rowberg <jeff@rowberg.net>), modified to work
 *  with the Intel Galileo Development Board:
 *  -- -- -- -- -- -- -- -- -- -- -- -- -- --
 *  I2Cdev device library code is placed under the MIT license
 *  Copyright (c) 2012 Jeff Rowberg
 *  Permission is hereby granted, free of charge, to any person obtaining a copy
 *  of this software and associated documentation files (the "Software"), to deal
 *  in the Software without restriction, including without limitation the rights
 *  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 *  copies of the Software, and to permit persons to whom the Software is
 *  furnished to do so, subject to the following conditions:
 *
 *  The above copyright notice and this permission notice shall be included in
 *  all copies or substantial portions of the Software.
 * * * * * * * * * * * * * * * * * * * * * */



// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include <I2Cdev.h>
#include <MPU6050_6Axis_MotionApps20.h>
#include <Wire.h>

// Specific I2C addresses may be passed as a parameter here
MPU6050 mpu;        			// Default: AD0 low = 0x68


// Define the pin-mapping
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
#define DIR_A 12                // Direction Pin, Motor A
#define DIR_B 13                // Direction Pin, Motor B
#define PWM_A 3                 // PWM, Motor A (Left Motor)
#define PWM_B 11                // PWM, Motor B (Right Motor)
#define BRK_A 9                 // Brake, Motor A
#define BRK_B 8                 // Brake, Motor B

#define BTN_1 10                 // On/Off Button
#define BTN_2 7                 // Set Centre of Gravity Button

#define LED_1 5                 // Low-battery Warning LED
#define LED_2 4                // Current mode LED


// Max PWM parameters
#define MAX_TURN 30


// MPU Control/Status
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
bool dmpReady = false;         	// Set true if DMP init was successful
uint8_t devStatus;              // Return status after device operation (0 = success, !0 = error)
uint8_t mpuIntStatus;           // Holds actual interrupt status byte from MPU
uint16_t packetSize;            // Expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;             // Count of all bytes currently in FIFO
uint8_t fifoBuffer[64];         // FIFO storage buffer


// Orientation/Motion
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
Quaternion q;                   // [w, x, y, z]       Quaternion Container
VectorFloat gravity;           	// [x, y, z]            Gravity Vector
int16_t gyro[3];               	// [x, y, z]            Gyro Vector
float ypr[3];                   // [yaw, pitch, roll]   Yaw/Pitch/Roll & gravity vector
float averagepitch[50];        	// Used for averaging pitch value


// For PID Controller
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
float Kp = 8;                   // (P)roportional Tuning Parameter
float Ki = 2;					// (I)ntegral Tuning Parameter        
float Kd = 5;					// (D)erivative Tuning Parameter       
float lastpitch;                // Keeps track of error over time
float iTerm;              		// Used to accumulate error (integral)
float targetAngle = 2.1;       	// Can be adjusted according to centre of gravity 

// You can Turn off YAW control, by setting
// the Tp and Td constants below to 0.
float Tp = 0.6;        			// Yaw Proportional Tuning Parameter
float Td = 0.1;					// Yaw Derivative Tuning Parameter
float targetYaw = 0;            // Used to maintain the robot's yaw
float lastYawError = 0;

float PIDGain = 0;				// Used for soft start (prevent jerking at initiation)


// Motor Control
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
int direction_A = 0;            // 0 - Forwards, 1 - Backwards
int direction_B = 0;            //
int brake_A = 1;                // 1 - On, 0 - Off
int brake_B = 1;                //


// Runtime variables
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
int modeSelect = 1;             // System Mode (0 = off, 1 = normal, 2 = guided)
bool initialised = true;        // Is the balancing system on?

char inchar = 0;                // Hold any incoming characters
float angular_rate = 0;         // Used to make sure rate is ~0 when balance mode is initiated

bool newCalibration = false;	// If set TRUE, the target angles are recalibrated


// Variables used for timing control
// Aim is 10ms per cycle (100Hz)
// -- -- -- -- -- -- -- -- -- -- -- -- -- --
#define STD_LOOP_TIME 9

unsigned long loopStartTime = 0;
unsigned long lastTime;             // Time since PID was called last (should be ~10ms)

// 0 = Off, 1 = On
int modes = 0;



// ------------------------------------------------------------------
// 					      INITIAL SETUP
// ------------------------------------------------------------------

void setup() {

    Wire.begin();

    // Initialize serial communication for debugging
    Serial.begin(115200);
	
	 // Configure LED for output
    pinMode(LED_1, OUTPUT);
    pinMode(LED_2, OUTPUT);

    digitalWrite(LED_1, LOW);
    digitalWrite(LED_2, LOW);

    // Set as input, internal pullup for buttons
    pinMode(BTN_1, INPUT_PULLUP);
    pinMode(BTN_2, INPUT_PULLUP);

    // Configure Motor I/O
    pinMode(DIR_A, OUTPUT);     // Left Motor Direction
    pinMode(DIR_B, OUTPUT);     // Right Motor Direction
    pinMode(BRK_A, OUTPUT);     // Left Motor Brake
    pinMode(BRK_B, OUTPUT);     // Right Motor Brake

    // Initialize MPU6050
    mpu.initialize();
    Serial.println("Testing MPU connection:");

    Serial.println(mpu.testConnection() ? "> MPU6050 connection successful" : "> MPU6050 connection failed");
    Serial.println("Initialising DMP");
    devStatus = mpu.dmpInitialize();

    /* * * * * * * * * * * * * * * * * * * *
     * IMPORTANT!
     * Supply your own MPU6050 offsets here
     * Otherwise robot will not balance properly.
     * * * * * * * * * * * * * * * * * * * */
    mpu.setXGyroOffset(93);
    mpu.setYGyroOffset(-15);
    mpu.setZGyroOffset(30);
    mpu.setXAccelOffset(-2500);
    mpu.setYAccelOffset(1783);
    mpu.setZAccelOffset(877);

    // Make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        Serial.println("Enabling DMP");
        mpu.setDMPEnabled(true);
        mpuIntStatus = mpu.getIntStatus();

        // Set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println("DMP Ready! Let's Proceed.");
        Serial.println("Robot is now ready to balance. Hold the robot steady");
        Serial.println("in a vertical position, and the motors should start.");
        dmpReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();

    } else {
		// In case of an error with the DMP
        if(devStatus == 1) Serial.println("> Initial Memory Load Failed");
        else if (devStatus == 2) Serial.println("> DMP Configuration Updates Failed");
    }

}



// -------------------------------------------------------------------
// 			 PID CONTROLLER
// -------------------------------------------------------------------

int PID(float pitch) {            

    // Calculate time since last time PID was called (~10ms)
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
    unsigned long thisTime = millis();
    float timeChange = float(thisTime - lastTime);

    // Calculate Error
    float error = targetAngle - pitch;


    // Calculate our PID terms
    // PID values are multiplied/divided by 10 in order to allow the
    // constants to be numbers between 0-10.
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
    float pTerm = Kp * error * 10;
    iTerm += Ki * error * timeChange / 10;  
    float dTerm = Kd * (pitch - lastpitch) / timeChange * 100; 
	
	if (Ki == 0) iTerm = 0;
    lastpitch = pitch;
    lastTime = thisTime;


    // Obtain PID output value
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
    float PIDValue = pTerm + iTerm - dTerm;

    // Set a minimum speed (motors will not move below this - can help to reduce latency)
    //if(PIDValue > 0) PIDValue = PIDValue + 10;
    //if(PIDValue < 0) PIDValue = PIDValue - 10;

	// Limit PID value to maximum PWM values
    if (PIDValue > 255) PIDValue = 255;
    else if (PIDValue < -255) PIDValue = -255; 

    return int(PIDValue);

}



// -------------------------------------------------------------------
// 			 YAW CONTROLLER
// -------------------------------------------------------------------

int yawPD(int yawError) {            


    // Calculate our PD terms
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
    float pTerm = Tp * yawError;
    float dTerm = Td * (yawError - lastYawError) / 10; 
	
    lastYawError = yawError;

    // Obtain PD output value
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
    int yawPDvalue = int(-pTerm + dTerm);

	// Limit PD value to maximum
    if (yawPDvalue > MAX_TURN) yawPDvalue = MAX_TURN;
    else if (yawPDvalue < -MAX_TURN) yawPDvalue = -MAX_TURN; 

    //Serial.print("Error: ");
    //Serial.print(yawError);
    //Serial.print(" - PD: ");
    //Serial.println(yawPDvalue);
    return yawPDvalue;

}



// -------------------------------------------------------------------
// 			 	MOVEMENT CONTROLLER
// -------------------------------------------------------------------
// This function calculate the PWM output required to keep the robot 
// balanced, to move it back and forth, and to control the yaw.

void MoveControl(int pidValue, float yaw){
	
    // Set both motors to this speed
    int left_PWM = pidValue;
    int right_PWM = pidValue;


    /* YAW CONTROLLER */

    // Check if turning left or right is faster
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
    int leftTurn, rightTurn;

    float newYaw = targetYaw;

    if((yaw > 0) && (newYaw < 0)){
        rightTurn = yaw + abs(newYaw);
        leftTurn = (180 - yaw) + (180 - abs(newYaw));

    } else if ((yaw < 0) && (newYaw > 0)){
        rightTurn = (180 - abs(yaw)) + (180 - newYaw);
        leftTurn = abs(yaw) + newYaw;

    } else if (((yaw > 0) && (newYaw > 0)) || ((yaw < 0) && (newYaw < 0))){
        rightTurn = newYaw - yaw;

        if (rightTurn > 0){
            leftTurn = rightTurn;
            rightTurn = 360 - leftTurn;
        } else if (rightTurn < 0){
            rightTurn = abs(rightTurn);
            leftTurn = 360 - abs(rightTurn);
        } else if (rightTurn == 0){
            rightTurn = leftTurn = 0;
        }
    }

    // Apply yaw PD controller to motor output
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
    if ((leftTurn == 0) && (rightTurn == 0)){
        // Do nothing
    } else if (leftTurn <= rightTurn){
    	leftTurn = yawPD(leftTurn);
        left_PWM = left_PWM - leftTurn;
        right_PWM = right_PWM + leftTurn;

    } else if (rightTurn < leftTurn){
        rightTurn = yawPD(rightTurn);
        left_PWM = left_PWM + rightTurn;
        right_PWM = right_PWM - rightTurn;
        
    }


    // Limits PID to max motor speed
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
    if (left_PWM > 255) left_PWM = 255;
    else if (left_PWM < -255) left_PWM = -255; 
    if (right_PWM > 255) right_PWM = 255;
    else if (right_PWM < -255) right_PWM = -255; 

    // Send command to left motor
    if (left_PWM >= 0) Move(0, 0, int(left_PWM));   	// '0' = Left-motor, '1' = Right-motor
    else Move(0, 1, (int(left_PWM) * -1));
	// Send command to right motor
    if (right_PWM >= 0) Move(1, 1, int(right_PWM)); 	// '0' = Forward, '1' = Backward
    else Move(1, 0, (int(right_PWM) * -1));    

}



// -------------------------------------------------------------------
// 			 MOTOR CONTROLLER
// -------------------------------------------------------------------

void Move(int motor, int direction, int speed) {            

	// Left Motor
	// -- -- -- -- -- -- -- -- -- -- -- -- -- --
	if (motor == 0){
	
		// Set motor direction (only if it is currently not that direction)
		if (direction == 0){
            if (direction_A == 1) digitalWrite(DIR_A, HIGH);		// Forwards
			direction_A = 0;
		} else {
			if (direction_A == 0)  digitalWrite(DIR_A, LOW);		// Backwards
			direction_A = 1;
		}
        
		// Release brake (only if brake is active)
		if (brake_A == 1){
			digitalWrite(BRK_A, LOW);
			brake_A = 0;
		}
		
		// Send PWM data to motor A
		analogWrite(PWM_A, speed);


    // Right Motor
	// -- -- -- -- -- -- -- -- -- -- -- -- -- --
    } else if (motor == 1){
	
		// Set motor direction (only if it is currently not that direction)
		if (direction == 0){
			if (direction_B == 1) digitalWrite(DIR_B, HIGH);		// Forwards
			direction_B = 0;
		} else {
			if (direction_B == 0)  digitalWrite(DIR_B, LOW);		// Backwards
			direction_B = 1;
		}
        
		// Release brake (only if brake is active)
		if (brake_B == 1){
			digitalWrite(BRK_B, LOW);
			brake_B = 0;
		}
		
		// Send PWM data to motor A
		analogWrite(PWM_B, speed);


    // Stop both motors
	// -- -- -- -- -- -- -- -- -- -- -- -- -- --
    } else if (motor = 3){  

        analogWrite(PWM_A, 0);
        analogWrite(PWM_B, 0);
        digitalWrite(BRK_A, HIGH);
        digitalWrite(BRK_B, HIGH);
        brake_A = 1;
        brake_B = 1;

    }
}



// -------------------------------------------------------------------
// 			 READ INPUT FROM SERIAL
// -------------------------------------------------------------------

void readSerial() {

    // Initiate all of the variables
    // -- -- -- -- -- -- -- -- -- -- -- -- -- --
	int changestate = 0;		// Which action needs to be taken?
	int no_before = 0;			// Numbers before decimal point
	int no_after = 0;			// Numbers after decimal point
	bool minus = false;			// See if number is negative
	inchar = Serial.read();		// Read incoming data

    if (inchar == 'P') changestate = 1;
    else if (inchar == 'I') changestate = 2;
    else if (inchar == 'D') changestate = 3;

    // Tell robot to calibrate the Centre of Gravity
    else if (inchar == 'G') calibrateTargets();


    // Records all of the incoming data (format: 00.000)
    // And converts the chars into a float number
    if (changestate > 0){
        if (Serial.available() > 0){

            // Is the number negative?
            inchar = Serial.read();
            if(inchar == '-'){
                minus = true;
                inchar = Serial.read();
            }
            no_before = inchar - '0';

            if (Serial.available() > 0){
                inchar = Serial.read();

                if (inchar != '.'){
                    no_before = (no_before * 10) + (inchar - '0');

                    if (Serial.available() > 0){
                        inchar = Serial.read();
                    }
                }

                if (inchar == '.'){
                    inchar = Serial.read();
                    if (inchar != '0'){
                        no_after = (inchar - '0') * 100;
                    }

                    if (Serial.available() > 0){
                        inchar = Serial.read();
                        if (inchar != '0'){
                            no_after = no_after + ((inchar - '0') * 10);
                        }

                        if (Serial.available() > 0){
                            inchar = Serial.read();
                            if (inchar != '0'){
                                no_after = no_after + (inchar - '0');
                            }
                        }
                    }
                }
            }

            // Combine the chars into a single float
            float answer = float(no_after) / 1000;
            answer = answer + no_before;
            if (minus) answer = answer * -1;

            // Update the PID constants
            if (changestate == 1){
                Kp = answer;
                Serial.print("P - ");
            } else if (changestate == 2){
                Ki = answer;
                Serial.print("I - ");
            } else if (changestate == 3){ 
                Kd = answer;
                Serial.print("D - ");
            }
            Serial.print("Constant Set: ");
            Serial.println(answer, 3);

        } else {
            changestate = 0;
        }
    }
}



// -------------------------------------------------------------------
// 			 RECALIBRATE TARGET VALUES
// -------------------------------------------------------------------
// Takes a number of readings and gets new values for the target angles.
// Robot must be held upright while this process is being completed.

void calibrateTargets(){

	targetAngle = 0;
	targetYaw = 0;
	
    for(int calibrator = 0; calibrator < 50; calibrator++){
	
		accelgyroData();
		targetAngle += pitch();
		targetYaw += yaw();
		delay(10);
	}
	
	// Set our new value for Pitch and Yaw
	targetAngle = targetAngle / 50;
	targetYaw = targetYaw / 50;
	Serial.print("Target Pitch: ");
	Serial.print(targetAngle, 3);
	Serial.print(", Target Yaw: ");
	Serial.print(targetYaw, 3);

	newCalibration = false;
}



// -------------------------------------------------------------------
// 			 GET PITCH AND YAW VALUES
// -------------------------------------------------------------------
// This simply converts the values from the accel-gyro arrays into degrees.

float pitch(){
	return (ypr[1] * 180/M_PI);
}

float yaw(){
	return (ypr[0] * 180/M_PI);
}

float angRate(){
	return -((float)gyro[1]/131.0);
}



// -------------------------------------------------------------------
// 			 GET ACCEL_GYRO DATA
// -------------------------------------------------------------------

void accelgyroData(){

    // Reset interrupt flag and get INT_STATUS byte
    mpuIntStatus = mpu.getIntStatus();

    // Get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // Check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // Reset so we can continue cleanly
        mpu.resetFIFO();
        Serial.println("Warning - FIFO Overflowing!");

    // otherwise, check for DMP data ready interrupt (this should happen exactly once per loop: 100Hz)
    } else if (mpuIntStatus & 0x02) {
        // Wait for correct available data length, should be less than 1-2ms, if any!
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();


        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;

        // Get sensor data
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGyro(gyro, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
        mpu.resetFIFO();

        //Serial.print(ypr[1]);
        //Serial.print(" - ");
        //Serial.println(ypr[0]);
    }
}



// -------------------------------------------------------------------
// 			 MAIN PROGRAM LOOP
// -------------------------------------------------------------------

void loop() {

	// If the "SET" button is pressed
	// -- -- -- -- -- -- -- -- -- -- -- -- -- --
	if (digitalRead(BTN_2) == LOW){

		digitalWrite(LED_1, HIGH);
	    calibrateTargets();

	    lastpitch = 0;
	    iTerm = 0;

	    Serial.println("> Setting new centre of gravity <");

		delay(250);
	    mpu.resetFIFO();
	    digitalWrite(LED_1, LOW);
	}


	// If the "POWER" button is pressed
	// -- -- -- -- -- -- -- -- -- -- -- -- -- --
	if (digitalRead(BTN_1) == LOW){
	    if (modeSelect == 1){
	        Serial.println("> Turning off balancing system <");
	        initialised = false;
	        modeSelect = 0;
	        Move(3,0,0);        // Stop both motors from moving
	        digitalWrite(LED_2, LOW);
	    } else if (modeSelect == 0){
	        Serial.println("> Turning on balancing system <");
	        initialised = false;
	        modeSelect = 1;
	        digitalWrite(LED_2, HIGH);
	    }
	    delay(500);
	    mpu.resetFIFO();
	}
		
	// Gather data from MPU6050
	accelgyroData();
		
	// If the Balance System is turned on:
	if (modeSelect == 1){
				
		if (!initialised){

	        // Wait until robot is vertical and angular rate is almost zero:
	        if ((pitch() < targetAngle+0.1) && (pitch() > targetAngle-0.1) && (abs(angRate()) < 0.3)){
	            Serial.println(">>>> Balancing System Active <<<<");
	            initialised = true;
	            lastpitch = pitch();
	            iTerm = 0;
	        }
	
	    // Otherwise, run the PID controller
		} else {

			// Stop the system if it has fallen over:
			if ((pitch() < -45) || (pitch() > 45)){
					
				// Stop the motors
				Move(3, 0, 0);
				// Reset runtime variables
				lastpitch = 0;
				iTerm = 0;
				initialised = false;
				Serial.println(">>>> Balancing System Stopped <<<<");

			} else {
				// A bit of function-ception happening here:
				//Serial.println(pitch());
				MoveControl(PID(pitch()), yaw());
			}
		}
	}

    if (Serial.available() > 0){    // If new PID values are being sent by the interface
        readSerial();               // Run the read serial method
    }

    // Call the timing function
    // Very important to keep the response time consistent!
    timekeeper();
}



// -------------------------------------------------------------------
// 			 	TIME KEEPER
// -------------------------------------------------------------------

void timekeeper() {

    // Calculate time since loop began
    float timeChange = millis() - loopStartTime;

    // If the required loop time has not been reached, please wait!

    if (timeChange < STD_LOOP_TIME) {
        delay(STD_LOOP_TIME - timeChange);
    } 


    // Update loop timer variables
    loopStartTime = millis();   
}

Here is a breakdown of how to use this code with your robot:

1. Before starting the program, connect the board via USB to your computer, and open a terminal window in the Arduino software (baud rate: 115200).
2. Wait until robot is ready: At the start the robot automatically initialises the MPU6050 module. Once this is done, the following message should appear:
   DMP Ready! Let's Proceed.
3. Set Centre of Gravity: You should set the centre of gravity of the robot, so that the robot knows which way is up! Do this by steadily holding the robot upright, with the wheels off the floor, and pressing the button connected to GPIO-4. The LED on GPIO-2 will flash, and the following message should appear:
   > Setting new centre of gravity <
4. Automatic On/Off: The motors of the robot automatically turn off if the robot has fallen over, or is lying on its side. To turn them back on, hold the robot steadily in an upright position. The motors should start and the following message appear:
   >>>> Balancing System Active <<<<
   If the robot has fallen over and motors are off, this message appears:
   >>>> Balancing System Stopped <<<<
5. Manual On/Off: To manually turn the balancing system on/off, press the button connected to GPIO-7. The LED on GPIO-10 will be bright, if the balancing system is turned on. One of the following messages will appear to let you know which state the robot is in:
   > Turning off balancing system <
> Turning on balancing system <
6. Sending new PID values: Please read my guide The PID Controller, to see how to calibrate your robot. You can send new PID values via the console window, by typing the letter of the constant (P, I or D) you want to set, followed by the number you want to set it at. Then press the enter/return key to send. The code accepts any numbers between 0.01 – 99.99. For example:
   P8.2 I1.51 D15
   This sets the [P]roportional Constant to 8.2, the [I]ntegral Constant to 1.51, and the [D]erivative Constant to 15.

Dealing with Common Errors

I have found that most of the common errors can be dealt with by checking the following:

1. Check the Pin Mapping: Make sure that the GPIO number on (lines 54-65) match up with the ones you are using on your robot.
2. Update the MPU-6050 Offsets: Each sensor has unique offset values, which have to be inputted on (lines 183-188). I explain how to find these offsets in my “Calibrating & Optimising the MPU6050” part of this tutorial.
3. Ensure sensor is working properly: Check that the MPU-6050 is working, by uncommenting (line 704) of my code. While running, the robot should display the current angle on the console. When held upright, the angle should be 0. When pitching forwards/backwards, the number should be positive/negative in degrees.
4. Both motors should spin in same direction: Set the turning constants on (lines 102-103) to zero. Now both motors should spin in same direction. If not, then one of the motors is wired backwards.
5. Motors balance in wrong direction: Instead of stopping the robot from falling, the motors speed up the fall. This means that both motors are wired in backwards!

This finally concludes my tutorial about self-balancing robots! If you have any questions or suggestions, please leave a comment below.

Updated: 23rd May 2019 – Reformatted post

The most important element of any robot is the controller. The control algorithm determines how the robot should react to different sensor inputs, allowing it to intelligently adapt to and interact with its environment. Especially for a self-balancing robot, the control program is vital as it interprets the motion sensor data and decides how much the motors need to be moved in order for the robot to remain stable and upright. The most common controller used for stabilisation systems is the PID controller. So let’s look at how it works…

The PID Controller

PID stands for Proportional, Integral and Derivative, referring to the mathematical equations used to calculate the output of the controller. This is perhaps the most common type of controller used in industry, as it is able to control relatively complex systems even though the calculations are actually quite straightforward (making it easy to program and fast to compute). Mathematically, the PID controller can be described by the following formula:

u(t)=K_pe(t) + K_i\int_{0}^{t}e(t)dt +K_d\frac{de(t)}{dt}

In this formula, the output of the controller u(t) is determined by the sum of three different elements, each dependent on the error e(t). The error simply is the difference between the target value and the current value (measured by the sensor). Now let’s quickly look at what each of the terms in the equation is doing for our robot…

K_pe(t)

This is the proportional component, which takes the current error value and multiplies it by a constant number (Kp). For our self-balancing robot, this simply takes in the current angle of the robot and makes the motors move in the same direction as the robot is falling. The further the robot falls off target, the faster the motors move. If the P-component is used on its own, the robot might stabilise for a while, but the system will tend to overshoot, oscillate and ultimately fall over.

K_i\int_0^te(t)dt

The integral component is used to accumulate any errors over time, and multiplies this accumulated value by a constant number (Ki). For example if the robot tends to fall over to one side, it knows that it needs to move in the opposite direction in order to keep on target and to prevent drifting left or right.

K_d\frac{de(t)}{dt}

Finally, the derivative component is responsible for dampening any oscillations and ensures that the robot does not overshoot the target value. Each time the controller is called, this term calculates the change in the error value and multiplies it by a constant number (Kd). Often this is simplified to calculate only the change in the current sensor value, rather than the change in error. If the target position remains constant, this gives the same result. This simplification helps to prevent sudden jumps in the output value which happen when the target position it changed.

Summing all three of these terms together then gives us an output value which we can send to the motor of the self-balancing robot. However, before the controller can successfully balance the robot, the three constants (Kp), (Ki) and (Kd) need to be tuned to suit our specific application. By increasing or decreasing the values of these constants, we control how much each of the three components of the controller contributes to the output of the system.

The formula I used above is called the “Independent” PID equation. If you read up on the PID controller online, you may also see the formula for the algorithm expressed in a different format (known as the “Dependent” PID equation):

u(t) = K_ce(t)+\frac{K_c}{T_i}\int_0^te(t)dt + K_cT_d\frac{de(t)}{dt}

The term (Kc) is known as controller gain, (Ti) the integral time and (Td) the derivative time. This format can be useful when using some automated tuning techniques, however the final result is the same.

Implementing the Controller

Now that we know the basic theory, we can start to write this in code. The basic layout of the function is shown below. The controller can be used by calling the “pid” function at regular intervals, providing the target position and the current position (as recorded by the sensor) as parameters of the function. The function then outputs the calculated result of the controller. Tuning of the controller can be done by changing the values of the (Kp), (Ki) and (Kd) variables at the top of the code.

// Declare variables
float Kp = 7;          // (P)roportional Tuning Parameter
float Ki = 6;          // (I)ntegral Tuning Parameter        
float Kd = 3;          // (D)erivative Tuning Parameter       
float iTerm = 0;       // Used to accumulate error (integral)
float lastTime = 0;    // Records the time the function was last called
float maxPID = 255;    // The maximum value that can be output
float oldValue = 0;    // The last sensor value

/**
 * PID Controller
 * @param  (target)  The target position/value we are aiming for
 * @param  (current) The current value, as recorded by the sensor
 * @return The output of the controller
 */
float pid(float target, float current) {
	// Calculate the time since function was last called
	float thisTime = millis();
	float dT = thisTime - lastTime;
	lastTime = thisTime;

	// Calculate error between target and current values
	float error = target - current;

	// Calculate the integral term
	iTerm += error * dT; 

	// Calculate the derivative term (using the simplification)
	float dTerm = (oldValue - current) / dT;

	// Set old variable to equal new ones
	oldValue = current;

	// Multiply each term by its constant, and add it all up
	float result = (error * Kp) + (iTerm * Ki) + (dTerm * Kd);

	// Limit PID value to maximum values
	if (result > maxPID) result = maxPID;
	else if (result < -maxPID) result = -maxPID;

	return result;
}

What does this program do?

First of all, the algorithm calculates the time since the last loop was called, using the “millis()” function. The error is then calculated; this is the difference between the current value (the angle recorded by the sensor), and the target value (the angle of 0 degrees we are aiming to reach).

The PID values are then calculated and summed up to give an output for the motors. The output is then constrained to ±255 as this is the maximum PWM value that can be output to the motors of the self-balancing robot.

Although this program is almost complete, I found that my robot only worked well once I included a timing function. This is a system that ensures the PID controller function is called at regular intervals. In my self-balancing robot, I set the loop time to be 10ms (meaning the function is run 100 times per second). Here is the timer code and a sample loop function:

// Any variables for the PID controller go here!
float targetValue = 0;

// Variables for Time Keeper function:
#define LOOP_TIME 10          // Time in ms (10ms = 100Hz)
unsigned long timerValue = 0;

/******** SETUP ************/
void setup() {
	// Put all of your setup code here
	timerValue = millis();
}

/******* MAIN LOOP *********/
void loop() {
	// Only run the controller once the time interval has passed
	if (millis() - timerValue > LOOP_TIME) {
		timerValue = millis();

		// Replace getAngle() with your sensor data reading
		float currentValue = getAngle();

		// Run the PID controller
		float motorOutput = pid(targetValue, currentValue);

		// Replace moveMotors() with your desired output
		moveMotors(motorOutput);
	}
}

/****** PID CONTROLLER *****/
float pid(float target, float current) {          

	// PID code from above goes in here!
	return result;
}

Unfortunately this is not the end of the story! Although the PID controller code is complete, suitable PID constants still need to be found to tune the controller for your specific robot. These constants depend on things such as weight, motor speed and the shape of the robot, and therefore they can vary significantly from robot to robot. Here is a quick explanation of how you should go about calibrating your PID values:

Calibrating your PID Controller

1. Create some way in which you can change the PID constant of your robot while it is running. One option is to use a potentiometer or some other analogue input to be able to increase or decrease the PID constant. I personally used the USB connection and the serial monitor to send new PID values. This is important as you can then see straight away how well the new PID values are working, and you won’t have to re-upload the code hundreds of times!
2. Set all PID constants to zero. This is as good a place to start as any…
3. Slowly increase the P-constant value. While you are doing this, hold the robot to make sure it doesn’t fall over and smash into a million pieces! You should increase the P-constant until the robot responds quickly to any tilting, and then just makes the robot overshoot in the other direction.
4. Now increase the I-constant. This component is a bit tricky to get right. You should keep this relatively low, as it can accumulate errors very quickly. In theory, the robot should be able to stabilise with only the P and I constants set, but will oscillate a lot and ultimately fall over.
5. Raise the D-constant. A lot. The derivative components works against any motion, so it helps to dampen any oscillations and reduce overshooting. I found that this constant has to be set significantly higher than the other two (x10 to x100 more) in order to have any effect. All the same, don’t set it too high, as it will reduce the robot’s ability to react to external forces (aka. being pushed around).
6. Spend many fruitless hours slightly modifying the PID values. This is probably the longest part of the procedure, as there isn’t much of a method to it. You just have to increase and decrease the values until you reach that perfect sweet-spot for your robot!

Please leave a comment below if you have any questions or suggestions.

Updated: 23rd May 2019 – Reformatted post and improved the code snippets.

Updated: 1st June 2020 – Add PID equations and improved the descriptions.

In this part of the tutorial I will cover how to get the most performance out of the MPU-6050 Accelerometer and Gyroscope module, using the “Motion Apps” library. This library is really amazing as the author reverse engineered how to use the Digital Motion Processor (DMP) integrated within the MPU-6050. This allows all the complicated sensor processing and fusion to be done using the DMP instead of by the micro-controller! I have found that reading the sensor through the Motion Apps Library is faster than doing the calculations manually, and the readings tend to be significantly more accurate.

1. Analysing the Output

Here is the code that I used to get the yaw, pitch and roll sensor data. It is based on the “Teapot” example sketch which comes with the MPU-6050 Motion Apps library. To use this sketch, you will need to have the “MPU6050” and the “I2Cdev” libraries installed.

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class 
// using DMP (MotionApps v2.0)
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>

/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2012 Jeff Rowberg

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    #include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

/* =========================================================================
   NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
   depends on the MPU-6050's INT pin being connected to the Arduino's
   external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
   digital I/O pin 2.

   For the Galileo Gen1/2 Boards, there is no INT pin support. Therefore
   the INT pin does not need to be connected, but you should work on getting
   the timing of the program right, so that there is no buffer overflow.
 * ========================================================================= */

/* =========================================================================
   NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
   when using Serial.write(buf, len). The Teapot output uses this method.
   The solution requires a modification to the Arduino USBAPI.h file, which
   is fortunately simple, but annoying. This will be fixed in the next IDE
   release. For more info, see these links:

   http://arduino.cc/forum/index.php/topic,109987.0.html
   http://code.google.com/p/arduino/issues/detail?id=958
 * ========================================================================= */


#define OUTPUT_READABLE_YAWPITCHROLL

// Unccomment if you are using an Arduino-Style Board
// #define ARDUINO_BOARD

// Uncomment if you are using a Galileo Gen1 / 2 Board
#define GALILEO_BOARD

#define LED_PIN 13      // (Galileo/Arduino is 13)
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
VectorFloat gravity;    // [x, y, z]            gravity vector
Quaternion q;           // [w, x, y, z]         quaternion container
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector



// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================

// This function is not required when using the Galileo 
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
    mpuInterrupt = true;
}



// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif

    Serial.begin(115200);
    while (!Serial);

    // initialize device
    Serial.println(F("Initializing I2C devices..."));
    mpu.initialize();

    // verify connection
    Serial.println(F("Testing device connections..."));
    Serial.println(F("MPU6050 connection "));
    Serial.print(mpu.testConnection() ? F("successful") : F("failed"));

    // wait for ready
    Serial.println(F("\nSend any character to begin DMP programming and demo: "));
    while (Serial.available() && Serial.read()); // empty buffer
    while (!Serial.available());                 // wait for data
    while (Serial.available() && Serial.read()); // empty buffer again

    // load and configure the DMP
    Serial.println(F("Initializing DMP..."));
    devStatus = mpu.dmpInitialize();

    // supply your own gyro offsets here, scaled for min sensitivity
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

    // make sure it worked (returns 0 if so)
    if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);

        // enable Arduino interrupt detection
        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        Serial.println(F("DMP ready! Waiting for first interrupt..."));
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }

    // configure LED for output
    pinMode(LED_PIN, OUTPUT);
}



// ================================================================
// ===                    MAIN PROGRAM LOOP                     ===
// ================================================================

void loop() {
    // if programming failed, don't try to do anything
    if (!dmpReady) return;

    // wait for MPU interrupt or extra packet(s) available

    #ifdef ARDUINO_BOARD
        while (!mpuInterrupt && fifoCount < packetSize) {
        }
    #endif

    #ifdef GALILEO_BOARD
        delay(10);
    #endif

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        // reset so we can continue cleanly
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
    } else if (mpuIntStatus & 0x02) {
        // wait for correct available data length, should be a VERY short wait
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

        // read a packet from FIFO
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;


        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif

        // blink LED to indicate activity
        blinkState = !blinkState;
        digitalWrite(LED_PIN, blinkState);
    }
}

After completing several trials, I noticed that the accelerometer-gyroscope module and the Motion Apps library seem to have some type of auto-calibration feature, which requires a couple of seconds to complete. Here is a graph of the orientation output I got directly after initiating the sensor:

There is a large variation in the values as the sensor starts up, especially in the yaw data. This sensor drift stops after around 13 seconds, probably due to the completion of an integrated auto-calibration process. I repeated this test a number of times and it seems that the sensor can on some occasions take up to 40 seconds to complete its calibration. Therefore we should take this delay into account in our program… If we want to use the yaw data, the robot should wait for around 40 seconds before beginning to use the sensor and starting the main program. If only the pitch and roll data is used in a non-critical control system (no quadcopters!), we probably could get away with not waiting for the sensor to settle.

2. Calibrating the Sensor

Just like most sensors, the MPU-6050 needs to be calibrated before it is used for the first time. What we want to do is remove the zero-error; this refers to when the sensor records a small angle even though it is totally level. This error can be removed by applying an offset to the raw accelerometer and gyroscope sensor readings. The offset needs to be adjusted until the gyroscope readings are zero (no rotation) and the accelerometer records the acceleration due to gravity pointing directly downwards. Fortunately I found a program that can calibrate the MPU-6050 for us! The original calibration sketch can be found on the I2Cdev library forum. To use the program, first make sure that the MPU-6050 is correctly wired up to the Arduino (or equivalent). Then upload the sketch and open up the serial monitor in the Arduino IDE, setting the baud rate to 115200. To start calibration, place the accel-gyro module in a flat and level position and send any character in the serial monitor. The program will make an average of a few hundred readings and display the offsets required to remove zero error.

// Arduino sketch that returns calibration offsets for MPU6050 
//   Version 1.1  (31th January 2014)
// Done by Luis Ródenas <luisrodenaslorda@gmail.com>
// Based on the I2Cdev library and previous work by Jeff Rowberg <jeff@rowberg.net>
// Updates (of the library) should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib

// These offsets were meant to calibrate MPU6050's internal DMP, but can be also useful for reading sensors. 
// The effect of temperature has not been taken into account so I can't promise that it will work if you 
// calibrate indoors and then use it outdoors. Best is to calibrate and use at the same room temperature.

/* ==========  LICENSE  ==================================
 I2Cdev device library code is placed under the MIT license
 Copyright (c) 2011 Jeff Rowberg
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
 in the Software without restriction, including without limitation the rights
 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 copies of the Software, and to permit persons to whom the Software is
 furnished to do so, subject to the following conditions:
 
 The above copyright notice and this permission notice shall be included in
 all copies or substantial portions of the Software.
 
 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 THE SOFTWARE.
 =========================================================
 */

// I2Cdev and MPU6050 must be installed as libraries
#include "I2Cdev.h"
#include "MPU6050.h"
#include "Wire.h"

///////////////////////////////////   CONFIGURATION   /////////////////////////////
//Change this 3 variables if you want to fine tune the skecth to your needs.
int buffersize=1000;     //Amount of readings used to average, make it higher to get more precision but sketch will be slower  (default:1000)
int acel_deadzone=8;     //Acelerometer error allowed, make it lower to get more precision, but sketch may not converge  (default:8)
int giro_deadzone=1;     //Giro error allowed, make it lower to get more precision, but sketch may not converge  (default:1)

// default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
//MPU6050 accelgyro;
MPU6050 accelgyro(0x68); // <-- use for AD0 high

int16_t ax, ay, az,gx, gy, gz;

int mean_ax,mean_ay,mean_az,mean_gx,mean_gy,mean_gz,state=0;
int ax_offset,ay_offset,az_offset,gx_offset,gy_offset,gz_offset;

///////////////////////////////////   SETUP   ////////////////////////////////////
void setup() {
  // join I2C bus (I2Cdev library doesn't do this automatically)
  Wire.begin();

  // initialize serial communication
  Serial.begin(115200);

  // initialize device
  accelgyro.initialize();

  // wait for ready
  while (Serial.available() && Serial.read()); // empty buffer
  while (!Serial.available()){
    Serial.println(F("Send any character to start sketch.\n"));
    delay(1500);
  }                
  while (Serial.available() && Serial.read()); // empty buffer again

  // start message
  Serial.println("\nMPU6050 Calibration Sketch");
  delay(2000);
  Serial.println("\nYour MPU6050 should be placed in horizontal position, with package letters facing up. \nDon't touch it until you see a finish message.\n");
  delay(3000);
  // verify connection
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  delay(1000);
  // reset offsets
  accelgyro.setXAccelOffset(0);
  accelgyro.setYAccelOffset(0);
  accelgyro.setZAccelOffset(0);
  accelgyro.setXGyroOffset(0);
  accelgyro.setYGyroOffset(0);
  accelgyro.setZGyroOffset(0);
}

///////////////////////////////////   LOOP   ////////////////////////////////////
void loop() {
  if (state==0){
    Serial.println("\nReading sensors for first time...");
    meansensors();
    state++;
    delay(1000);
  }

  if (state==1) {
    Serial.println("\nCalculating offsets...");
    calibration();
    state++;
    delay(1000);
  }

  if (state==2) {
    meansensors();
    Serial.println("\nFINISHED!");
    Serial.print("\nSensor readings with offsets:\t");
    Serial.print(mean_ax); 
    Serial.print("\t");
    Serial.print(mean_ay); 
    Serial.print("\t");
    Serial.print(mean_az); 
    Serial.print("\t");
    Serial.print(mean_gx); 
    Serial.print("\t");
    Serial.print(mean_gy); 
    Serial.print("\t");
    Serial.println(mean_gz);
    Serial.print("Your offsets:\t");
    Serial.print(ax_offset); 
    Serial.print("\t");
    Serial.print(ay_offset); 
    Serial.print("\t");
    Serial.print(az_offset); 
    Serial.print("\t");
    Serial.print(gx_offset); 
    Serial.print("\t");
    Serial.print(gy_offset); 
    Serial.print("\t");
    Serial.println(gz_offset); 
    Serial.println("\nData is printed as: acelX acelY acelZ giroX giroY giroZ");
    Serial.println("Check that your sensor readings are close to 0 0 16384 0 0 0");
    Serial.println("If calibration was succesful write down your offsets so you can set them in your projects using something similar to mpu.setXAccelOffset(youroffset)");
    while (1);
  }
}

///////////////////////////////////   FUNCTIONS   ////////////////////////////////////
void meansensors(){
  long i=0,buff_ax=0,buff_ay=0,buff_az=0,buff_gx=0,buff_gy=0,buff_gz=0;

  while (i<(buffersize+101)){
    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    
    if (i>100 && i<=(buffersize+100)){ //First 100 measures are discarded
      buff_ax=buff_ax+ax;
      buff_ay=buff_ay+ay;
      buff_az=buff_az+az;
      buff_gx=buff_gx+gx;
      buff_gy=buff_gy+gy;
      buff_gz=buff_gz+gz;
    }
    if (i==(buffersize+100)){
      mean_ax=buff_ax/buffersize;
      mean_ay=buff_ay/buffersize;
      mean_az=buff_az/buffersize;
      mean_gx=buff_gx/buffersize;
      mean_gy=buff_gy/buffersize;
      mean_gz=buff_gz/buffersize;
    }
    i++;
    delay(2); //Needed so we don't get repeated measures
  }
}

void calibration(){
  ax_offset=-mean_ax/8;
  ay_offset=-mean_ay/8;
  az_offset=(16384-mean_az)/8;

  gx_offset=-mean_gx/4;
  gy_offset=-mean_gy/4;
  gz_offset=-mean_gz/4;
  while (1){
    int ready=0;
    accelgyro.setXAccelOffset(ax_offset);
    accelgyro.setYAccelOffset(ay_offset);
    accelgyro.setZAccelOffset(az_offset);

    accelgyro.setXGyroOffset(gx_offset);
    accelgyro.setYGyroOffset(gy_offset);
    accelgyro.setZGyroOffset(gz_offset);

    meansensors();
    Serial.println("...");

    if (abs(mean_ax)<=acel_deadzone) ready++;
    else ax_offset=ax_offset-mean_ax/acel_deadzone;

    if (abs(mean_ay)<=acel_deadzone) ready++;
    else ay_offset=ay_offset-mean_ay/acel_deadzone;

    if (abs(16384-mean_az)<=acel_deadzone) ready++;
    else az_offset=az_offset+(16384-mean_az)/acel_deadzone;

    if (abs(mean_gx)<=giro_deadzone) ready++;
    else gx_offset=gx_offset-mean_gx/(giro_deadzone+1);

    if (abs(mean_gy)<=giro_deadzone) ready++;
    else gy_offset=gy_offset-mean_gy/(giro_deadzone+1);

    if (abs(mean_gz)<=giro_deadzone) ready++;
    else gz_offset=gz_offset-mean_gz/(giro_deadzone+1);

    if (ready==6) break;
  }
}

If all goes well, the sketch should output the accelerometer and gyroscope offsets through the serial monitor. It may take up to a minute for the sketch to converge on the correct offsets. If the sensor is accidentality moved during calibration, it could take even longer to complete.

To use the calibration values, all you have to do is plug them into the initialisation code at the start of any sketch in which you use the MPU-6050! Please note that the required offsets vary significantly from sensor to sensor, so you have to repeat the calibration program above for each MPU-6050 sensor you are using. That concludes the basic calibration of the MPU-6050; the sensor should now be more than accurate enough for most applications, such as self-balancing robots and quad-copters.

Updated: 23rd May 2019 – Reformatted post

Updated: 26th May 2020 – Improved the descriptions and reformatted post

If you haven’t already read the first part of this Self-balancing robot series, I would encourage you to do so now! The first section below deals with a little bit of theory behind the sensors, so if you want to get straight to the programming/building part, please feel free to skip to the second section…

Deciding which Gyroscope Module to Use

So you have decided to go ahead with the project and construct your own self-balancing robot? That’s great! Now we can start by looking at one of the most significant elements of this project; the sensor. Traditionally the sensor of preference for stabilisation is a gyroscope. Now-days gyroscopes are extremely small and very cheap to buy, so they are ideal for amateur electronics projects. Unfortunately these gyroscopes (both the cheap and the not-so-cheap versions) also come with their own problems. They are good for short-term and quick movements, but tend to drift over time as the error accumulates. They also record a lot of jitter and noise, which needs to be filtered by the micro-controller before the data can be used.

To reduce this drifting effect of the gyroscope, it is possible to combine the sensor data with that from an accelerometer. The accelerometer is good at sensing slower and more prolonged movements, rather than the fast motion. Therefore if we take the best of both worlds and fuse the data together, we will be left with an extremely accurate picture of the motion of the robot.

As a result I decided to use a combined accelerometer & gyroscope breakout module (the MPU-6050), which is slightly more expensive than a simple gyro, but should lead to a superior stabilisation performance. Note: the MPU-6050 comes with a library which does all of the sensor fusion calculations for you, so that definitely is a plus!

Getting started with the Accel-Gyro Module

The MPU-6050 uses I²C to communicate with the micro-controller, so I started by connecting up the pins as shown in the schematics: the SDA line connects to the Analog pin 4, the SCL to Analog pin 5, power input to the 3.3v pin and the ground to the GND pin. If you are using one of the newer Arduinos, you could also connect the sensor to the dedicated SDA and SCL header pins.

As I am using an Intel Galileo Gen2 board, I did not use the interrupt pin. In general it is very bad practice to disregard the interrupts, as it might cause the buffer holding the sensor data to overflow, but for some reason the Galileo does not support normal interrupts! In order to get the sensor working correctly on the Galileo Board, I spent a lot of time on getting the timing right so that there is no overflow of incoming sensor data.For all other Arduino-compatible board, you should use the interrupt so that the micro-controller deals with new sensor data the moment that it is sent. (The interrupt pin is connected to Arduino digital pin 2)

Now it is time to get some data from the accel-gyro module! To do this I simply used the sample code which came with the documentation of the MPU6050 in order to read the raw sensor data. For this sample to work, the I2Cdev and the MPU6050 libraries need to be installed. Here is the code:

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class
// 10/7/2011 by Jeff Rowberg <jeff@rowberg.net>
/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2011 Jeff Rowberg

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
	#include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
//MPU6050 accelgyro(0x69); // <-- use for AD0 high

int16_t ax, ay, az;
int16_t gx, gy, gz;

#define LED_PIN 13
bool blinkState = false;

void setup() {
	// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
	Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
	Fastwire::setup(400, true);
#endif

	// initialize serial communication
	// it's really up to you depending on your project)
	Serial.begin(115200);

	// initialize device
	Serial.println("Initializing I2C devices...");
	accelgyro.initialize();

	// verify connection
	Serial.println("Testing device connections...");
	Serial.print("MPU Connection ");
	Serial.println(accelgyro.testConnection() ? "successful" : "failed");

	// configure Arduino LED
	pinMode(LED_PIN, OUTPUT);
}

void loop() {
	// read raw accel/gyro measurements from device
	accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

	// display tab-separated accel/gyro x/y/z values
	Serial.print("a/g:\t");
	Serial.print(ax); Serial.print("\t");
	Serial.print(ay); Serial.print("\t");
	Serial.print(az); Serial.print("\t");
	Serial.print(gx); Serial.print("\t");
	Serial.print(gy); Serial.print("\t");
	Serial.println(gz);

	// blink LED to indicate activity
	blinkState = !blinkState;
	digitalWrite(LED_PIN, blinkState);
}

The result I got on the Serial Monitor looked like this:

Initializing I2C devices...
Testing device connections...
MPU Connection successful 
a/g:	-1428	14240	12120	-536	131	-149
a/g:	-1416	14196	11972	-505	110	-169
a/g:	-1484	14260	11948	-524	108	-147
a/g:	-1508	14220	11968	-513	87	-151
a/g:	-1540	14176	11920	-501	15	-164
a/g:	-1408	14212	11984	-523	27	-153
a/g:	-1472	14104	11888	-526	123	-137
a/g:	-1400	14236	11936	-514	136	-148
a/g:	-1512	14216	12008	-522	132	-142
a/g:	-1412	14172	11956	-520	127	-158
a/g:	-1456	14120	11936	-533	94	-166
a/g:	-1496	14124	11936	-529	97	-161
a/g:	-1496	14208	11996	-526	108	-177
a/g:	-1420	14236	11992	-505	104	-151
a/g:	-1540	14264	11984	-510	115	-171
a/g:	-1468	14300	12068	-504	51	-143
a/g:	-1508	14172	11868	-545	105	-137
a/g:	-1416	14244	11812	-523	69	-154
a/g:	-1472	14276	11956	-504	92	-146
a/g:	-1492	14192	12028	-517	143	-175

In this data we can see the readings from the accelerometer already divided into the x/y/z values, and the readings from the gyroscope are also divided into its x/y/z components. This is a great first step, but unfortunately this data is not very usable in its current form. We still have to fuse the accelerometer and gyroscope data together, and then filter it to remove all of the noise!

Manipulating the data

It is possible to calculate the tilt of the sensor manually through the use of a number of formulas, but fortunately (at least for the MPU-6050) there is a library to do this for us! As a matter of fact, the MPU-6050 has a built-in “Motion Processing Unit” (hence the initials) which can be used to process the sensor data, therefore minimising the load on the micro-processor. This library also automatically filters the data so that we get a clean and usable result straight away.

Above is the equation used to calculate the angle of inclination from the accelerometer data, where (Ax) and (Ay) are the x- and y- accelerometer values. If you want to find out about calculating the angles manually, please visit: http://www.kerrywong.com/2012/03/08/a-self-balancing-robot-i/. Kerry Wong does an awesome job at describing the whole system, so I would encourage you to check it out! He covers all of the main topics such as the calculations, sensor fusion and the Kalman filter.

As regards the filtering, here is another great post which looks at the main advantages and disadvantages of using the a complimentary filter instead of the MPU6050 motion-apps library: http://www.geekmomprojects.com/mpu-6050-redux-dmp-data-fusion-vs-complementary-filter/

Updated: 23rd May 2019 – Reformatted post

Why build a self-balancing robot?

The aim of a self-balancing robot is to balance itself on two wheels, being able to drive around without toppling over. Self-balancing robots use a “closed-loop feedback control” system; this means that real-time data from motion sensors is used to control the motors and quickly compensate for any tilting motion in order to keep the robot upright. Similar self-balancing feedback control systems can be seen in many other applications. Some of the obvious examples include Segways, bipedal robots and space rockets (A few rockets have been lost due to a faulty balancing system).

But what many people don’t realise is that often the same type of controller is also used in a large variety of other applications which aren’t related to balance. Proportional-integral-derivative (PID) controllers are used by elevators to control their motion and position, used by air-conditioning units to control the temperature within a room, and even used to control the operation of jet engines. Of course rockets use significantly more complex controllers than air-conditioners, but the underlying principle is still the same: how to adjust the system in order to get as close to the desired target value (be it temperature, angle, or position) as possible. That is why building a self-balancing robot is so educational; you can use the same control methods over and over again for other projects. And don’t forget, self-balancing robots are a fun toy to play with! Here is a video of my self-balancing in action:

A Brief Overview

It takes several steps to build a self-balancing robot. The easiest part is the hardware, so that is always a good place to start. The robot requires two motors, a motor controller, a sensor to detect its current tilt angle, a micro-controller and some type of frame. Surprisingly enough due to the inertia of an inverted pendulum, the robot actually finds it easier to stabilise if the frame is very tall with a lot of weight on top, instead of a small frame with a low centre of gravity!

Once all of the physical components have been assembled, we can proceed to the tricky stuff; the software. As the system is inherently unstable and wants to topple over, the micro-controller needs to continuously monitor the current angle of the robot and be as fast and efficient as possible in order to retain its balance. While the loop time (refresh rate) should be short, it also needs to remain regular so that the control system can properly performs its calculations. For my robot I used a standard loop time of 10ms (100Hz). Therefore, the controller recalculates its response 100 times per second!

Required Components

Before we dive into how to make a self-balancing robot, we need to get our hardware together. Here is a list of the required component, with some links as reference:

• Essential:
• 1 x Arduino-compatible Controller
• 1 x Power Supply/Battery
• 1 x Motor Controller
• 1 x Gyroscope/Accelerometer Module (Eg. MPU-6050)
• 2 x High Torque Gearbox Motors
• Colour-coded wires, preferably single core
• Some type of frame!

• Optional:
• Some buttons to control the robot
• A on/off power switch
• A number of colourful LEDs

The links above are for reference only! I recommend that you look around for the best deals before buying anything.

I decided to use the Intel Galileo Gen2 Development Board (now discontinued) for my micro-controller as I still had a couple lying around, and it is compatible with the Arduino environment. In reality you could use any board (Eg. Uno, Mega, Red-board) to make your self-balancing robot. In all of my robots I used the MPU-6050 sensor, which contains both a gyroscope and an accelerometer. The benefit of using both of the sensors together is that their data can be fused, giving us much stabler and more accurate readings for angle.

To create my frame I designed all of the components on the computer using SketchUp, and then sent them to be 3D printed. If you don’t have access a printer, an alternative would be to build your own frame out of scrap parts! I built my first self-balancing robot prototype out of lollipop sticks and glue, and I was surprised how well it worked! A frame design which I have commonly seen other people implement is to use a couple of plastic/wood rectangles as platforms, and to connect these using a long threaded bolt and some nuts (as shown in the diagram below).

Please leave a comment below if you have any questions or suggestions. In the next part of this tutorial I will cover the basics of how accelerometer and gyroscope sensors work, and how we can use them in our robot.

Updated: 23rd May 2019 – Reformatted post

Updated: 29th May 2020 – Updated the descriptions and reformatted post