Feedback Control of a Rotary Driven

Inverted Pendulum (a.k.a. Pendubot)

ECE 572 Term Project

Abdullah Al-Refai

Kevin Bowman

John Eisenbraun

April 2011
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Abstract
The inverted pendulum represents an entire class of control problems known as balance systems. The
properties of balance systems and all manner of solutions to controlling such systems (i.e. balancing a
pendulum in the upright position) are fundamental concepts covered in numerous control systems and
control theory textbooks. The inverted pendulum problem is often presented as a “cart pendulum”
problem, in which a pendulum is attached to a moving cart by means of a frictionless pivot. Other
mechanisms, such as the rotary control we investigate here, are less common. A closely related problem
is the controlled movement, or anti-sway, of a hanging pendulum (e.g. cranes lifting and moving heavy
objects).

In this report, we present our implementation of a system to balance an inverted pendulum by means of
a rotary actuator (i.e. a motor). This system is often called a “Pendubot”. Of primary interest are:
a) the derivation of a mathematical model that encompasses the entire system (motor and
pendulum arms),
b) the computer modeling and simulation we constructed for experimentation prior to the
availability of a physical system, and
c) the challenges we faced in making the transition to the “real world” physical system.

Much has been written about deriving equations of motion for a forced dual pendulum but our survey
of the literature regarding Pendubots and other motor-based balancing systems failed to produce a
single description or example that includes the actuator (motor) in the analysis of the system. This is
perplexing because the universally accepted method of derivation results in equations of motion that
declare torque to be the controlling force; yet, motors are typically controlled by an applied voltage,
which has a non-linear relationship to torque. In this report, we explain how we extend the
mathematical model to include the motor in the system dynamics.

Accurately modeling even the simplest of non-linear dynamic systems is difficult. If the model does not
closely represent conditions and relationships found in the real world, it is of little practical use. Yet, the
use of computer models in designing control systems has advantages – not the least of which is the
ability to experiment with various aspects of the system when no physical system is immediately
available. In this report, we show the incremental and iterative development of a simulation model that
aided us in the design of the Pendubot control.

Finally, we discuss some of the practical matters we encountered while making the transition from an
ideal simulation environment to a physical electro-mechanical prototype. In particular, we discuss issues
with the mechanical design, fabrication, and construction of the pendulum arms and the limitations and
constraints imposed by a high level model-based and inherently non-real-time computing platform.

Page 2 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Table of Contents
Acknowledgments......................................................................................................................................... 5
Introduction .................................................................................................................................................. 6
Objectives ..................................................................................................................................................... 7
Summary of Results ...................................................................................................................................... 8
Significant Milestones ............................................................................................................................... 8
Conclusions ................................................................................................................................................. 12
Recommendations ...................................................................................................................................... 13
Discussions .................................................................................................................................................. 14
Dual pendulum visualization ................................................................................................................... 14
Modeling and simulation of the (non-linear) unforced pendulum system ............................................ 15
The Math Model ................................................................................................................................. 15
Simulink model of the dual pendulum system ................................................................................... 16
Linearization about balance points ..................................................................................................... 16
Modeling a DC motor in Simulink ........................................................................................................... 17
Modeling and simulation of the pendulum system under direct torque control................................... 19
Modeling and simulation of the entire plant (pendulums plus motor) .................................................. 19
Design of the feedback control ............................................................................................................... 19
Simulating the system under control...................................................................................................... 21
Implement the physical system .............................................................................................................. 23
Interfacing the motor and encoders with MATLAB ................................................................................ 24
The physical system under control ......................................................................................................... 25
References .................................................................................................................................................. 26
Bibliography ................................................................................................................................................ 26
Appendix A – Derivation of non-linear equations of motion...................................................................... 27
Appendix B – Derivation of the seventh order state space model ............................................................. 34
Appendix C – Program Listings.................................................................................................................... 39
Appendix D – Data Sheets ........................................................................................................................... 48

List of Figures
Figure 1 - System schematic ......................................................................................................................... 7

Page 3 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Figure 2 – Visualization window in a MATLAB GUI ....................................................................................... 9

Figure 3 - Simulink model of non-linear dual pendulum dynamics .............................................................. 9
Figure 4 - Simulink model of a DC motor .................................................................................................... 10
Figure 5 - Simulink model for simulation .................................................................................................... 10
Figure 6 - Response plots from LQR control design .................................................................................... 11
Figure 7 - Physical prototype ...................................................................................................................... 11
Figure 8 - MATLAB m-file “DispArms.m” function to display the pendulum arms ..................................... 14
Figure 9 - MATLAB m-file “ArmCoords.m” helper function ........................................................................ 14
Figure 10 - Visualization of the dual pendulum system .............................................................................. 15
Figure 11 - Free body diagram of the pendulum arms ............................................................................... 15
Figure 12 - Simulink subsystem for pendulum dynamics ........................................................................... 16
Figure 13 - Simulink motor subsystem........................................................................................................ 18
Figure 14 - Motor model with friction and gearbox efficiency ................................................................... 19
Figure 15 – m-file to compute LQR control gain factors ............................................................................. 20
Figure 16 - Swing up and balance control blocks........................................................................................ 22
Figure 17 - LQR control ............................................................................................................................... 22
Figure 18 - Partial linearization control ...................................................................................................... 23
Figure 19 – function to set the motor command voltage and direction .................................................... 24

List of Tables
Table 1 – Project Goals ................................................................................................................................. 8

Page 4 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Acknowledgments
The beam-balancing concept that is the main focus of this project was chosen from a list of suggestions
provided by Dr. Cheok. In place of the machine vision method for determining beam angles, we chose to
use rotary encoders. Other components of the system were employed as suggested.

Dr. Cheok and Sami Oweis selected and ordered the motor (with integrated encoder), H-bridge, and
DAQ hardware. Pete Wilson, the engineering department machinist, fabricated the pendulum arms.
John Eisenbraun assembled and tested the H-Bridge kit.

We would like to thank Dr. Cheok for his many suggestions and for his help in formulating a
comprehensive state space model. We also recognize Sami Oweis, who spent many hours in the lab with
us and other groups.

Magna Electronics generously loaned us the TMS320 “Piccolo” development kit.

MATLAB and Simulink are registered trademarks of The Mathworks, Inc.

Page 5 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Introduction
Examples of inverted pendulums exist everywhere. The last decade has seen the introduction of several
self-balancing personal vehicles like the Segway® Personal Transporter. Using technology developed for
the ASIMO robot, Honda has created a self-balancing unicycle[1] . Every human being is himself an
example of an inverted pendulum, balancing the torso on two legs consisting of several joints having
several degrees of freedom.

The inverted pendulum is inherently unstable; to keep the pendulum upright the forces at the bottom of
the pendulum must be constantly adjusted. The failure of a Trident 2 missile launched from a submarine
during a test in 1989 is a testament to the susceptibility of such systems to external disturbances. The
missile had been successful in several land-based launches but spiraled out of control after breaking
through the surface of the water from an underwater launch[2]. This really is rocket science!

Complex systems are naturally non-linear. The academic study of inverted pendulums and solutions for
their controlled balancing leads to an understanding of concepts common to non-linear systems in
general. These concepts include linearization about equilibrium points; control strategies, including
strategies for guiding a system between discontinuous equilibrium points; and systems involving
multiple disciplines (e.g. mechanical, electrical, software), to name a few. In this project, we investigate
modeling a complete system consisting of a double pendulum and a motor, the design of a control
system to balance the pendulum in different configurations (including anti-sway), and a “swing-up”
strategy for moving the pendulum from rest to a balanced position.

Page 6 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Objectives
The overall objective of this project is to apply control systems theory and the microcomputer-based
tools presented in class to demonstrate the controlled balancing of a “beam”. We place heavy emphasis
on the use of modeling and simulation and the challenge of real-time control in an inherently non-real-
time development environment.

24 V DC
Motor power cable
H-Bridge Amplifier

Encoder 1 (on motor) Motor Direction

to H-Bridge
Motor
Motor Voltage
to H-Bridge

Motor Current
from H-Bridge

Encoder Inputs Analog Input/Output

Digital Inputs/Outputs
USB

National
Instruments DAQ

Slack in encoder cable to

Encoder 2
allow for ±360° rotation of
arm from down position

Computer running
control software

Figure 1 - System schematic

The following table lists the goals we wished to achieve and the criteria for their success:

# Goal Success Criteria

1 Dual pendulum visualization Demonstrate the visualization in all possible angle
combinations using a test “clock” pattern.
2 Modeling and simulation of the (non- Observe that the movement of the pendulum arms
linear) unforced pendulum system “released” from various non-static positions behaves
in a reasonable manner (does it look right?).
3 Modeling a DC Motor in Simulink Motor responds to voltage and load torque inputs,
produces torque and speed outputs.
4 Modeling and simulation of the Visualized pendulum responds to applied torque.
pendulum system under direct torque
control
5 Modeling and simulation of the entire With the motor in the loop, the visualized pendulum
plant (pendulums plus motor) responds to voltage commands.

Page 7 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

# Goal Success Criteria

6 Design of the feedback control Analysis of the system’s reaction to initial conditions
and pulse inputs shows convergence within reasonable
response times.
7 Ultimately, we could not reconcile the Visualize the simulated anti-sway control and
compliance factor (we didn’t have a value balancing of the inverted pendulum
for it and had no way to determine the
value experimentally), so we looked for
other methods of integrating the motor
into the system.
Simulating the system under control
8 Implement the physical system All the mechanical parts work as designed. The motor
and H-bridge respond to test signals.
9 Interfacing the motor and encoders with Pendulum arm angles and rates of rotation display
MATLAB properly in real time within a running Simulink model.
10 The physical system under control Demonstrate anti-sway and beam balancing.
Table 1 – Project Goals

Summary of Results
We were successful in applying control theory to design a beam-balancing control system. Simulated
models were built in Simulink. Simulations of an LQR full state feedback control system showed that it
was effective at balancing an inverted pendulum. Practicalities and limitations of the physical prototype
prevented us from achieving the balance of a real inverted pendulum. We were, however, able to
demonstrate the use of the control in an anti-sway application.

Significant Milestones
Pendulum Visualization
The ability to see the pendulums in action was indispensible during simulation. While scope plots and
other collected data provided more detail for close analysis, visualization is a quick and intuitive
indication of the system’s behavior.

Page 8 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Figure 2 – Visualization window in a MATLAB GUI

Non-linear system dynamics model

We implemented the pendulum’s non-linear equations of motion in a Simulink block. It was tested by
applying various input signals (torque) and observing the output (the pendulum angles) in the
visualization window.

Figure 3 - Simulink model of non-linear dual pendulum dynamics

Motor Model
We developed a Simulink model of a DC motor and assigned the various parameters according to the
physical motor’s datasheet. The output variable for our system in torque (speed or angle are more
common outputs).

Page 9 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Figure 4 - Simulink model of a DC motor

Simulation model of entire plant

We combined the separate motor and pendulum dynamics models into a single Simulink model for
simulation of the system. Manual switches allowed us to easily select the various control configurations.

Figure 5 - Simulink model for simulation

Feedback control design

We used MATLAB’s lsim and initial commands to tune the LQR gains. An m-file was written to
automate the this system analysis.

Page 10 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Figure 6 - Response plots from LQR control design

Physical Prototype
The amount of time we had to experiment with the physical prototype was reduced because of delays in
receiving critical components and problems manufacturing the pendulum arms. After our first few tests,
we realized that the motor would need to be mounted to a heavy base.

Figure 7 - Physical prototype

Model for real-time operation of the physical system

For real-time control of the physical system, we adapted the simulation model. The motor model block
is replaced by an interface that commands the motor via the NIDAQ. The pendulum dynamics model
block is replaced by an interface with the encoders.

Page 11 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

MATLAB interfaces (Motor, encoder)

We developed m-files to interface with the physical components of the system. The motor control uses
MATLAB libraries provided by National Instruments for the NIDAQ. Motor voltage is commanded via a
D/A channel while the direction is set with a digital output. Angle and speed information from the
encoders is communicated over a serial channel (COM port) using a simple protocol we developed. An
auxiliary processor keeps track of the angle and speed of each encoder and communicates these values
to MATLAB on demand.

Anti-sway demonstration
We were able to demonstrate the operation of the control in an anti-sway configuration. Responding to
any disturbance of the pendulum from it rest position (hanging down) the control acted to dampen the
swinging and quickly bring the pendulum back to its rest position.

Conclusions
We applied knowledge gained in class (analog & digital signal interface, system modeling, MATLAB
tools), to design a microcomputer-based control system to balance an inverted pendulum. We
addressed and solved the problem of getting accurate and timely feedback from the angle sensors.
Despite detailed modeling and simulation, “real-world” implementation required considerable
experimentation, tuning and adjustment. Given more time, the balancing the pendulum in the upright
position could be achieved. Ultimately, the anti-sway we demonstrated is quite impressive in its
performance.

Page 12 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Recommendations
The study of inverted pendulum balancing has numerous applications, notably in the fields of aerospace
and robotics (and increasingly in medical prosthetics). This project can be viewed as a test-bed for the
investigation of efficient and robust control strategies. Control of the pendulums in the apparent “dead
band” region of the motor could be improved. This would lead to the ability to balance the beam
upright. Additional functionality might include “swing-up” to a balanced position from rest and balance
while moving the controlled arm.

Page 13 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Discussions

Dual pendulum visualization

Our first task was to devise a method of visualizing the pendulum system. Seeing a graphical
representation of the pendulums enabled quick and intuitive evaluation of the behavior of the system,
especially during simulation. Using the facilities of MATLAB’s GUI commands, we created a reusable
function to display the pendulum positions, given the angle of each arm. The MATLAB command
makehgtform was especially useful for working with the rotated and translated arms of the
pendulum. A helper function calculates the end points of each arm.

%% DispArms.m
%
% Author: Kevin Bowman
%
% function: DispArms
% description: displays the two arms at the given angles on the current
% figure. Assumes the arms (patch objects) are attached
% to a transform (hgtrasform)
% inputs: h1,h2 - handle of a patch graphic object that is a child of
% a hgtransform
% theta1,theta2 - angles of the arms
function[] = DispArms(theta1,theta2)
global h1 h2

t1 = get(h1,'Parent');
t2 = get(h2,'Parent');

ys = get(h1,'YData');
L1 = ys(2);
ys = get(h2,'YData');
L2 = ys(2); % not really used - just for call to ArmCoords

[x1,y1,x2,y2] = ArmCoords(L1, theta1, L2, theta2);

X1 = makehgtform('zrotate',-theta1);
set(t1,'Matrix',X1);
X2 = makehgtform('translate',[x1 y1 0],'zrotate',-theta2);
set(t2,'Matrix',X2);
drawnow;
Figure 8 - MATLAB m-file “DispArms.m” function to display the pendulum arms

%% ArmCoords.m
% Author: Kevin Bowman
%
% Function: ArmCoords
% Input: r1,theta1, r2,theta2 - length and angle (from verticle) of each arm
% Output: x1,y1,x2,y2 - the end point of each arm, assuming first arm
% is anchored at the origin
%
function[x1,y1,x2,y2] = ArmCoords(r1, theta1, r2, theta2)
x1 = r1*sin(theta1);
y1 = r1*cos(theta1);
x2 = x1 + r2*sin(theta2);
y2 = y1 + r2*cos(theta2);
Figure 9 - MATLAB m-file “ArmCoords.m” helper function

Page 14 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Figure 10 - Visualization of the dual pendulum system

Modeling and simulation of the (non-linear) unforced pendulum system

The Math Model

mathematical description of the pendulum dynamics. The Lagrangian
is defined to be the difference
The energy-based Lagrangian (1) and the Euler-Lagrange equation (2) were used to derive a

between the total kinetic energy  and the potential energy  of the system. The Euler-Lagrange
equation is then used to derive the equations of motion.

 ,  
  



(1)


 non‐conservative forces
   
For the generalized coordinates,  , we chose the angle of each arm of the pendulum from a reference
(2)

by a motor at one end of the controlled arm, indicated by  in the figure.

position as shown in Figure 11. For this project, the only non-conservative force will be a torque applied

Figure 11 - Free body diagram of the pendulum arms

Page 15 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Using equations (1) and (2), the equations of motion (3) and (4) can be derived (see Appendix A for the
details).

 !" # $ !"$ cos % # & !$$ sin % # ' sin !


( !"$ # ($ !" cos % # (& ! $ sin % # (' sin !$
0
(3)
(4)

!" 1 ( ($ cos % & !$$ sin %  ' sin ! # 

Or, in matrix form:

* +
- .* +
!"$ $ cos %  (& ! $ sin % # (' sin !$ (5)


2 # 3$ 4$ (
2$ # 3$ 5$$
where the following lumped parameters are utilized:

1 1 %
!  !$
$
3$ 4 5$ 7 ($
3$ 4 5$ 7
0&
3$ 4 5$ 0(&
3$ 4 5$
 (  $ ($ cos$ %
/'
3 5 # 3$ 4 6 /('
3$ 65$ (6)

To create a Simulink model of the dual pendulum system, we inserted the equations for !" and !"$ in
Simulink model of the dual pendulum system

function blocks, derived speed and angle by integrating and feeding back all the resulting independent
variables. In hindsight, this may have been more effectively done in matrix form, since MATLAB is
capable of working with matrices.

theta1dd = (B1/(A1*B1 - A2*B2*cos(phi)^2)) * ( A2*B3/(2*B1) * theta1d^2 * sin(2*phi) + A2*B4/B1*sin(theta2)*cos(phi) - A3*theta2d^2*sin(phi) - A4*sin(theta1) + tau)
theta2dd = (A1/(A1*B1 - A2*B2*cos(phi)^2)) * ( A3*B2/(2*A1) * theta2d^2 * sin(2*phi) + B2/A1*(A4*sin(theta1) - tau)*cos(phi) - B3*theta1d^2*sin(phi) - B4*sin(theta2))

1
u1
[phi]
[theta1d] [theta1]
[theta1]
B1*(A2*B3/(2*B1) * u(5)^2 * sin(2*u(2)) + A2*B4/B1*sin(u(4))*cos(u(2)) - A3*u(6)^2*sin(u(2)) - A4*sin(u(3)) + u(1)) 1
[theta2] 1
theta1dd s theta1d
xo s 1
theta1dd calculation 0 theta1
[theta1d] Integrate theta1
theta1dd Integrate
[theta2d] theta1d
3
theta1d

[theta1]
A1*B1 - A2*B2*cos(u(1))^2
[theta2] phi
[phi] denominator

[theta2d] [theta2]
[phi]

[theta1]
1
A1*(A3*B2/(2*A1) * u(6)^2 * sin(2*u(2)) + B2/A1*(A4*sin(u(3)) - u(1))*cos(u(2)) - B3*u(5)^2*sin(u(2)) - B4*sin(u(4))) 1
theta2dd s theta2d
xo s 2
[theta2] 0
Integrate theta2
theta2dd calculation theta2dd Integrate
[theta1d]
theta2d
4
[theta2d]
theta2d

Figure 12 - Simulink subsystem for pendulum dynamics

The resulting pendulum dynamics block was tested by creating various initial conditions with the
pendulum in some raised position then virtually letting it fall and observing the motion in the
visualization window.

Linearization about balance points

Equation (5) is non-linear. To take advantage of state space techniques, the equations of motion need to
be stated in state space form:

Page 16 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

8
98 # :;

We chose state variables 8

<! !$ ! !$ => and linearized the equations about the chosen
balance points ?@@
<0 0 0 0= (“up-up”), ?A@
<B 0 0 0= (“down-up”), and ?AA

<B B 0 0= (“down-down”). (The down-down position is used for anti-sway control.) With the help
of MATLAB’s Symbolic Math Toolbox and MuPad notebook (see Error! Reference source not found. for
details), we derived a first-term expansion of the Taylor series evaluated at the chosen points to obtain
the linear approximations shown in equations (7), (8), and (9).

0 0 1 0 0
F 0 0 0 1 O F 0 O
E GHI JK N E N
?@@
<0 0 0 0= C 8
EH J GH 0 0N 8 # EH J GH N
HL JI JK
L JL HK JK GHL JL L JL
E HI JL N E GJL N
K K K K

0 0M
GHK JI
DHK JK GHL JL HK JK GHL JL DHK JK GHL JL M
0 0 1 0 0
(7)

F 0 0 0 1O F 0 O
E HJ N E N
?A@
<B 0 0 0= C 8
EH J IGHK J 0 0N 8 # EH J GH J N 
GHL JI JK
HK JK GHL JL
E HI JL N E N
K K L L K K L L

0 0M
GHK JI JL
DHK JK GHL JL HK JK GHL JL DHK JK GHL JL M
0 0 1 0 0
(8)

F 0 0 0 1O F 0 O
E HJ N E N
?AA
<B B 0 0= C 8
EH J IGHK J 0 0N 8 # EH J GH J N 
GHL JI JK
HK JK GHL JL
E GHI JL N E GJL N
K K L L K K L L

0 0M
HK JI
DHK JK GHL JL HK JK GHL JL DHK JK GHL JL M (9)

Modeling a DC motor in Simulink

The DC motor is an electro-mechanical device that is governed by the following equations.

PQ RSTU WQ 1
Electrical equation:

 !V  PQ # XQ
 4Q 4Q 4Q (10)

YTU RZTU 1
Mechanical Equation:

!V"
 !V # PQ  
2TU 2TU 2TU [ (11)

RSTU
RS \V RZTU
RZ \V 2TU
2Q \V$ YTU
YQ \V$
where
(12)

The torque produced by the motor is proportional to the motor current:

Q
RZ PQ (13)
A gearbox acts as a torque multiplier and an angle/speed divider.

Page 17 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

!Q
\V !V

V
\V Q
1
 [

\V ]
V is the torque produced by the motor at the gearbox “output” (consider the motor side of the gearbox
(14)

as the “input”). [ is the torque applied by the load on the motor armature and ] is the torque applied
by the pendulum arms on the gearbox output.

Electrical Characterization Mechanical Characterization

[v] Motor 1
Current torque
[i] [T]
[v1]
1 Gamp 2
voltage ext torque Motor Speed
Power Vm Limit (rad/s)
1 1
Amp Gain -K- Kt Ng 1/Jeq [w]
s s
Limit Current Torque Gear Ratio Dead Zone Limit No-Load
Winding Moment
to Stall Current Constant Speed
Inductance of Inertia 2
shaft w out
Ra Deq

Winding
Friction
Resistance
Torque

Ke Ng

Back emf Gear Ratio1

[v]
[v1]

[i]

[T ]

[w]
Scope

Figure 13 - Simulink motor subsystem

This model does not take into consideration frictional forces within the motor (other than viscous

of the gearbox affects the torque output. The efficiency ^ is a factor in all torque terms, as in (15), but
damping) nor the gearbox efficiency. A value for motor friction is given on the datasheet. The efficiency

does not affect speed. An improved model is shown in Figure 14.

V
^\V Q

RZTU
^RZ \V (15)

Page 18 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Figure 14 - Motor model with friction and gearbox efficiency

Modeling and simulation of the pendulum system under direct torque control
We approached the development of the system incrementally. This allowed us to gain confidence in
each new block as they were added to the system and reduced uncertainty about the location of bugs
when new problems arose. Before inserting the motor model into our simulation, we exercised the
pendulum dynamics block by directly applying various torque control signals

Modeling and simulation of the entire plant (pendulums plus motor)

Having tested the motor and pendulum dynamics blocks independently, we added the motor to the
simulation. We applied various voltage commands to the motor and watched the visualization of the
pendulums as the motor produced torque on the pendulums.

Design of the feedback control

We chose to implement direct state feedback with an LQR control. LQR is an “optimal” control strategy
that is easily tuned by adjusting gains on the state variables (weights in the Q matrix). We wrote the
following m-file to calculate the gains (K matrix).

% GetLQR_KValues
% author: John Eisenbraun
%
% Generate gains for LQR control for a given angle
% ------------------------------------------------------------------------
function [K_0_0 K_PI_0 K_PI_PI] = GetLQR_KValues()

global Continuous Ts;

[A1, A2, ~, A4, B1, B2, ~, B4] = GetMathModelConstants;

% Generate K values for

% theta1 theta2
% 0 0
% PI 0
% PI PI
% See TermProject.mn for derivation of constants

Page 19 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

% Generate K for 0, 0

Denominator = (A1*B1 - A2*B2);

A = [0 0 1 0;
0 0 0 1;
-((A4*B1)/Denominator) (A2*B4)/Denominator 0 0;
(A4*B2)/Denominator -((A1*B4)/Denominator) 0 0];

B = [0 0 B1/(Denominator) -(B2/(Denominator))]';

C = [1 1 1 1];

D = [0];

SYS = ss(A, B, C, D);

Q = [1 0 0 0;
0 1 0 0;
0 0 1 0;
0 0 0 1];

R = [10];

if Continuous
[K_0_0, ~, ~] = lqr(SYS, Q, R);
else
[K_0_0, ~, ~] = dlqr(SYS, Q, R, Ts);
end

% Generate K for PI, 0

A = [0 0 1 0;
0 0 0 1;
(A4*B1)/Denominator -((A2*B4)/Denominator) 0 0;
(A4*B2)/Denominator -((A1*B4)/Denominator) 0 0];

B = [0 0 B1/Denominator B2/Denominator]';

SYS = ss(A, B, C, D);

if Continuous
[K_PI_0, ~, ~] = lqr(SYS, Q, R);
else
[K_PI_0, ~, ~] = dlqr(SYS, Q, R, Ts);
end

% Generate K for PI, PI

A = [0 0 1 0;
0 0 0 1;
(A4*B1)/Denominator -((A2*B4)/Denominator) 0 0;
-((A4*B2)/Denominator) (A1*B4)/Denominator 0 0];

B = [0 0 B1/Denominator -(B2/Denominator)]';

SYS = ss(A, B, C, D);

if Continuous
[K_PI_PI, ~, ~] = lqr(SYS, Q, R);
else
[K_PI_PI, ~, ~] = dlqr(SYS, Q, R, Ts);
end

end
Figure 15 – m-file to compute LQR control gain factors

Page 20 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Alternate Strategy Considered

We experienced difficulty integrating the motor model into the control – the pendulum system itself
could be controlled with a torque control signal. Two areas of concern: 1) The control output ultimately
needs to be in terms of a voltage (input to the motor). We tried creating a conversion block that
accepted a desired torque and the current shaft speed and calculated the motor voltage needed to
produce the desired torque. 2) The load torque (the force applied to the motor shaft by the pendulum
arms) is not accurately modeled.

We addressed both concerns at once by creating a state space model of the entire plant (motor and
pendulum arms). To do this, we also needed an equation that couples the motor equations to the
pendulum equations. In consultation, Dr. Cheok suggested considering a “compliance factor” that
relates the shaft angle at the motor to the measured (or computed) shaft angle. The difference in these
two quantities creates a load torque on the motor shaft.

[
_` a!V  ! b
Where _` is a compliance factor that describes the “twistability” (torsional springiness) of the motor
(16)

shaft.

In the Simulink model, this torque is fed back into the motor subsystem as external torque.

The state variables for the entire system are 8

c! !$ ! !$ !V !V PQ d . Combining
>

equation (16) with the pendulum and motor equations, we obtain a seventh order state space
representation of the system (see Appendix B – Derivation of the seventh order state space modelfor
the derivation):

! 0 0 1 0 0 0 0 !
F O F 0
!
E $N E  0 0 0 1 0 0 0 O F! O F O
N 0
E !" N Eg  h& _` g $ 0 0 h& _` 0 0 NE  N E N
$
! 0
E !" N Eg$  h' _` g$$ 0 0 h' _` 0 0 NE N E0N
E  N E N
E $N
E 0 0 0 0 0 1 0 N E!$ N # E 0 N XQ

E !V N E _` 0 0 0 i _`
G Gljk mnjk N !V
E0N
E !V" N E ijk N E  N
i i
E!V N E N
Eef N E Gmojk Gpf N
jk jk jk

0 0 0 0 0 D [f M
D eZ M D [f [f M D PQ M (17)
Ultimately, we could not reconcile the compliance factor (we didn’t have a value for it and had no way
to determine the value experimentally), so we looked for other methods of integrating the motor into
the system.

Simulating the system under control

Matlab/Simulink was used to simulate the proposed control system. The control system was initially
written to just control balance, but was eventually expanded to control both swing up and balance with
automatic switching between the two. The following Simulink diagram shows the overall control.

Page 21 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

If
if (u1 == 0) ControlState
elseif (u1 <= 4) u1 1
else

theta1
2

else { } theta1 <theta1> theta2

<theta2> <theta1> theta1 3
theta2 elseif { } theta1
<theta1d> <theta2> theta2
Switch theta1d theta2
u <theta2d> <theta1d> theta1d
u theta2d u theta1d theta1d
1 <theta1_pos> <theta2d> theta2d 4
theta1_pos theta2d
<theta2_pos> <theta1_pos> theta1_pos theta2d
theta2_pos theta1_pos
<K> theta2_pos 5
K
Partial Linearization Control
LQR Control theta1_pos
6

theta2_pos
7
K
K
8

Figure 16 - Swing up and balance control blocks

The approach taken in defining physical constants was to use variables to define the constants in the
system and load those constants into the model as part of initialization using Matlab functions. This
approach allowed us to use estimated values before the physical model was actually built and then to
quickly and accurately switch to actual physical parameters when the Pendubot was actually built.

The following block shows the control for LQR Control.

else { }

Action Port

5
theta1_pos
7 -1
1 K
Gain1
theta1
6 1
theta2_pos u
Gain
2
theta2
3
theta1d

4
theta2d

Figure 17 - LQR control

Page 22 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Partial Linearization Control was used for swing up control during the simulation. This technique was
borrowed from Spong’s paper on swing up control [1] Damon Lavrinc. (2009, September) Tokyo
Preview: Honda unveils U3-X personal mobility device - Autoblog. [Online].
http://www.autoblog.com/2009/09/24/tokyo-preview-honda-unveils-u3-x-personal-mobility-device

[2] E. J. Dionne Jr., "Trident Missile Explodes in Test; Failure Is 2d of 3 Sea Launchings," The New York
Times, August 16, 1989.

[3]. Spong’s system used an Acrobot, but the equations are easily modified for a Pendubot.

elseif { }
5 200
v1 Action Port
theta1_pos
Kp
1
theta1
theta1
v1
3 10 (A1*B1 - A2*B2*cos(u(2))^2)/B1*u(1) 1
theta1d
theta1d phi
Kd u
Linearized function

theta1
theta1d
phi
phi A3*u(5)^2*sin(u(3)) - A2*B3/B1*u(2)^2*cos(u(3))*sin(u(3)) + A4*sin(u(1)) - A2*B4/B1*cos(u(3))*sin(u(4))
Add
2 theta2
theta2
theta2 theta2d Linearizing function

4
theta2d
theta2d

Figure 18 - Partial linearization control

Successfully implementing swing-up control (even in simulation) is a trial and error process. The control
needs to provide enough energy to swing the pendulum to a position where the LQR control is in a
linear region and the control can switch to LQR control, but not too much energy or the LQR control isn’t
stable. We were able to implement swing-up control successfully in simulation but we didn’t have
enough time to be successful in the real world.

The overall control of the system was done by the Matlab m file SetControlState.m listed in Appendix C
– Program Listings. This program is responsible for taking input from the user (in the form of switches)
and translating those into the desired control (swing-up, balance) and the desired position of the
pendulum (down-down, down-up and up-up).

Implement the physical system

Implementing the physical system was among the most frustrating parts of the project. The building of
the Pendubot was initially delayed due to sickness of the machinist. Then the original tube design of the
Pendubot did not meet our requirements. We finally were able to meet with the machinist and relayed
our requirements. Unfortunately, the first implementation of the design still would not work (the
encoder shaft was not fixed in relation to the upper arm). Finally, we received a Pendubot that would
work, although the joint wasn’t as strong as it should be. In fact the joint did fall apart several times
during testing.

Page 23 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Our approach for determining what angle the arms were in relation to the upright position was to use
encoders. One encoder was supplied on the motor we were provided with. The other encoder was
purchased and attached to the Pendubot joint.

An H-Bridge for controlling the motor was provided to our team in kit form. We assembled the H-Bridge
and after a minor issue with the assembly (the surface mount current sensing chip was not making
contact with the board), we were able to successfully calibrate the board.

Another problem with the implementation was that the base initially wasn’t as stable as it needed to be
either. We initially placed the motor on a plexiglass base, but this base didn’t prove to be strong or
heavy enough to be stable when the Pendubot went out of control.

Yet a third problem with the physical system was that the H-Bridge we were using proved to have a
defective part. After replacing the H-Bridge with a good one, we were able to control the system.

Interfacing the motor and encoders with MATLAB

The motor is controlled with a command voltage and a digital direction line via the NIDAQ. The NIDAQ is
controlled by MATLAB library functions provided by National Instruments.

% SetHBridgeVoltage.m
% Author: John Eisenbraun
%
% Function: SetHBridgeVoltage
% Input: The voltage to set the H-Bridge to
% Output: None
%
% Input is a voltage and sign. The sign sets the H-Bridge digital output
% and the magnitude goes to the voltage (after being scaled).
%
function [i] = SetHBridgeVoltage(u)

global AO AI DIO;

% Output the value and sign

if u(2) == 0 % ControlState == 0
putsample(AO, 0);
else
Direction = u(1) >= 0;
putvalue(DIO, Direction);
output = abs(u(1)) * 5/24;
putsample(AO, output);
% i = getsample(AI);
end
i = 0;
end
Figure 19 – function to set the motor command voltage and direction

Our initial design relied on the NIDAQ to decode the encoder quadrature signals. However, it quickly
became evident that MATLAB was not going to execute fast enough (in real time) to continuously
monitor and decode the signals. Our solution was to employ a small auxiliary processor with the sole
function of decoding quadrature signals and counting pulses. After a few false starts with a dsPIC and a
PSoC microcontroller, we successfully implemented the function on a TI TMS320 controlSTICK. The
processor counts edge transitions on the A/B encoder lines and communicates the current count when
queried via a Serial Communications Interface (routed through a virtual COM port on the PC). In

Page 24 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

addition, the processor computes the present speed of each encoder (in counts per millisecond). A
source code listing can be found in Appendix C.

Although this solution worked, it introduced undesirable delays between the physical world and the
control due to Matlab’s slowness in processing data from the serial port. This delay almost certainly
affected our ability to control the Pendubot in the upright position.

The physical system under control

After overcoming the difficulties detailed above, we were able to control the physical system in the
“down-down” position using our designed control. The “Q” values of the LQR control needed to be
modified for the physical system but after some tuning the control behaved very similarly to how it did
during simulation.

A video of the successful control is submitted as part of the project deliverables.

Unfortunately there was insufficient time to successfully show balance control in either (down-up or up-
up) positions. Control was attempted (once) in the down-up position, but we were unable to maintain
control. We did not attempt to do swing-up, although we did test that the basic swing-up control
(partial linearization) did work.

Page 25 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

References
[1] Damon Lavrinc. (2009, September) Tokyo Preview: Honda unveils U3-X personal mobility device -
Autoblog. [Online]. http://www.autoblog.com/2009/09/24/tokyo-preview-honda-unveils-u3-x-
personal-mobility-device

[2] E. J. Dionne Jr., "Trident Missile Explodes in Test; Failure Is 2d of 3 Sea Launchings," The New York
Times, August 16, 1989.

[3] Mark W. Spong, “Partial Feedback Linearization of Underactuated Mechanical Systems”

Bibliography
Bryan Kappa, State Space Control Design of a Dual Inverted Pendulum,
www.bryankappa.com/Resume/Kappa_MSME_Paper.pdf , retrieved 2/10/2011

Dufour Aurelie, et. al., Final Report Pendubot, www.e.kth.se/~aurelied/Finalreport.pdf, retrieved

2/11/2011

ece.ut.ac.ir/SMRL/members/Sharifi/Inverted%20Pendulum.pdf, retrieved 2/11/2011

Gene F. Franklin, J. David Powell, and Abbas Emami-Naeini, Feedback Control of Dynamic Systems, 3rd
ed.: Addison-Wesley Publishing Company, 1994.

Karl Johan °Aström and Richard M. Murray, Feedback Systems: An Introduction for Scientists and
Engineers, Electronic Edition ed.: Princeton University Press, 2009.

Page 26 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Appendix A – Derivation of non-linear equations of motion

3 mass of link P
Nomenclature

2 moment of inertia of link P

4 length of link P
5 distance from pivot to center of mass for link P
! angle from vertical of link P
 external torque applied (by control)

A system’s dynamics can be described by Langrange’s equations:

 ,  
  



(A1)


 non‐conservative torques
   
where   , P
1. . t is a convenient set of parameters,  is the total kinetic energy of the system, and
(A2)

 is the total potential energy.
 ,   is called the Lagrangian of the system. For the pendubot, we
choose the parameters  to be u! ; !$ w.

To find the Lagrangian, we will first determine the kinetic and potential energies.

Kinetic energy
The kinetic energy of each of the links is:

1

2 ! $
2
1 1
(A3)

$
2$ !$$ # 3$ |z
{|$ |$
2 2 (A4)

where

{|$

z <4 sin ! # 5$ sin !$ }{| # 4 cos ! # 5$ cos !$ ~{|=



4 ! cos ! # 5$ !$ cos !$ }{| # a4 ! sin !  5$ !$ sin !$ b~
{| (A5)
is the translational velocity of the center of mass of link 2. Then,

{|$ |$
4$ ! $ cos $ ! # 5$$ !$$ cos$ !$ # 2! !$ 4 5$ cos ! cos !$ # 4$ ! $ sin$ ! # 5$$ !$$ sin$ !$
|z
# 2! !$ 4 5$ sin ! sin !$

4 ! # 5$$ !$$ # 24 5$ ! !$ cos!  !$
$ $
(A6)
The total kinetic energy is

1 1 1

2 ! $ # 2$ !$$ # 3$ c4$ ! $ # 5$$ !$$ # 24 5$ ! !$ cos!  !$ d
2 2 2 (A7)

Potential Energy
Potential energy, due to gravity, is:


3 65 cos ! # 3$ 64 cos ! # 5$ cos !$ (A8)

Page 27 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

3 5 # 3$ 4 6 cos ! # 3$ 65$ cos !$

Lagrange’s Equations
Subtracting the potential from the kinetic energy gives us the Lagrangian.


1 1 1


2 ! $ # 2$ !$$ # 3$ c4$ ! $ # 5$$ !$$ # 24 5$ ! !$ cos!  !$ d  3 5 # 3$ 4 6 cos !
2 2 2
 3$ 65$ cos !$ (A9)

We now compute the derivatives according to equation (A2) to form the equations of motion for the
system.






 ! !



(A10)


0
 !$ !$ (A11)




c2 ! # 3$ 4$ ! # 3$ 4 5$ !$ cos!  !$ d
 ! 

2 !" # 3$ 4$ !" # 3$ 4 5$ !" cos!  !$  3$ 4 5$ !$ a!  !$ b sin!  !$

2 # 3$ 4$ !" # 3$ 4 5$ c!" cos!  !$  !$ a!  !$ b sin!  !$ d

2 # 3$ 4$ !" # 3$ 4 5$ c!" cos!  !$  ! !$ sin!  !$ # !$$ sin!  !$ d


(A12)

3$ 4 5$ ! !$ sin!  !$ # 3 5 # 3$ 4 6 sin !
!



(A13)


2 # 3$ 4$ !"
 ! !
# 3$ 4 5$ c!" cos!  !$  ! !$ sin!  !$ # !$$ sin!  !$ d
# 3$ 4 5$ ! !$ sin!  !$  3 5 # 3$ 4 6 sin !

$ !"
2 # 3$ 4 # 3$ 4 5$ c!"$ cos!  !$ # !$$ sin!  !$ d  3 5 # 3$ 4 6 sin !

(A14)
(A15)




c2$ !$ # 3$ 5$$ !$ # 3$ 4 5$ ! cos!  !$ d

 !$ 

2$ !"$ # 3$ 5$$ !"$ # 3$ 4 5$ !" cos!  !$  3$ 4 5$ ! a!  !$ b sin!  !$

2$ # 3$ 5$$ !"$ # 3$ 4 5$ c!" cos!  !$  ! a!  !$ b sin!  !$ d

2$ # 3$ 5$$ !"$ # 3$ 4 5$ c!" cos!  !$ # ! !$ sin!  !$  ! $ sin!  !$ d


(A16)

3$ 4 5$ ! !$ sin!  !$ # 3$ 65$ sin !$
!$ (A17)

Page 28 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun





2$ # 3$ 5$$ !"$
 !$ !$
# 3$ 4 5$ c!" cos!  !$  ! $ sin!  !$ # ! !$ sin!  !$ d
 3$ 4 5$ ! !$ sin!  !$  3$ 65$ sin !$
0

2$ # 3$ 5$$ !"$ # 3$ 4 5$ c!" cos!  !$  ! $ sin!  !$ d  3$ 65$ sin !$

0
(A18)
(A19)

To simplify equations (A15) and (A19), we introduce lumped parameters:

 !" # $ !"$ cos % # & !$$ sin % # ' sin !


( !"$ # ($ !" cos % # (& ! $ sin % # (' sin !$
0
(A20)
(A21)


2 # 3$ 4$ (
2$ # 3$ 5$$
where

1 1
$
3$ 4 5$ 7 ($
3$ 4 5$ 7 %
!  !$
0&
3$ 4 5$ 0(&
3$ 4 5$
/'
3 5 # 3$ 4 6 /('
3$ 65$ (A22)

To take advantage of a state space representation of the system, we can isolate !" and !"$ in each of the

a! , !$ , ! , !$ b.
equations (20) and (21); substitute and rearrange to obtain two functions of the four state variables

1
!"
a$ !"$ cos %  & !$$ sin %  ' sin ! # b

1
(A23)

!"$
a($ !" cos %  (& ! $ sin %  (' sin !$ b
( (A24)
Substituting equation (A23) into (A20) we get:

1
 !" # $ a($ !" cos %  (& ! $ sin %  (' sin !$ b cos % # & !$$ sin % # ' sin !

(

$ ($ $ (& $ $ ('
 !"  !" cos$ %  ! sin % cos %  sin !$ cos % # & !$$ sin % # ' sin !

( ( (

$ ($ $ (& $ $ ('
  cos$ %€ !"
! sin % cos % # sin !$ cos %  & !$$ sin %  ' sin ! # 
( ( (

( $ (& $  (
!"
- ! sin 2% # $ ' sin !$ cos %  & !$$ sin %  ' sin ! # .
 (  $ ($ cos$ % 2( ( (A25)

Likewise, substituting equation (A22) into (A21):

1
( !"$ # ($ a$ !"$ cos %  & !$$ sin %  ' sin ! # b cos % # (& ! $ sin % # (' sin !$
0


Page 29 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

$ ($ & ($ $ ' ($ ($
( !"$  !"$ cos$ %  !$ sin % cos %  sin ! cos % #  cos % # (& ! $ sin %
   
# (' sin !$
0

$ ($ & ($ $ ($
(  cos $ %€ !"$
!$ sin 2% # ' sin !   cos %  (& ! $ sin %  (' sin !$
 2 

 & ($ $ ($
!"$
- !$ sin 2% # ' sin !   cos %  (& ! $ sin %  (' sin !$ .
 (  $ ($ cos % 2
$  (A26)

Equations (A20) & (A21) in matrix form:

 ($ cos % !"  ! $ sin %  ' sin ! # 

- .* +
* & $$ +
$ cos % ( !"$ (& ! sin % # (' sin !$ (A27)

!"  ($ cos % G & !$$ sin %  ' sin ! # 

* +
- . * +
"!$ $ cos % ( (& ! $ sin % # (' sin !$ (A28)

!" 1 ( ($ cos % & !$$ sin %  ' sin ! # 

* +
- .* +
!"$  (  $ ($ cos % $ cos %
$  (& ! $ sin % # (' sin !$ (A29)

We choose state variables 8

<! !$ ! !$ => and rewite (A29) in state space form:
Linearization

8
8 # ‚ (A30)

!
where

F O
E !$ N
8
E
ƒ a! , !$ , ! , !$ b
!" N
E N
Dƒ$ a! , !$ , ! , !$ b
!"$ M
0
(A31)

F 0 O
E N
‚
E„…K N 
E„…L N
„†

D „† M (A32)

In general, a linear approximation of 8 near a point ? can be obtained by an expansion of the first
term of a Taylor series:

8 ‡ ? # ˆ7|? ‰ 8  ? (A33)

where

Page 30 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

0 0 1 0
F 0 0 0 1 O
E„… „…K N
ˆ7|?
E „Š K
„…K „…K
„ŠL „ŠK „ŠL N
E„…L „…L N
K
„…L „…L
D „ŠK „ŠL „ŠK „ŠL M? (A34)

Matlab Symbolic Math Toolbox (MuPAD) solutions for ˆ partial derivatives:

File: TermProject.mn
Author: Kevin Bowman
Description: derivation of gradient of motion equations for pendubot
Set Preferences for MuPAD
Pref::alias(TRUE);
Pref::abbreviateOutput(FALSE);

Set up some aliases for easier readability

alias(`&phi;` = x_1 - x_2);
alias(`&theta;_1` = x_1);
alias(`&theta;_2` = x_2);
alias(`&omega;_1` = x_3);
alias(`&omega;_2` = x_4);

theta1dd & theta2dd

[x1, x2, x3, x4] = [theta1, theta2, theta1d, theta2d]
f_1 := (x_1,x_2,x_3,x_4) -> B_1/(A_1*B_1 - A_2*B_2*cos(x_1-x_2)^2) *
(A_2*B_2/(2*B_1)*x_3^2*sin(2*x_1-2*x_2) + A_2*B_4/B_1 * sin(x_2)*cos(x_1-x_2) -
A_2*x_4^2*sin(x_1-x_2)-A_4*sin(x_1)+`&tau;`);
f_2 := (x_1,x_2,x_3,x_4) -> A_1/(A_1*B_1 - A_2*B_2*cos(x_1-x_2)^2) *
(A_3*B_2/(2*A_1)*x_4^2*sin(2*x_1-2*x_2) + B_2/A_1 * (A_4*sin(x_1) - `&tau;`)*cos(x_1-x_2) -
B_3*x_3^2*sin(x_1-x_2) - B_4*sin(x_2));

Partial derivatives of theta1dd w.r.t. each state variable [x1, x2, x3, x4]
df_1_1 := (x_1,x_2,x_3,x_4) --> diff(f_1(x_1,x_2,x_3,x_4),x_1);
df_1_2 := (x_1,x_2,x_3,x_4) --> diff(f_1(x_1,x_2,x_3,x_4),x_2);
df_1_3 := (x_1,x_2,x_3,x_4) --> diff(f_1(x_1,x_2,x_3,x_4),x_3);
df_1_4 := (x_1,x_2,x_3,x_4) --> diff(f_1(x_1,x_2,x_3,x_4),x_4);
df_2_1 := (x_1,x_2,x_3,x_4) --> diff(f_2(x_1,x_2,x_3,x_4),x_1);
df_2_2 := (x_1,x_2,x_3,x_4) --> diff(f_2(x_1,x_2,x_3,x_4),x_2);
df_2_3 := (x_1,x_2,x_3,x_4) --> diff(f_2(x_1,x_2,x_3,x_4),x_3);
df_2_4 := (x_1,x_2,x_3,x_4) --> diff(f_2(x_1,x_2,x_3,x_4),x_4);

Page 31 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Partial derivatives w.r.t. tau

dB1:= (x_1,x_2,x_3,x_4) --> diff(diff(f_1(x_1,x_2,x_3,x_4),`&tau;`));
dB2:= (x_1,x_2,x_3,x_4) --> diff(diff(f_2(x_1,x_2,x_3,x_4),`&tau;`));

Partial derivatives evaluated at equilibrium point (0, 0, 0, 0)

df_1_1(0,0,0,0), df_1_2(0,0,0,0), df_1_3(0,0,0,0), df_1_4(0,0,0,0)

df_2_1(0,0,0,0), df_2_2(0,0,0,0), df_2_3(0,0,0,0), df_2_4(0,0,0,0)

dB1(0,0,0,0), dB2(0,0,0,0)

Partial derivatives evaluated at equilibrium point (pi, 0, 0, 0)

df_1_1(PI,0,0,0), df_1_2(PI,0,0,0), df_1_3(PI,0,0,0), df_1_4(PI,0,0,0)

df_2_1(PI,0,0,0),df_2_2(PI,0,0,0), df_2_3(PI,0,0,0), df_2_4(PI,0,0,0)

dB1(PI,0,0,0), dB2(PI,0,0,0)

Partial derivatives evaluated at equilibrium point (pi, pi, 0, 0)

df_1_1(PI,PI,0,0), df_1_2(PI,PI,0,0), df_1_3(PI,PI,0,0), df_1_4(PI,PI,0,0)

df_2_1(PI,PI,0,0),df_2_2(PI,PI,0,0), df_2_3(PI,PI,0,0), df_2_4(PI,PI,0,0)

dB1(PI,PI,0,0), dB2(PI,PI,0,0)

Page 32 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Linearization around the upright balanced position ?

0 0 0 0 :

0 0 1 0 0
F 0 0 0 1O F 0 O
E GHI JK N E N
?@@
<0 0 0 0= C 8
E 0 0N 8 # EH J GH J N 
HL JI JK
HK JK GHL JL HK JK GHL JL
E HI JL N E GJL N
K K L L

0 0M
GHK JI
DHK JK GHL JL HK JK GHL JL DHK JK GHL JL M (A35)

Linearization around the “down-up” balanced position ?

B 0 0 0

0 0 1 0 0
F 0 0 0 1O F 0 O
E HI JK N E N
?A@
<B 0 0 0= C 8
EH J GH J 0 0N 8 # EH J GH J N 
GHL JI JK
HK JK GHL JL
E HI JL N E N
K K L L K K L L

0 0M
GHK JI JL
DHK JK GHL JL HK JK GHL JL DHK JK GHL JL M (A36)

Linearization around the “down-up” balanced position ?

B B 0 0

0 0 1 0 0
F 0 0 0 1 O F 0 O
E HJ N E N
?AA
<B B 0 0= C 8
EH J IGHK J 0 0N 8 # EH J GH J N 
GHL JI JK
HK JK GHL JL
E GHI JL N E GJL N
K K L L K K L L

0 0M
HK JI
DHK JK GHL JL HK JK GHL JL DHK JK GHL JL M (A37)

Page 33 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Appendix B – Derivation of the seventh order state space model

Motor equations:

PQ RSTU WQ 1

 !V  PQ # XQ
 4Q 4Q 4Q
YTU RZTU 1
(B1)

!V"
 ! # P  
2TU V 2TU Q 2TU T‹Z
T‹Z
_` a!V  ! b
(B2)

YTU RZTU 1 1
(B3)

!V"
 !V # PQ  _` !V # _!
2TU 2TU 2TU 2TU ` (B4)
In matrix form:

!V 0 1 0 0
F mnjk O !V 0
0 
ljk
"
Œ !V 
E ijk N !  #  0  XQ #   T‹Z
G

E mojk Gpf N
Žjk
V ijk
P
D0 G [f 0
ef
[f M
Q [f
eZ (B5)
Linearized system dynamics, from Appendix A:

! 0 0 1 0 F! O 0
F O
E!$ N 0 0 0 1 E!$ N 0
E!" N
Œg g $ 0

0 E! N
# Œ  T‹Z
h&
E N
D!"$ M g$ g$$ 0 0 D!$ M h' (B6)

g
H g
H h&
H
where
GHK JI HL JI JK
$
K JK GHL JL K K GHL JL
J K K GHL JL
J

g$
g
h'

HI JL GHK JI GJL
HK JK GHL JL $ HK JK GHL JL HK JK GHL JL (B7)

States:

!$ ! !$ !V !V PQ d
>
8
c! (B8)
Compliance factor

!V  !
_`
!
(B9)

F! O
E $N
E! N
T‹Z
<_` 0 0 0 _` 0 0= E!$ N
a_` ! # _` !V b
E!V N
E N
E!V N
D PQ M (B10)
Combining with (B7)), we get:

Page 34 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

! 0 0 1 0 0 0 0
F! O F 0 0 0 1 0 0 0 O F! O 0 0
F0O F0O
E $N E N E!$ N
g g 0 0 0 0 0 N E! N EE 0 NN E N
E! N E E h& N
$
g
E $ g 0 0 0 0 0 NE  N E0N
E!$ N
h
N !$ # E 0 N XQ # EE ' NN a_` ! # _` !V b
$$
E 0 0 0 0 0 1 0
E!V N E 0 E
mnjk N !V N 0
0 0 0 0 E N E G N
Gljk
E N E ijk N E N 0
E!V N E!V N E N Eijk N
ijk
E Gpf N
D M D0M
D 0 0 0 0 0
Gmojk
D PQ M [f [f M D PQ M
[ f (B11)
Expanding the final term:

0 0 0 1 0 0 0 0 !
F0O F 0 F O
0 0 1 0 0 0O E!$ N
E N E N 
E h& N Eh& _` 0 0 0 h& _` 0 0N E! N
E h' N a_` ! # _` !V b
Eh' _` 0 0 0 h' _` 0 0N E!$ N
E0N E 0 0 0 0 0 1 0N E!V N
EG N E _ 0 0 0 i _` 0 0N E! N
G
Eijk N E ijk ` N E VN
D0M D 0 0 0M D PQ M
jk
0 0 0 0
Which is of the form <·=x x and can be directly added to the first <·=x
x matrix:
(B12)

! 0 0 1 0 0 0 0
F O F
 0 0 0 1 0 0 0 O F! O 0
!
E $N E ! F0O
NE N
g  h& _` g $ 0 0 h& _` 0 0 N E! N E 0 N
$
E! N E "
E !" N E $ g  h' _` g$$ 0 0 h' _` 0 0 N E  N EE 0 NN
E N E $

0 0 0 0 0 1 0 N !$ # E 0 N XQ

E !V N E mnjk N E!V N
_` 0 0 0 _` E0N
G Gljk
E !V" N E ijk ijk N E  N
i ijk
E!V N E N
Eef N E Gpf N
jk

0 0 0 0 0
Gmojk
D [f M
D M D [f [f M D PQ M
eZ (B13)

Reducing the seventh order system

We can define a new seventh order system as follows:

! 0 0 1 0 0 0 0 ! 0
F O
! F 0 0 0 1 0 0 0 O F O F O
E N !$
N Eg  h& _` g $ NE N E0N
$
E !" 0 0 h _ 0 0 NE ! N E0N
N Eg$  h' _` g$$
& `
E "$ 0 0 h' _` 0 0 NE
! E N E0N
E N

E 0 0 0 0 0 1 0 NE !$ N # E N XQ
E a!V  ! b N E
e
Gljk mnjk N E !V  ! N E0N
E eL N E g g $ i 0  i “h& € _` i
eZ Gljk
N E N E N
EeZ L a!V  ! bN E a!  ! bN E 0 N
i e
Gmojk Gpf N EeZ V
jk jk jk jk

E N 0 0 0 0 0
ef D [f M D PQ M D[fM
D M
[f
eZ
Assuming a!V  ! b is small, eZ a!V  ! b is also small, and assuming PQ gt eZ are nearly zero, we can
e ef
(B14)

reduce the seventh order system to a fourth order system by partitioning (B14) and applying the
following algorithm:

Page 35 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

X A A $ X B
* +
- . - . # - . XQ
X$ A$ A$$ X$ B$

According to our assumptions, we set X$
c!V !V " >
d
0 and write the system as two equations:
(B15)
—˜f
—n

X
A X # A $ X$ # B XQ
0
A$ X # A$$ X$ # B$ XQ
(B16)

Solving (B17) for X$ we get:

(B17)

X$
™$$ G A
A$ X  B$ XQ (B18)
Substituting (B18) back into (B16):

X
A X # A $ ™$$ G A
A$ X  B$ XQ # B XQ (B19)
This can then be reduced to the form:

X
M'›' X # N'› XQ (B20)
The following MATLAB MuPAD notebook script was used to determine the solution to our specific case:

File: 7thOrderSS.mn
Reduce 7th order state space to 4th order
A_1_1:= matrix([[0,0,1,0],[0,0,0,1],[a_1_1-b_3*C_s,a_1_2,0,0],[a_2_1-
b_4*C_s,a_2_2,0,0]])

A_1_2 := matrix([[0,0,0],[0,0,0],[b_3*C_s,0,0],[b_4*C_s,0,0]])

A_2_1:= matrix([[0,0,0,0],[-a_1_1,-a_1_2,-D/J,0],[0,0,0,0]])

A_2_2:= matrix([[0,1,0],[-(1/J+b_3)*C_s,-D/J,K_t/J],[0,-K_b/L,-R/L]])

X_1:=matrix([`&theta;_1`,`&theta;_2`,Symbol::accentDot(`&theta;_1`),Sy
mbol::accentDot(`&theta;_2`)])

Page 36 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

X_2:=matrix([`&theta;_g`-`&theta;_1`,Symbol::accentDot(`&theta;_g`)-
Symbol::accentDot(`&theta;_1`),i])

B_1:=matrix([0,0,0,0])

B_2:= matrix([0,0,1/L])

F:= Xd_1 = hold(A_1_1*X_1+A_1_2*X_2+B_1*v_a)

G:= 0 = hold(A_2_1*X_1+A_2_2*X_2+B_2*v_a)

X2:= 1/A_2_2 * (-A_2_1*X_1 - B_2*v_a)

X1dot:= A_1_1 * X_1 + A_1_2*X2 + B_1*va

Page 37 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

simplify(X1dot)

By collecting terms and simplifying, X can be simplified (MuPad was unable to do this, for some
reason). The result can be re-written in expanded matrix form:

0 0 1 0 ! 0
!
F O F OF O F O
E!$ N E 0 0 0 1N E! N E 0 N
E N
E N
0 EE! NN # EEpfijk Sj “ N XQ
$ Gm S
E!" N E ijk S “  _` h&
QKK QKL Gljk S njk
ijk S “ ijk S “ N N
E N E N E N E Gmnjk oI N
D!"$ M Dg$  _` h'  ijk S “ g$$  ijk SKK“ I 0 0 D!$ M Dpfijk Sj “
QKK SI i Q S
M M
jk  (B21)

Page 38 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Appendix C – Program Listings

Encoder counter/reader program (Piccolo)
//----------------------------------------------------------------------------------
// FILE: Encoder_Reader.C
//
// AUTHOR: Kevin Bowman
//----------------------------------------------------------------------------------

#include "PeripheralHeaderIncludes.h"

#define QUAD_LOOPBACK_TEST 0

#define ENC1_CPR (Uint16)(39400u) // 500 * 19.7 * 4

#define ENC2_CPR (Uint16)(8192u)

#define S00 0
#define S01 1
#define S10 2
#define S11 3

#if QUAD_LOOPBACK_TEST == 1
#define EOT 'O'
#define ENQ 'E'
#define DEL 'Z'
#else
#define EOT 0x04
#define ENQ 0x05
#define DEL 0x7F
#endif

#define ABS(x) ((x)<0 ? -(x) : (x))

typedef unsigned char Uint8;

typedef struct {
Uint8 state;
Uint16 cnt;
Uint16 CPR;
Uint16 cnt_per_ms;
Uint16 prevCnt;
} Enc_T;

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// FUNCTION PROTOTYPES
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

void DeviceInit(void);
void InitFlash();
Uint8 GetEnc1State(void);
Uint8 GetEnc2State(void);
void NextState(Enc_T* e, Uint8 s);
void Incr(Enc_T* e);
void Decr(Enc_T* e);
void CalcSpeed(Enc_T* e);
int16 SCIGetChar(void);

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// VARIABLE DECLARATIONS - GENERAL
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

// Used for running BackGround in flash and the ISR in RAM

extern Uint16 *RamfuncsLoadStart, *RamfuncsLoadEnd, *RamfuncsRunStart;

//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// MAIN CODE - starts here
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void main(void)
{
Enc_T enc1;
Enc_T enc2;
Uint8 state;
int16 rxChar;
Uint16 msCounter = 0;
#if QUAD_LOOPBACK_TEST == 1
Uint8 flipFlop = 1;
Uint8 digit;

Page 39 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Uint16 remainder;
#endif

//=================================
// INITIALISATION - General
//=================================

DeviceInit(); // Device Life support & GPIO mux settings

// Only used if running from FLASH

// Note that the variable FLASH is defined by the compiler (-d FLASH)
#ifdef FLASH
// Copy time critical code and Flash setup code to RAM
// The RamfuncsLoadStart, RamfuncsLoadEnd, and RamfuncsRunStart
// symbols are created by the linker. Refer to the linker files.
MemCopy(&RamfuncsLoadStart, &RamfuncsLoadEnd, &RamfuncsRunStart);

// Call Flash Initialization to setup flash waitstates

// This function must reside in RAM
InitFlash(); // Call the flash wrapper init function
#endif //(FLASH)

// Initialise Period of Cpu Timers

// Timer period definitions found in PeripheralHeaderIncludes.h
#if QUAD_LOOPBACK_TEST == 1
CpuTimer0Regs.PRD.all = mSec50; // 500ms * 2(# of LED states) = 1s blink rate
#else
CpuTimer0Regs.PRD.all = mSec1; // 500ms * 2(# of LED states) = 1s blink rate
#endif
// CpuTimer1Regs.PRD.all = mSec1000;
// CpuTimer2Regs.PRD.all = mSec5000;

//=================================
// INTERRUPT INITIALISATION (not needed in this example)
// (best to run this section after other initialisation)
//=================================

// Enable Peripheral, global Ints and higher priority real-time debug events:
// IER |= M_INT3;
// EINT; // Enable Global interrupt INTM
// ERTM; // Enable Global realtime interrupt DBGM

//=================================
// Forever LOOP
//=================================

// init encoder structs

enc1.cnt = 0;
enc1.CPR = ENC1_CPR;
enc1.state = GetEnc1State();
enc1.prevCnt = 0;
enc1.cnt_per_ms = 0;
enc2.cnt = 0;
enc2.CPR = ENC2_CPR;
enc2.state = GetEnc2State();
enc2.prevCnt = 0;
enc2.cnt_per_ms = 0;

// init serial port

SciaRegs.SCICTL1.bit.SWRESET = 1;
SciaRegs.SCIHBAUD = 0;
SciaRegs.SCILBAUD = 48; // 48 = 38400, baud 0.3% error
SciaRegs.SCICCR.bit.SCICHAR = 7; // 8 data bits
SciaRegs.SCICCR.bit.STOPBITS = 0; // 1 stop bit
SciaRegs.SCIFFTX.bit.SCIFFENA = 1; // enable FIFO operation
SciaRegs.SCICTL1.bit.TXENA = 1;
SciaRegs.SCICTL1.bit.RXENA = 1;

for(;;) //infinite loop

{
state = GetEnc1State();
if(state != enc1.state)
{
NextState(&enc1, state);
}
state = GetEnc2State();
if(state != enc2.state)

Page 40 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

{
NextState(&enc2, state);
}

rxChar = SCIGetChar();
switch(rxChar)
{
case ENQ:
#if QUAD_LOOPBACK_TEST == 1
digit = (Uint8)(enc1.cnt/1000);
remainder = enc1.cnt % 1000;
SciaRegs.SCITXBUF = digit + '0';

digit = (Uint8)(remainder/100);
remainder = remainder % 100;
SciaRegs.SCITXBUF = digit + '0';

digit = (Uint8)(remainder/10);
remainder = remainder % 10;
SciaRegs.SCITXBUF = digit + '0';

digit = (Uint8)(remainder);
SciaRegs.SCITXBUF = digit + '0';
#else
SciaRegs.SCITXBUF = enc1.cnt >> 8;
SciaRegs.SCITXBUF = enc1.cnt & 0x00FF;
SciaRegs.SCITXBUF = enc2.cnt >> 8;
SciaRegs.SCITXBUF = enc2.cnt & 0x00FF;
#endif
break;

case EOT:
SciaRegs.SCITXBUF = enc1.cnt_per_ms >> 8;
SciaRegs.SCITXBUF = enc1.cnt_per_ms & 0x00FF;
SciaRegs.SCITXBUF = enc2.cnt_per_ms >> 8;
SciaRegs.SCITXBUF = enc2.cnt_per_ms & 0x00FF;
break;

case DEL:
enc1.cnt = 0;
enc1.prevCnt = 0;
enc1.cnt_per_ms = 0;
enc2.cnt = 0;
enc2.prevCnt = 0;
enc2.cnt_per_ms = 0;
break;

default:
break;
}

if(CpuTimer0Regs.TCR.bit.TIF == 1)
{
CpuTimer0Regs.TCR.bit.TIF = 1; // clear flag

// every ms, get counter diffs for speed

CalcSpeed(&enc1);
CalcSpeed(&enc2);

msCounter++;
if(msCounter >= 500)
{
msCounter = 0;
//-----------------------------------------------------------
GpioDataRegs.GPBTOGGLE.bit.GPIO34 = 1; //Toggle GPIO34 (LD2)
//-----------------------------------------------------------
#if QUAD_LOOPBACK_TEST == 1
// Generate Test signal
if(flipFlop)
{
GpioDataRegs.GPATOGGLE.bit.GPIO3 = 1;
flipFlop = 0;
}
else
{
GpioDataRegs.GPATOGGLE.bit.GPIO4 = 1;
flipFlop = 1;
}
#endif

Page 41 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

}
}

#if QUAD_LOOPBACK_TEST == 1
GpioDataRegs.GPATOGGLE.bit.GPIO0 = 1; // loop speed profiling
#endif
}
} //END MAIN CODE

Uint8 GetEnc1State(void)
{
Uint8 A,B;

A = GpioDataRegs.GPADAT.bit.GPIO16;
B = GpioDataRegs.GPADAT.bit.GPIO17;
return (A<<1) | B;
}

Uint8 GetEnc2State(void)
{
Uint8 A,B;

A = GpioDataRegs.GPADAT.bit.GPIO18;
B = GpioDataRegs.GPADAT.bit.GPIO19;
return (A<<1) | B;
}

void NextState(Enc_T* e, Uint8 s)

{
Uint8 state = e->state;

// note: error transitions are not handled - stay in same state

switch(state)
{
case S00:
switch(s)
{
case S01:
Incr(e);
e->state = s;
break;
case S10:
Decr(e);
e->state = s;
break;
}
break;

case S01:
switch(s)
{
case S11:
Incr(e);
e->state = s;
break;
case S00:
Decr(e);
e->state = s;
break;
}
break;

case S10:
switch(s)
{
case S00:
Incr(e);
e->state = s;
break;
case S11:
Decr(e);
e->state = s;
break;
}
break;

case S11:
switch(s)
{
case S10:

Page 42 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Incr(e);
e->state = s;
break;
case S01:
Decr(e);
e->state = s;
break;
}
break;
}
}

void Incr(Enc_T* e)
{
if((e->CPR-1) == e->cnt)
{
e->cnt = 0;
}
else
{
e->cnt++;
}
}

void Decr(Enc_T* e)
{
if(0 == e->cnt)
{
e->cnt = e->CPR - 1;
}
else
{
e->cnt--;
}
}

void CalcSpeed(Enc_T *e) // assume called every 1ms

{
int32 temp;

temp = ((int32)(e->cnt) - (int32)(e->prevCnt));

if(ABS(temp) > e->CPR / 2)
{
// we wrapped
if(temp < 0)
temp += e->CPR;
else
temp -= e->CPR;
}

e->cnt_per_ms = (Uint16)ABS(temp);
e->prevCnt = e->cnt;
}

int16 SCIGetChar(void)
{
int16 c;

if(SciaRegs.SCIFFRX.bit.RXFFST > 0)
{
c = (int16)(SciaRegs.SCIRXBUF.all & 0x00FF);
SciaRegs.SCIFFRX.bit.RXFIFORESET = 0;
SciaRegs.SCIFFRX.bit.RXFIFORESET = 1;

}
else
{
c = -1;
}

return c;
}

Page 43 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

SetControlState.m
% SetControlState
% author: John Eisenbraun
%
% Set the control state and outputs based on the input
% If control state is >= 3 then LQR control output is enabled
% ------------------------------------------------------------------------
function [out] = SetControlState(u)

global Ts;
global ControlState;
global BalanceControl;
global Theta1Pos Theta2Pos;
global K_0_0 K_PI_0 K_PI_PI;
global UpUpTrajectory DownUpTrajectory;

persistent Theta1PosState;
persistent PosStep PosCount PosTimer TimTimer;
persistent StepSize NumSteps Amplitude;
persistent K;
persistent TrajectoryIndex TrajectoryCount;
persistent OperatingMode Start PendulumPosition

% Pendulum arm may have wrapped, so allow for that

Theta1Sign = sign(u(4));
if Theta1Sign == 0, Theta1Sign = 1; end
Theta2Sign = sign(u(5));
if Theta2Sign == 0, Theta2Sign = 1; end

% The offset is an even 2pi value equal to the number of times that the
% arm has wrapped. The offset may be a positive or negative number.

theta1 = rem(u(4), 2*pi);

theta2 = rem(u(5), 2*pi);

Theta1PosOffset = u(4) - theta1;

Theta2PosOffset = u(5) - theta2;

% Use absolute values to make comparisons easier

theta1 = abs(theta1);
theta2 = abs(theta2);
theta1d = abs(u(6));
theta2d = abs(u(7));

% If out of control, then stop the simulation

if theta1d > 20 || theta2d > 20

StopSimulation = 1;
else
StopSimulation = 0;
end

% Check if operating mode was changed. If so, then start over

if OperatingMode ~= u(2), ControlState = 0; end

OperatingMode = u(2);

Start = u(1);
if OperatingMode == 1

% Balance mode

if Start == 1

% If the pendulum position was changed, then start over.

if PendulumPosition ~= u(3), ControlState = 0; end

PendulumPosition = u(3);

switch BalanceControl
case 0
BalanceControl = 1;
K = K_PI_PI; % Default
case 1
switch PendulumPosition

Page 44 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

case 1 % Up-Up
Theta1Pos = Theta1PosOffset;
Theta2Pos = Theta2PosOffset;
K = K_0_0;
BalanceControl = 2;
case 2 % Down-Up
Theta1Pos = Theta1PosOffset + Theta1Sign * pi;
Theta2Pos = Theta2PosOffset;
K = K_PI_0;
BalanceControl = 3;
case 3 % Down-Down
Theta1Pos = Theta1PosOffset + Theta1Sign * pi;
Theta2Pos = Theta2PosOffset + Theta2Sign * pi;
K = K_PI_PI;
BalanceControl = 4;
end
case 2 % Up-Up
if theta2 < .3 && theta2d < 2 && theta1 < .28
ControlState = 5; % Balance
end
case 3 % Down-Up
if theta2 < .3 && theta2d < 7 && ...
abs(pi - theta1) < .28
ControlState = 5; % Balance
end
case 4 % Down-Down
if abs(pi - theta1) < .7 && abs(pi - theta2) < .7 && ...
theta1d < 4 && theta2d < 4
ControlState = 5; % Balance
end
case 5 % Balance
end
else
K = K_PI_PI; % Default
ControlState = 0; % No control
end

else % Swing Up

Start = u(1);
PendulumPosition = u(3);

% Check if stop requested and not already stopping

if Start == 2 && ControlState ~= 6 && OperatingMode ~= 1

ControlState = 3; % Swing down
end

switch ControlState
% No control
case 0
if Start == 1, ControlState = 1; end
K = K_PI_PI; % Default
% Starting up
case 1
ControlState = 2;
PosTimer = 0;
TimTimer = 0;
TrajectoryIndex = 1;
PosCount = 1;
PosStep = 0;
if PendulumPosition == 1 % Up-Up
Theta1PosState = 1;
[TrajectoryCount n] = size(UpUpTrajectory);
StepSize = UpUpTrajectory(1,3);
NumSteps = UpUpTrajectory(1,4);
Amplitude = UpUpTrajectory(1,5);
elseif PendulumPosition == 2 % Down-Up
Theta1PosState = 2;
[TrajectoryCount n] = size(DownUpTrajectory);
StepSize = DownUpTrajectory(1,3);
NumSteps = DownUpTrajectory(1,4);
Amplitude = DownUpTrajectory(1,5);
else % Down-Down
ControlState = 0;
end
% Swing-up
case 2
switch Theta1PosState
case 1 % Up-Up
PosTimer = PosTimer + 1;

Page 45 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

if PosTimer * Ts >= .01

% Formula to figure out the
% next position. A sine wave gets
% generated.
if (TrajectoryIndex == 2) && (PosCount == 53)
Theta1PosState = 3;
end
Theta1Pos = ...
Theta1PosOffset + pi + Amplitude * sin(PosStep);
if PosCount == NumSteps
PosStep = 0;
PosCount = 0;
if TrajectoryIndex < TrajectoryCount
TrajectoryIndex = TrajectoryIndex + 1;
StepSize = UpUpTrajectory(TrajectoryIndex,3);
NumSteps = UpUpTrajectory(TrajectoryIndex,4);
Amplitude = UpUpTrajectory(TrajectoryIndex,5);
end
else
PosStep = PosStep + StepSize;
end
PosCount = PosCount + 1;
PosTimer = 0;
end

case 2 % Down-Up
PosTimer = PosTimer + 1;
if PosTimer * Ts >= .01
% Formula to figure out the
% next position. A sine wave gets
% generated.
if (TrajectoryIndex == 2) && (PosCount == 53)
Theta1PosState = 4;
end
Theta1Pos = ...
Theta1PosOffset + pi + Amplitude * sin(PosStep);
if PosCount == NumSteps
PosStep = 0;
PosCount = 0;
if TrajectoryIndex < TrajectoryCount
TrajectoryIndex = TrajectoryIndex + 1;
StepSize = DownUpTrajectory(TrajectoryIndex,3);
NumSteps = DownUpTrajectory(TrajectoryIndex,4);
Amplitude = DownUpTrajectory(TrajectoryIndex,5);
end
else
PosStep = PosStep + StepSize;
end
PosCount = PosCount + 1;
PosTimer = 0;
end
case 3 % Up-Up go to balance point
if theta2 < .3 && theta2d < 2
ControlState = 5; %Balance
Theta1Pos = Theta1PosOffset;
Theta2Pos = Theta2PosOffset;
K = K_0_0;
else
TimTimer = TimTimer + 1;
if TimTimer * Ts >= 2.0
ControlState = 3; % Didn't go to balance
else
Distance = Theta1PosOffset - Theta1Pos;
if abs(Distance) > 0
PosTimer = PosTimer + 1;
if PosTimer * Ts >= .01
Theta1Pos = ...
Theta1Pos + sign(Distance) * .02;
PosTimer = 0;
end
else
Theta1Pos = Theta1PosOffset;
end
end
end
case 4 % Down-Up go to balance point
if theta2 < .2 && theta2d < 5 && ...
abs(pi - theta1) < .2
ControlState = 5; %Balance
Theta1Pos = Theta1PosOffset + Theta1Sign * pi;
Theta2Pos = Theta2PosOffset;

Page 46 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

K = K_PI_0;
else
TimTimer = TimTimer + 1;
if TimTimer * Ts >= 2.0
ControlState = 3; % Didn't go to balance
else
Theta1PosEnd = ...
Theta1PosOffset + Theta1Sign * pi; % Down-Up
Distance = Theta1PosEnd - Theta1Pos;
if abs(Distance) > 0
PosTimer = PosTimer + 1;
if PosTimer * Ts >= .01
Theta1Pos = ...
Theta1Pos + sign(Distance) * .02;
PosTimer = 0;
end
else
Theta1Pos = Theta1PosEnd;
end
end
end
end
% Swing down (Partial linearization control)
case 3
Theta1PosEnd = Theta1PosOffset + Theta1Sign * pi; % Down-Down

Distance = Theta1PosEnd - Theta1Pos;

if abs(Distance) > 0
PosTimer = PosTimer + 1;
if PosTimer * Ts >= .01
Theta1Pos = Theta1Pos + sign(Distance) * .02;
PosTimer = 0;
end
else
Theta1Pos = Theta1PosEnd;
end
if abs(pi - theta1) < .7 && abs(pi - theta2) < .7 && ...
theta1d < 4 && theta2d < 4
Theta1Pos = Theta1PosEnd;
Theta2Pos = Theta2PosOffset + Theta2Sign * pi;
ControlState = 6; % Goto LQR control
K = K_PI_PI;
end
% Balance LQR
case 5
% Check for out of control condition
if theta2 > .5 || theta2d > 20
ControlState = 3;
end
% Swing down LQR
case 6
if abs(pi - theta1) < .01 && abs(pi - theta2) < .01
ControlState = 0; % Stopped
end
end
end
out = [ControlState Theta1Pos Theta2Pos K StopSimulation];
end

Page 47 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Appendix D – Data Sheets

• Pittman GM9236S021 LoCog® DC Servo Gearmotor with rotary encoder
• Dataspeed H-bridge Schematic
• CUI ATM-10X Incremental Encoder
• National Instruments NI USB-6008/6009 data acquisition (DAQ) device (excerpts)
• Texas Instruments Piccolo TMS320F2802X/2803X 32-bit Microcontrollers

Page 48 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 49 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 50 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 51 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 52 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 53 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 54 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 55 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 56 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 57 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 58 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 59 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 60 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 61 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 62 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 63 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 64 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 65 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 66 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 67 of 68
ECE 572 Term Project Al-Refai, Bowman, Eisenbraun

Page 68 of 68