We used Arduino Mega to implement for our Segway project which does the following computations for us. Complete code is attached for reference. Following tasks are performed using Arduino which are explained later in this page.
1. Reading encoder inputs from optical encoders using interrupts.
2. Reading Accelerometer Input and Gyro Input.
3. Implemented Complementary filtering technique for Segway angle computations.
4. Implemented strong filter (smoothening) for Segway Velocity computation.
5. Implemented Simpson integrator for generating accurate turning torque.
1. IMU (Accelerometer and Gyro)
2. Optical Encoders (3 required)
a. Steer count b. Left Motor Count c. Right Motor Count Circuit Diagram of Segway
Please click here to go through the code explanation.
Complete code can be found here.
For details refer to Mechanical design, Simulink model and control system design please refer here.
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.
The following section describes the MCU related operations:
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=c|m1;
c<<=1;
c=c|m2;
c=c&15;
if (c==2||c==4||c==11||c==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.
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(pcount-current)>=4 || times>50000)
{
pdot=((pcount-current)*1.0f*1000000*PI/(((micros()-c_time)*180)));
mdot=((pcount-current)*1.0f*1000000*PI/(((micros()-c_time)*180)));
flag=1;
}
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;
}