Control Systems Lab

Inverted Pendulum Balancing Report

Keshav Samdani (22B3952)
Hardik Khariwal (22B3954)
Aditya Singh (22B3964)
October 2, 2024

Objective
The aim of this experiment is to design and implement a control action using the Linear
Quadratic Regulator (LQR) technique to balance the inverted pendulum in an upright
position, even under external disturbances. The specific goals for this experiment are:
• To restrict the pendulum arm vibration (α) within ±3◦ .
• To maintain the base angle oscillation (θ) within ±30◦ .

LQR Method
The LQR method is a state-space control technique used to design an optimal controller
that minimizes a cost function. The cost function balances performance and control
effort.
For a state-space system:
ẋ = Ax + Bu
where:
• x represents the system states (e.g., pendulum angle, base angle),
• u is the control input,
• A and B represent system dynamics.
The LQR finds an optimal control input u by minimizing:
Z ∞
J= (xT Qx + uT Ru) dt
0
where:
• Q penalizes state deviations from the desired state,
• R penalizes control effort.
The feedback control law is given by:
u = −Kx
where K is the optimal gain matrix calculated using the Riccati equation.

1
 Control Algorithm
We chose the following initial values for the design of the LQR:

Q = diag(10, 1, 5, 1), R = 10

Where the values of q1 , q2 , q3 , q4 and r are tuned for our specific system.
After testing, the gain matrix K was fine-tuned to improve performance. The final
values obtained for K are as follows:

Parameter Value
K1 -3.189
K2 92.86
K3 -4.16
K4 12.12

Table 1: LQR Controller Parameters

Code Implementation
Note: Insert the code used for LQR implementation in this section. The following LaTeX
code block formats the code.
Listing 1: LQR Controller Code
1
2
3 //* Include the SPI library for the arduino boards */
4 #include <SPI.h>
5

6 /* Serial rates for UART */

7 #define BAUDRATE 115200
8
9 /* SPI commands */
10 #define AMT22_NOP 0x00
11 #define AMT22_RESET 0x60
12 #define AMT22_ZERO 0x70
13
14 /* Define special ascii characters */
15 #define NEWLINE 0x0A
16 #define TAB 0x09
17

18 /* We will use these define macros so we can write code once compatible with 12 or 14
bit encoders */
19 #define RES12 12
20 #define RES14 14
21
22 /* SPI pins */
23 #define ENC_0 2
24 #define ENC_1 3
25 #define SPI_MOSI 51
26 #define SPI_MISO 50
27 #define SPI_SCLK 52
28

2
29

30
31 #define motorPin1 12
32 #define motorPin2 13
33
34 int alpha_raw;
35 int theta_raw;
36 float alpha_rad;
37 float theta_rad;
38
39 float theta_prev = 0;
40 float alpha_prev = 0;
41 float theta_dot;
42 float alpha_dot;
43
44 int t_old = 0;
45 int t_new = 0;
46 int dt = 0;
47

48
49
50 //Q = diag(10,1,5,1) and R=10
51 //Working Test 2
52 float K_theta = -3.1623;//-3.1623,-2.5623
53 float K_alpha = 92.84756;//92.84756
54 float K_theta_dot = -4.0991504;//-3.8991504
55 float K_alpha_dot = 12.003874;//12.003874
56
57 float control_signal;
58 int motor_voltage = 0;
59 float scaling_factor = 2.8;
60 void setup()
61 {
62 //Set the modes for the SPI IO
63 pinMode(SPI_SCLK, OUTPUT);
64 pinMode(SPI_MOSI, OUTPUT);
65 pinMode(SPI_MISO, INPUT);
66 pinMode(ENC_0, OUTPUT);
67 pinMode(ENC_1, OUTPUT);
68
69 //Initialize the UART serial connection for debugging
70 Serial.begin(BAUDRATE);
71

72 //Get the CS line high which is the default inactive state

73 digitalWrite(ENC_0, HIGH);
74 digitalWrite(ENC_1, HIGH);
75 SPI.setClockDivider(SPI_CLOCK_DIV32); // 500 kHz
76 //start SPI bus
77 SPI.begin();
78 }
79
80 void loop()
81 {
82 delay(5);
83

84 // Read alpha (pendulum 1 angle) using SPI

85 alpha_raw = getPositionSPI(ENC_0, RES14);
86 alpha_rad = convertToRadians(alpha_raw);

3
 87

88 // Read theta (pendulum 2 angle) using SPI

89 theta_raw = getPositionSPI(ENC_1, RES14);
90 theta_rad = convertToRadians(theta_raw);
91
92 t_new = millis();
93 dt = t_new - t_old;
94
95 // Calculate angular velocities (x. = dx/dt)
96 theta_dot = (theta_rad - theta_prev)*1000 / dt;
97 alpha_dot = (alpha_rad - alpha_prev)*1000 / dt;
98
99 // Calculate control signal using LQR (u = -kx)
100 control_signal = -(K_theta * theta_rad + K_alpha * (alpha_rad-177) + K_theta_dot *
theta_dot + K_alpha_dot * alpha_dot);
101
102
103 float duty_cycle = control_signal / 905.0;
104 float c = 1;
105 if (duty_cycle >=c) {
106 duty_cycle = c;
107
108 } else if (duty_cycle <= -c) {
109 duty_cycle = -c;
110 }
111
112 if (duty_cycle > 0) {
113 analogWrite(motorPin2, duty_cycle * 255);
114 analogWrite(motorPin1, 0);
115 } else {
116 analogWrite(motorPin1, -duty_cycle * 255);
117 analogWrite(motorPin2, 0);
118 }
119
120 // Save current angles for the next iteration
121 theta_prev = theta_rad;
122 alpha_prev = alpha_rad;
123 t_old = t_new;
124 Serial.print("Theta: ");
125 Serial.print(theta_rad, DEC);
126 Serial.write(NEWLINE);
127 Serial.print(" Alpha: ");
128 Serial.print(alpha_rad, DEC);
129 Serial.write(NEWLINE);
130 Serial.print(" Control Signal: ");
131 Serial.println( duty_cycle * 255);
132 //Serial.println( control_signal);
133 }
134

135 float convertToRadians(int raw_value) {

136 float angle_deg = (raw_value) * (360.0 / 16384);
137 return angle_deg;
138 }
139
140 uint16_t getPositionSPI(uint8_t encoder, uint8_t resolution)
141 {
142 uint16_t currentPosition; //16-bit response from encoder
143 bool binaryArray[16]; //after receiving the position we will populate this array

4
 and use it for calculating the checksum
144
145 //get first byte which is the high byte, shift it 8 bits. don’t release line for
the first byte
146 currentPosition = spiWriteRead(AMT22_NOP, encoder, false) << 8;
147
148 //this is the time required between bytes as specified in the datasheet.
149 //We will implement that time delay here, however the arduino is not the fastest
device so the delay
150 //is likely inherantly there already
151 delayMicroseconds(3);
152
153 //OR the low byte with the currentPosition variable. release line after second byte
154 currentPosition |= spiWriteRead(AMT22_NOP, encoder, true);
155
156 //run through the 16 bits of position and put each bit into a slot in the array so
we can do the checksum calculation
157 for(int i = 0; i < 16; i++) binaryArray[i] = (0x01) & (currentPosition >> (i));
158

159 //using the equation on the datasheet we can calculate the checksums and then make
sure they match what the encoder sent
160 if ((binaryArray[15] == !(binaryArray[13] ^ binaryArray[11] ^ binaryArray[9] ^
binaryArray[7] ^ binaryArray[5] ^ binaryArray[3] ^ binaryArray[1]))
161 && (binaryArray[14] == !(binaryArray[12] ^ binaryArray[10] ^ binaryArray[8]
^ binaryArray[6] ^ binaryArray[4] ^ binaryArray[2] ^ binaryArray[0])))
162 {
163 //we got back a good position, so just mask away the checkbits
164 currentPosition &= 0x3FFF;
165 }
166 else
167 {
168 currentPosition = 0xFFFF; //bad position
169 }
170
171 //If the resolution is 12-bits, and wasn’t 0xFFFF, then shift position, otherwise
do nothing
172 if ((resolution == RES12) && (currentPosition != 0xFFFF)) currentPosition =
currentPosition >> 2;
173
174 return currentPosition;
175 }
176
177 uint8_t spiWriteRead(uint8_t sendByte, uint8_t encoder, uint8_t releaseLine)
178 {
179 //holder for the received over SPI
180 uint8_t data;
181
182 //set cs low, cs may already be low but there’s no issue calling it again except
for extra time
183 setCSLine(encoder ,LOW);
184
185 //There is a minimum time requirement after CS goes low before data can be clocked
out of the encoder.
186 //We will implement that time delay here, however the arduino is not the fastest
device so the delay
187 //is likely inherantly there already
188 delayMicroseconds(3);
189

5
190 //send the command
191 data = SPI.transfer(sendByte);
192 delayMicroseconds(3); //There is also a minimum time after clocking that CS should
remain asserted before we release it
193 setCSLine(encoder, releaseLine); //if releaseLine is high set it high else it stays
low
194

195 return data;

196 }
197
198 /*
199 * This function sets the state of the SPI line. It isn’t necessary but makes the
code more readable than having digitalWrite everywhere
200 * This function takes the pin number of the desired device as an input
201 */
202 void setCSLine (uint8_t encoder, uint8_t csLine)
203 {
204 digitalWrite(encoder, csLine);
205 }
206
207 void setZeroSPI(uint8_t encoder)
208 {
209 spiWriteRead(AMT22_NOP, encoder, false);
210
211 //this is the time required between bytes as specified in the datasheet.
212 //We will implement that time delay here, however the arduino is not the fastest
device so the delay
213 //is likely inherantly there already
214 delayMicroseconds(3);
215
216 spiWriteRead(AMT22_ZERO, encoder, true);
217 delay(250); //250 second delay to allow the encoder to reset
218 }
219
220
221 void resetAMT22(uint8_t encoder)
222 {
223 spiWriteRead(AMT22_NOP, encoder, false);
224
225 //this is the time required between bytes as specified in the datasheet.
226 //We will implement that time delay here, however the arduino is not the fastest
device so the delay
227 //is likely inherantly there already
228 delayMicroseconds(3);
229
230 spiWriteRead(AMT22_RESET, encoder, true);
231
232 delay(250); //250 second delay to allow the encoder to start back up
233 }

Graphs
Below are the two graphs representing the pendulum’s performance:

6
Results
The experiment yielded the following results:

In Degrees Min Max

θ -17.93 14.87
α -2.95 2.98

Table 2: Results of Pendulum Oscillations

• The pendulum arm vibration (α) was restricted to within ±3◦ .

• The base angle oscillation (θ) was maintained within ±30◦ .

7
Challenges Faced
During the experiment, we encountered the following challenges:
1. Tuning Complexity: Finding the right combination of Q and R was challenging.
Improper tuning resulted in either instability or slow response.
2. System Nonlinearity: Our system’s nonlinearity caused difficulties in controlling
the pendulum during large swings.
3. Sensor Noise: The encoder feedback was noisy, affecting state estimation accu-
racy.
4. Actuator Delays: The motor driving the pendulum base exhibited delays in
response, contributing to oscillations.

Observations and Inferences

From the experiment, we observed:
1. The system’s response was highly sensitive to changes in K values. Careful tuning
was necessary for stability.
2. The control system managed to recover from external disturbances, indicating ro-
bustness in the LQR design.
3. Noise in encoder readings reduced precision, suggesting future improvements using
noise filtering techniques.
4. Base vibrations were present, but overall, the system managed to keep the pendulum
balanced within the specified limits.