MCU code for Inverted Pendulum System
This page is mainly focussed on describing how Arduino UNO (Atmega microcontroller) could be used for inverted pendulum.For basics on inverted pendulum please refer here.
Simulink modelling and design of Inverted Pendulum is done in Matlab.
It was previously implemented using NI Labview on computer using parallel port.
Thereafter we implemented the same using microcontroller (Arduino).
Please refer to the of Inverted Pendulum System.
The Inverted Pendulum system consists of 2 optical encoders (one for base motor and another for Pendulum) which tells the angle of motor and pendulum. State space controls are designed for this system using matlab/simulink simulations. Four parameters namely pendulum angle, pendulum angular velocity, base motor angle and base motor velocity are required to compute base motor control voltage. Apart from mechanical design, we also need servo amplifier as a motor driver.
Micro controller operations
The following section describes the MCU related operations:
Reading optical Encoder
Two interrupts were used for reading quadrature encoder input. Please refer to the code:
const int en11=14;
const int en12=15;
void motorcount()
{
int m1=digitalRead(en11);
int m2=digitalRead(en12);
c<<=1;
c=cm1;
c<<=1;
c=cm2;
c=c&15;
if (c==2c==4c==11c==13)
{
mcount++;
} else {
mcount;
}
}
Here en11 and en12 refers to pins where two inputs from quadrature encoders were connected. We initially tried without quadrature encoder but couldn’t balance the pendulum because of insufficient resolution. Here register C stores the two input from optical encoders that helps in making the decision of direction of rotation.
Angular Velocity
We used the technique of forward differentiation to compute the angular velocity. Please not that we are using two parameters to decide when to compute the angular velocity namely times (time past since the last computation in microsecond) and minimum number of counts after which the velocity needs to be computed. The reason for keeping the time factor is because once the pendulum gets balanced, since the count is less 4, it never computes the velocity and gives us previous false velocity.
if (abs(pcountcurrent)>=4  times>50000)
{
pdot=((pcountcurrent)*1.0f*1000000*PI/(((micros()c_time)*180)));
mdot=((pcountcurrent)*1.0f*1000000*PI/(((micros()c_time)*180)));
flag=1;
}
State Space Control
This piece of code implements the swing up gain along with state space control. Here we are giving swing up voltage if pendulum angle in beyond 25 degree and controller controller comes into picture within 20 degree of range. 20 to 25 degree is considered as dead zone where pendulum is free (no voltage is applied). The first four lines of the code defines the respective gain values as the name denotes.
float pgain=10;
float pdotgain=2;
float mgain=0.5;
float mdotgain=0.2;
// Enabling Swing up gain
if (pcount_norm*180/PI > 25  pcount_norm*180/PI < 25)
{
vout=1+ref;
} else if (pcount_norm*180/PI <20  pcount_norm*180/PI > 20) {
// Enabled Swing up Gain float vout=(pgain*pcount_norm*a_f+pdotgain*pdot*v_f+mgain*mcount_norm*a_f+mdotgain*mdot*v_f)+base; } else {
vout=0;
}

For complete code refer here.
Leave a Reply