Inverted pendulum

Experiment No. 3

EE324: Control System Lab

Group No. 6

Full Name Enrollment No.

Om Unhale 210070058
Harsh Agrawal 210070032
Kadiyala Sai Susrush 210070038

November 1, 2023
Contents

1 Objectives 1

2 Control Algorithm 1

3 Challenges Faced and Solutions 9

4 Results 9

5 Observations and Inferences 10

6 References 12
 Inverted pendulum

1 | Objectives
The aim of the experiment is:
To design and implement control action for maintaining a pendulum in the upright position (even when
subjected to external disturbances) through the LQR technique using Arduino Mega and an IC with the
following objectives. The objectives to be followed:

■ To restrict the pendulum arm vibration (α) within ±3 degrees.

■ To restrict the base angle oscillation (θ) within ±30 degrees.
This was implemented as described in the following steps :

■ Go through the given resources, like notes on LQR lecture and PendulumSetup manual, and study
the controller design and various matrices for LRQ control.
■ Evaluate A, B, C and D matrices. Thereafter, matrices Q and R are to be created where Q is a 4 ×
diagonal matrix, and R is 1 × 1 matrix with some random values.
■ By using the MATLAB code, which is given below, the K matrix is to be obtained.
■ Modify the sample Arduino code so that the Arduino can deliver the required power to the motor
involving the constants k1, k2, k3 and k4 from the K-matrix.
■ Fine-tune the system by running the whole set-up and checking it by changing parameters inside Q
and R matrices consistently.

2 | Control Algorithm
Matlab code:

Kt = 0.02797;
Km = 0.02797;
Rm = 16.6;
Jm = 0.3e-4;
Mp = 0.027;
Lp = 0.191;%Total Length of Pendulum in m
lp = 0.153;%Distance of Pendulum Cm from Pivot in m
Jp = 1.1e-4;%Pendulum Moment of Inertia about Centre of Mass in Kg*mˆ2

Marm = 0.028;%Mass of Arm in Kg

r = 0.0826;%Distance b/w Arm pivot and Pendulum Pivot in m
Jeq = 1.23e-4;% Equivalent Moment of inertia of Arm about Motor Pivot in Kg*mˆ2

g = 9.8;%Acceleration due to Gravity in m/sˆ2

%**********
%Reduced Variables
%r, g, Km, Kt, Rm
Jtot2 = Jp*Jeq + Mp*lp*lp*Jeq + Jp*Mp*r*r;
Jpend_axis = Jp + Mp*lp*lp;
Jmot_axis = Jeq + Mp*r*r;
Mplp = Mp*lp;
%disp(Mplp);
%**********
%Matrices
A = [
0 0 1 0
0 0 0 1
0 (r*(Mplpˆ2)*g)/Jtot2 Kt*Km*Jpend_axis/(Jtot2*Rm) 0

Page 1
 Inverted pendulum

0 Mplp*g*Jmot_axis/Jtot2 -r*Mplp*Kt*Km/(Jtot2*Rm) 0
];

B = [
0
0
Kt*Jpend_axis/(Jtot2*Rm)
r*Mplp*Kt/(Jtot2*Rm)
];

A_lqr = [
0 1 0 0
0 Kt*Km*Jpend_axis/(Jtot2*Rm) (r*(Mplpˆ2)*g)/Jtot2 0
0 0 0 1
0 -r*Mplp*Kt*Km/(Jtot2*Rm) Mplp*g*Jmot_axis/Jtot2 0
];

B_lqr = [
0
Kt*Jpend_axis/(Jtot2*Rm)
0
r*Mplp*Kt/(Jtot2*Rm)
];

C = [
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
];

D = [
0
0
0
0
];

%**********
Q = [
384.756 0 0 0
0 121.312 0 0
0 0 350.756 0
0 0 0 520.249
];
R = [100];
%**********
[K, S, P] = lqr(A_lqr, B_lqr, Q, R);
disp(K);
%**********

Arduino code:

/*
*
* This sample code can be used with the Arduino Uno to control the AMT22 encoder.

Page 2
 Inverted pendulum

* It uses SPI to control the encoder and the the Arduino UART to report back to the PC
* via the Arduino Serial Monitor.
* For more information or assistance contact CUI Inc for support.
*
* After uploading code to Arduino Uno open the open the Serial Monitor under the Tools
* menu and set the baud rate to 115200 to view the serial stream the position from the AMT22.
*
* Arduino Pin Connections
* SPI Chip Select Enc 0(motor): Pin 2
* SPI Chip Select Enc 1(pendulum): Pin 4//3
* SPI MOSI Pin 51
* SPI MISO Pin 50
* SPI SCLK: Pin 52
*
* AMT22 Pin Connections
* Vdd (5V): Pin 1
* SPI SCLK: Pin 2
* SPI MOSI: Pin 3
* GND: Pin 4
* SPI MISO: Pin 5
* SPI Chip Select: Pin 6
*/
//****************************************
/* Include the SPI library for the arduino boards */
#include <SPI.h>
/* Serial rates for UART */
#define BAUDRATE 115200
/* SPI commands */
#define AMT22_NOP 0x00
#define AMT22_RESET 0x60
#define AMT22_ZERO 0x70
/* Define special ascii characters */
#define NEWLINE 0x0A
#define TAB 0x09
/* We will use these define macros so we can write code once compatible with 12 or 14 bit encoders */
#define RES12 12
#define RES14 14
/* SPI pin connections to Arduino Board*/
#define ENC_0 2
#define ENC_1 3
#define SPI_MOSI 51
#define SPI_MISO 50
#define SPI_SCLK 52
//*******************************************
//motor output pins
#define motor_plus_pin 9
#define motor_minus_pin 8
//variables
double motor_angle = 0;
double pendulum_angle = 0;
double motor_angle_prev = 0;
double pendulum_angle_prev = 0;
double delta_motor_angle = 0;
double delta_pendulum_angle = 0;
double V_m = 0;
double k_mot = 0;
double k_pend = 0;
double k_dmot = 0;

Page 3
 Inverted pendulum

double k_dpend = 0;
int motor_plus_val = 0;
int motor_minus_val = 0;
double t = 0;
double time_prev = 0;
double del_time = 0;
double pi_val = 3.141592654;
//*****************************************
void setup()
{
//Set the modes for the SPI IO8
pinMode(SPI_SCLK, OUTPUT);
pinMode(SPI_MOSI, OUTPUT);
pinMode(SPI_MISO, INPUT);//The only INPUT to the Arduino
pinMode(ENC_0, OUTPUT);//wtf?
pinMode(ENC_1, OUTPUT);//wtf?
//Initialising Serial Monitor
Serial.begin(BAUDRATE);
//Get the CS line high which is the default inactive state
digitalWrite(ENC_0, HIGH);//omg!
digitalWrite(ENC_1, HIGH);

//set the clockrate. Uno(Mega too) clock rate is 16Mhz, divider of 32 gives 500 kHz.
//500 kHz is a good speed for our test environment
//SPI.setClockDivider(SPI_CLOCK_DIV2); // 8 MHz
//SPI.setClockDivider(SPI_CLOCK_DIV4); // 4 MHz
//SPI.setClockDivider(SPI_CLOCK_DIV8); // 2 MHz
//SPI.setClockDivider(SPI_CLOCK_DIV16); // 1 MHz
SPI.setClockDivider(SPI_CLOCK_DIV32); // 500 kHz
//SPI.setClockDivider(SPI_CLOCK_DIV64); // 250 kHz
//SPI.setClockDivider(SPI_CLOCK_DIV128); // 125 kHz

//start SPI bus

SPI.begin();
}

void loop()
{
//create a 16 bit variable for the encoders position
uint16_t encoderPosition;
//let’s also create a variable where we can count how many times we’ve tried to obtain the position
uint8_t attempts;
//if you want to set the zero position before beggining uncomment the following function call
setZeroSPI(ENC_0);
setZeroSPI(ENC_1);
Serial.println(" ");
Serial.println("Started...");
//***********************************************
delay(5000);//wait 10 seconds in Arduino to let the USER erect the pendulum and rotate the motor to
//************************************************
//once we enter this loop we will run forever
//initialising the state variables
motor_angle = 180.0;
pendulum_angle = 180.0;
motor_angle_prev = 180.0;
pendulum_angle_prev = 180.0;
//************************************************
while(1)

Page 4
 Inverted pendulum

{
//set attemps counter at 0 so we can try again if we get bad position
attempts = 0;
//************************************************
//this function gets the encoder position and returns it as a uint16_t
//send the function either res12 or res14 for your encoders resolution
encoderPosition = getPositionSPI(ENC_0, RES14);
//if the position returned was 0xFFFF we know that there was an error calculating the checksum
//make 3 attempts for position. we will pre-increment attempts because we’ll use the number later
while (encoderPosition == 0xFFFF && ++attempts < 3)
{
encoderPosition = getPositionSPI(ENC_0, RES14); //try again
}

if (encoderPosition == 0xFFFF) //position is bad, let the user know how many times we tried
{
Serial.print("Motor Angle error. Attempts: ");
Serial.print(attempts, DEC); //print out the number in decimal format. attempts - 1 is used sin
//Serial.write(TAB);
}
else //position was good, print to serial stream
{

//Serial.print("Motor Angle: ");

//Serial.print(encoderPosition, DEC); //print the position in decimal format
//Serial.write(TAB);
motor_angle = encoderPosition*360.0/16384.0;
}
//////////again for second encoder//////////////////////////////
//set attemps counter at 0 so we can try again if we get bad position
attempts = 0;
//this function gets the encoder position and returns it as a uint16_t
//send the function either res12 or res14 for your encoders resolution
encoderPosition = getPositionSPI(ENC_1, RES14);
//if the position returned was 0xFFFF we know that there was an error calculating the checksum
//make 3 attempts for position. we will pre-increment attempts because we’ll use the number later
while (encoderPosition == 0xFFFF && ++attempts < 3)
{
encoderPosition = getPositionSPI(ENC_1, RES14); //try again
}

if (encoderPosition == 0xFFFF) //position is bad, let the user know how many times we tried
{
Serial.print("Pendulum Angle error. Attempts: ");
Serial.print(attempts, DEC); //print out the number in decimal format. attempts - 1 is used sin
//Serial.write(NEWLINE);
}
else //position was good, print to serial stream
{
//Serial.print("Pendulum Angle: ");
//Serial.print(encoderPosition, DEC); //print the position in decimal format
//Serial.write(NEWLINE);
pendulum_angle = encoderPosition*360.0/16384.0;
}
//angle sensing part ends here

t = millis();
t = t/1000.000;//returns Current Run Time in Seconds

Page 5
 Inverted pendulum

del_time = t - time_prev;//time difference b/w steps in Seconds

time_prev = t;//storing current time(in Seconds) for next step
//************************************************
//control part
//already obtained
//motor_angle
//pendulum_angle
//the derivatives of angles...
delta_motor_angle = motor_angle - motor_angle_prev;
delta_pendulum_angle = pendulum_angle - pendulum_angle_prev;
k_mot = -1.9615;//
k_dmot = -2.2743;//-2.2963;97.5198
k_pend = 95.9159;//;-2.1832;//-2.1832
k_dpend = 11.6154;//11.7282; 11.8323;//
//control variable
//+ve V_m moves motor in increasing direction of Theta(increasing motor angle)
//also convert degrees in SI units, Radians
//if ((pendulum_angle<=183) && (pendulum_angle>=177))

V_m = (-(255/12)(pi_val/180.0)( k_mot*(motor_angle-180) + k_pend*(pendulum_angle-180) + k_dmot*

//if(motor_angle>180){V_m = -255;}
//if(motor_angle<180){V_m = 255;}
//****************************************************
//update the previous angle values...do this at the end of all computations
motor_angle_prev = motor_angle;
pendulum_angle_prev = pendulum_angle;
//****************************************************
//actuation part
//clip V_m
if(V_m>255){V_m = 255;}
if(V_m<-255){V_m = -255;}
motor_plus_val = floor(128 + (V_m/2.000));
motor_minus_val = floor(128 - (V_m/2.000));
analogWrite(motor_plus_pin, motor_plus_val);//floor(V_m/2.0));
analogWrite(motor_minus_pin, motor_minus_val);//floor(V_m/2.0));
//analogWrite(motor_plus_pin, 100);
//analogWrite(motor_minus_pin, 0);
//delay(100);
//analogWrite(motor_plus_pin, 0);
//analogWrite(motor_minus_pin, 100);
//delay(100);
//************************************************
//Printing the State and Input
Serial.print("Motor Angle: ");
Serial.print(motor_angle);
Serial.print("\t Pendulum Angle: ");
Serial.print(pendulum_angle);
Serial.print("\t Time: ");
Serial.print(t);
Serial.print("\t Previous Time Step: ");
Serial.print(del_time);
Serial.print("\t Mot_Vel: ");
Serial.print(delta_motor_angle/del_time);
Serial.print("\t Pend_Vel: ");
Serial.print(delta_pendulum_angle/del_time);
Serial.print("\t Vm: ");
Serial.println(V_m);

Page 6
 Inverted pendulum

}
}
//***************************************************
//function definitions
/*
* This function gets the absolute position from the AMT22 encoder using the SPI bus. The AMT22 posit
* for position verification. Both 12-bit and 14-bit encoders transfer position via two bytes, giving
* For 12-bit encoders the position is left-shifted two bits, leaving the right two bits as zeros. Th
* is actually sending 14-bits, when it is actually sending 12-bit values, where every number is mult
* This function takes the pin number of the desired device as an input
* This funciton expects res12 or res14 to properly format position responses.
* Error values are returned as 0xFFFF
*/
uint16_t getPositionSPI(uint8_t encoder, uint8_t resolution)
{
uint16_t currentPosition; //16-bit response from encoder
bool binaryArray[16]; //after receiving the position we will populate this array and use

//get first byte which is the high byte, shift it 8 bits. don’t release line for the first byte
currentPosition = spiWriteRead(AMT22_NOP, encoder, false) << 8;

//this is the time required between bytes as specified in the datasheet.

//We will implement that time delay here, however the arduino is not the fastest device so the dela
//is likely inherantly there already
delayMicroseconds(3);

//OR the low byte with the currentPosition variable. release line after second byte
currentPosition |= spiWriteRead(AMT22_NOP, encoder, true);

//run through the 16 bits of position and put each bit into a slot in the array so we can do the ch
for(int i = 0; i < 16; i++) binaryArray[i] = (0x01) & (currentPosition >> (i));

//using the equation on the datasheet we can calculate the checksums and then make sure they match
if ((binaryArray[15] == !(binaryArray[13] ˆ binaryArray[11] ˆ binaryArray[9] ˆ binaryArray[7] ˆ bin
&& (binaryArray[14] == !(binaryArray[12] ˆ binaryArray[10] ˆ binaryArray[8] ˆ binaryArray[6
{
//we got back a good position, so just mask away the checkbits
currentPosition &= 0x3FFF;
}
else
{
currentPosition = 0xFFFF; //bad position
}

//If the resolution is 12-bits, and wasn’t 0xFFFF, then shift position, otherwise do nothing
if ((resolution == RES12) && (currentPosition != 0xFFFF)) currentPosition = currentPosition >> 2;

return currentPosition;
}

/*
* This function does the SPI transfer. sendByte is the byte to transmit.
* Use releaseLine to let the spiWriteRead function know if it should release
* the chip select line after transfer.
* This function takes the pin number of the desired device as an input
* The received data is returned.
*/

Page 7
 Inverted pendulum

uint8_t spiWriteRead(uint8_t sendByte, uint8_t encoder, uint8_t releaseLine)

{
//holder for the received over SPI
uint8_t data;

//set cs low, cs may already be low but there’s no issue calling it again except for extra time
setCSLine(encoder ,LOW);

//There is a minimum time requirement after CS goes low before data can be clocked out of the encod
//We will implement that time delay here, however the arduino is not the fastest device so the dela
//is likely inherantly there already
delayMicroseconds(3);

//send the command

data = SPI.transfer(sendByte);
delayMicroseconds(3); //There is also a minimum time after clocking that CS should remain asserted
setCSLine(encoder, releaseLine); //if releaseLine is high set it high else it stays low

return data;
}

/*
* This function sets the state of the SPI line. It isn’t necessary but makes the code more readable
* This function takes the pin number of the desired device as an input
*/
void setCSLine (uint8_t encoder, uint8_t csLine)
{
digitalWrite(encoder, csLine);
}

/*
* The AMT22 bus allows for extended commands. The first byte is 0x00 like a normal position transfer
* second byte is the command.
* This function takes the pin number of the desired device as an input
*/
void setZeroSPI(uint8_t encoder)
{
spiWriteRead(AMT22_NOP, encoder, false);

//this is the time required between bytes as specified in the datasheet.

//We will implement that time delay here, however the arduino is not the fastest device so the dela
//is likely inherantly there already
delayMicroseconds(3);

spiWriteRead(AMT22_ZERO, encoder, true);

delay(250); //250 second delay to allow the encoder to reset
}

/*
* The AMT22 bus allows for extended commands. The first byte is 0x00 like a normal position transfer
* second byte is the command.
* This function takes the pin number of the desired device as an input
*/
void resetAMT22(uint8_t encoder)
{
spiWriteRead(AMT22_NOP, encoder, false);

//this is the time required betw mveen bytes as specified in the datasheet.

Page 8
 Inverted pendulum

//We will implement that time delay here, however the arduino is not the fastest device so the dela
//is likely inherantly there already
delayMicroseconds(3);

spiWriteRead(AMT22_RESET, encoder, true);

delay(250); //250 second delay to allow the encoder to start back up

}

3 | Challenges Faced and Solutions

We did face challenges during the lab experiment. We had no clue what to put the values in Q and R
matrices. So, we started with very random values.

The pendulum we used has a green colour wire that was broken internally and couldn’t be seen externally.
Because of this, we couldn’t even proceed further in the initial stages, and it took a long time to figure it out.

We were unaware of the fact that the tuning depends on the pendulum set-up, too. So after writing
partial code in one of the labs and running it in the succeeding lab using the same code but on a different
pendulum, it wasn’t running properly. So like one complete lab is wasted in this whole process.

Even after assembling the whole circuit correctly, We didn’t know why, but the circuit was shorted
somehow, and we replaced nearly 5 ICs, which overheated because of large currents.

The biggest challenge was to tune the pendulum.It took so much time to get to the right values of k1,k2,k3
and k4.

4 | Results
Made the pendulum stand erect, as shown in the figure.

Page 9
 Inverted pendulum

Figure 4.1: Pendulum in upright position

5 | Observations and Inferences

α was almost constant butθ was varying rapidly.Using the LQR technique, we could successfully make the
pendulum stand upright with the following observations. For LQR technique

384.756 0 0 0
 
 0 121.312 0 0 
Q=  0 0 350.756 0 


0 0 0 520.249

Page 10
 Inverted pendulum

R = [100]
K [-1.9615 -2.2743 95.9159 11.6514]
θ = 150 degrees
α = 1.5 degrees.

Page 11
 Inverted pendulum

6 | References

Page 12