Project showcase # Inverted Pendulum on a Cart © GPL3+

Balancing an inverted pendulum on a cart with a DC motor. A perfect project for studying mechanical engineering and feedback control theory.

• 19,673 views
• 43 respects

## Apps and online services

### Overview

An inverted pendulum is a classical problem for those who study mechanical engineering and feedback control theory. In this tutorial I will go through the steps of building an inverted pendulum on a cart stabilized with a DC motor. I will describe how to measure motor’s parameters and calculate coefficients for the feedback regulator.

### Equations of motion

Equations of motion can be obtained by differentiating of a Lagrangian which will give the following system:

To make sure I got the equations right I simulated free pendulum with this script.

### Stabilization with feedback

In order to keep the pendulum in the upright position an external force should be applied depending on the state of the system. In this project full state control is used meaning that the control (u) equals to

where x is vector of state, K is a vector of coefficients obtained using LQR. I simulated controlled pendulum with this script.

In physical device we can measure motor's position and an angle of the rod using incremental rotary encoder, the velocities are then calculated as a derivative.

### Motor control

In order to stabilize pendulum we need to apply a certain force which we calculated in a previous section. But the control signal coming out of the Arduino is the width of the PWM signal, basically is the voltage applied to the DC motor. In order to calculate the voltage given the force required we need to model the DC motor and estimate it’s parameters.

The DC motor is modeled by the following equation:

We don’t need to know the exact parameters, instead we can simplify the equations with regards to friction as follows:

In order to determine parameters (a, b, c) we can record the relation of cart's velocity to time depending on different voltages. We’ll obtain the following plot:

Parameters then are found by brute force using the following script. Knowing the parameters and the required force we can calculate the exact voltage.

Now we have all the pieces, the last step is to calculate LQR parameters for the real device (using this script) and program the Arduino.

### Conclusion

Building an inverted pendulum may seem like a daunting task, it took me about two years to get from the idea to the working device. Now it comes down to understanding mechanics, manually measuring parameters of the motor and configuring the regulator. Those steps can be done during the class. This device can be used as an educational stand for experiments on different control techniques. I plan to further refine and productize it so it can be assembled easily and installed in a school or technical museum. Any collaboration on that is welcomed.

## Code

##### Balance an inverted pendulum with a DC motorArduino
```/**
* == Inverted pendulum stabilisation with state control with DC motor ==
*
* == Hardware specification ==
* OMRON E6B2-CWZ6C pinout
* - Brown - Vcc
* - Black - Phase A
* - White - Phase B
* - Orange - Phaze Z
* - Blue - GND
*
* LPD3806-600BM-G5-24C pinout
* - Green - Phase A
* - White - Phase B
* - Red - Vcc
* - Black - GND
*/

#include <Arduino.h>

// motor encoder pins
#define OUTPUT_A  3 // PE5
#define OUTPUT_B  2 // PE4

// pendulum encoder pins
#define REF_OUT_A 18 // PD3
#define REF_OUT_B 19 // PD2

// pulses per revolution
#define PPR  2400
#define SHAFT_R 0.00573
#define PENDULUM_ENCODER_PPR  10000

#define PWM_PIN 10
#define DIR_PIN 8

#define POSITION_LIMIT  0.145

#define A 35.98
#define B 2.22
#define C 2.79

#define Kth 147.1
#define Kw  36.0
#define Kx  54.0
#define Kv  39.5

const float THETA_THRESHOLD = PI / 12;
const float PI2 = 2.0 * PI;

volatile long encoderValue = 0L;
volatile long lastEncoded = 0L;

volatile long refEncoderValue = 0;
volatile long lastRefEncoded = 0;

unsigned long now = 0L;
unsigned long lastTimeMicros = 0L;

float x, last_x, v, dt;
float theta, last_theta, w;
float control, u;

unsigned long log_prescaler = 0;

void encoderHandler();
void refEncoderHandler();

void setup() {

// setting PWD frequency on pin 10 to 31kHz
TCCR2B = (TCCR2B & 0b11111000) | 0x01;

pinMode(OUTPUT_A, INPUT_PULLUP);
pinMode(OUTPUT_B, INPUT_PULLUP);

pinMode(REF_OUT_A, INPUT_PULLUP);
pinMode(REF_OUT_B, INPUT_PULLUP);

attachInterrupt(digitalPinToInterrupt(OUTPUT_A), encoderHandler, CHANGE);
attachInterrupt(digitalPinToInterrupt(OUTPUT_B), encoderHandler, CHANGE);

attachInterrupt(digitalPinToInterrupt(REF_OUT_A), refEncoderHandler, CHANGE);
attachInterrupt(digitalPinToInterrupt(REF_OUT_B), refEncoderHandler, CHANGE);

pinMode(PWM_PIN, OUTPUT);
pinMode(DIR_PIN, OUTPUT);

digitalWrite(DIR_PIN, LOW);

Serial.begin(9600);
lastTimeMicros = 0L;
}

float saturate(float v, float maxValue) {
if (fabs(v) > maxValue) {
return (v > 0) ? maxValue : -maxValue;
} else {
return v;
}
}

float getAngle(long pulses, long ppr) {
float angle = (PI + PI2 * pulses / ppr);
while (angle > PI) {
angle -= PI2;
}
while (angle < -PI) {
angle += PI2;
}
return angle;
}

float getCartDistance(long pulses, long ppr) {
return 2.0 * PI * pulses / PPR * SHAFT_R;
}

void driveMotor(float u) {
digitalWrite(DIR_PIN, u > 0.0 ? LOW : HIGH);
analogWrite(PWM_PIN, fabs(u));
}

boolean isControllable(float theta) {
return fabs(theta) < THETA_THRESHOLD;
}

void log_state(float control, float u) {
if (fabs(w) > 100) {
return;
}

if (log_prescaler % 20 == 0) {
Serial.print(theta, 4);Serial.print("\t");
Serial.print(w, 4);Serial.print("\t");
Serial.print(x, 4);Serial.print("\t");
Serial.print(v, 4);Serial.print("\t");
Serial.print(control, 4);Serial.print("\t");
Serial.println(u, 4);
}
log_prescaler++;
}

void loop() {
now = micros();
dt = 1.0 * (now - lastTimeMicros) / 1000000;
x = getCartDistance(encoderValue, PPR);
v = (x - last_x) / dt;

theta = getAngle(refEncoderValue, PENDULUM_ENCODER_PPR);
w = (theta - last_theta) / dt;

if (isControllable(theta) && fabs(x) < POSITION_LIMIT) {
control = (Kx * x + Kv * v + Kth * theta + Kw * w);
u = (control + A * v + copysignf(C, v)) / B;
u = 255.0 * u / 12.0;
driveMotor(saturate(u, 254));
} else {
driveMotor(0);
}

last_x = x;
last_theta = theta;
lastTimeMicros = now;

log_state(control, u);

delay(5);
}

/**
* Motor encoder handler
*/
void encoderHandler() {
int MSB = (PINE & (1 << PE5)) >> PE5; //MSB = most significant bit
int LSB = (PINE & (1 << PE4)) >> PE4; //LSB = least significant bit
int encoded = (MSB << 1) | LSB; //converting the 2 pin value to single number
int sum  = (lastEncoded << 2) | encoded; //adding it to the previous encoded value

if(sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) {
encoderValue++; //CW
}
if(sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) {
encoderValue--; //CCW
}

lastEncoded = encoded; //store this value for next time
}

/**
* Pendulum encoder handler
* Encoder attached to pins:
* Phase A - 18 PD3
* Phase B - 19 PD2
*/
void refEncoderHandler() {
int MSB = (PIND & (1 << PD3)) >> PD3; //MSB = most significant bit
int LSB = (PIND & (1 << PD2)) >> PD2; //LSB = least significant bit
int encoded = (MSB << 1) | LSB; //converting the 2 pin value to single number
int sum  = (lastRefEncoded << 2) | encoded; //adding it to the previous encoded value

if(sum == 0b1101 || sum == 0b0100 || sum == 0b0010 || sum == 0b1011) {
refEncoderValue++; //CW
}
if(sum == 0b1110 || sum == 0b0111 || sum == 0b0001 || sum == 0b1000) {
refEncoderValue--; //CCW
}

lastRefEncoded = encoded; //store this value for next time
}
```
##### Inverted pendulum repo
Contains all the sources and scripts used during the project

#### Remote Controlled Shopping Cart Using evive

Project tutorial by STEMpedia

• 3,316 views
• 4 respects

#### Slider Built with Recycled Printer Cart

Project showcase by Guiye Perez Bongiovanni

• 4,027 views
• 17 respects

#### Creating Images Using One LED

Project tutorial by Arduino “having11” Guy

• 13,687 views
• 39 respects

#### Bluetooth-Controlled Arduino Robot

Project showcase by jayesh_nawani

• 10,660 views
• 1 comment
• 10 respects

#### Tvc15 Robot

Project showcase by lefthandsh8k

• 1,316 views