An inverted pendulum is a pendulum that has its center of mass above its pivot point. It is often implemented with the pivot point mounted on a cart that can move horizontally and may be called a cart-and-pole system. A normal pendulum is always stable since the pendulum hangs downward, whereas the inverted pendulum is inherently unstable and trivially underactuated (because the number of actuators is less than the degrees of freedom). For these reasons, the inverted pendulum has become one of the most important classical problems of control engineering. Since the 1950s, the inverted-pendulum benchmark, especially the cart version, has been used for the teaching and understanding of the use of linear-feedback control theory to stabilize an open-loop unstable system. The objectives of this project are to: • Focus on hardware and software troubleshooting and enhancement of an inverted-pendulum system controlled by a DSP28355 microprocessor and CCSv7.1 software. • Use the swing-up strategy to move the pendulum into the unstable upward position (‘saddle’). The cart/pole system employs linear bearings for back-and-forward motion. The motor shaft has a pinion gear that rides on a track permitting the cart to move in a linear fashion. Both rack and pinion are made of hardened steel and mesh with a tight tolerance. The rack-and-pinion mechanism eliminates undesirable effects found in belt-driven and free-wheel systems, such as slippage or belt stretching, ensuring consistent and continuous traction. • The motor shaft is coupled to a high-resolution optical encoder that accurately measures the position of the cart. The angle of the pendulum is also measured by an optical encoder, and the system employs an LQR controller to stabilize the pendulum rod at the unstable-equilibrium position. • Addition of real-time status reporting and visualization of the system. For the project, the Quanser High Frequency Linear Cart (HFLC) was used. The HFLC system consists of a precisely machined solid aluminum cart driven by a high-power 3-phase brushless DC motor. The cart slides along two high-precision, ground-hardened stainless steel guide rails, allowing for multiple turns and continuous measurement over the entire range of motion. Our team implemented a control strategy that consists of a linear stabilizing LQR controller, proportional-integral swing-up control, and a supervisory coordinator that determines the control strategy (LQR or swing-up) to be used at any given time. The function of the linear stabilizer is to stabilize the system when it is in the vicinity of the unstable equilibrium. When the pendulum is in its natural state (straight-down stable-equilibrium node), the swing-up controller provides the cart/pendulum system with adequate energy to move the pendulum to the unstable equilibrium inside the “region of attraction” in which the linearized LQR controller is functional.

1. MEM 459/540
Control Applications of DSP Microprocessors
Spring 2017
Final Project Report
Troubleshooting and Enhancement of Inverted Pendulum System
Controlled by DSP28355 Microprocessor and CCSv7.1 Software
Thomas Templin (tt464@drexel.edu)
Feiyang Xue (fx28@drexel.edu)
Akinsola Aladesanmi (aja348@drexel.edu)
David Mensah (dom24@drexel.edu)

2. 1
Introduction
An inverted pendulum is a pendulum that has its center of mass above its pivot point. It is often
implemented with the pivot point mounted on a cart that can move horizontally and may be
called a cart-and-pole system. A normal pendulum is always stable since the pendulum hangs
downward, whereas the inverted pendulum is inherently unstable and trivially underactuated
(because the number of actuators is less than the degrees of freedom). For these reasons, the
inverted pendulum has become one of the most important classical problems of control
engineering. Since the 1950s, the inverted-pendulum benchmark, especially the cart version, has
been used for the teaching and understanding of the use of linear-feedback control theory to
stabilize an open-loop unstable system [1].
The objectives of this project are to:
 Focus on hardware and software troubleshooting and enhancement of an inverted-pendulum
system controlled by a DSP28355 microprocessor and CCSv7.1 software.
 Use the swing-up strategy to move the pendulum into the unstable upward position
(‘saddle’). The cart/pole system employs linear bearings for back-and-forward motion. The
motor shaft has a pinion gear that rides on a track permitting the cart to move in a linear
fashion. Both rack and pinion are made of hardened steel and mesh with a tight tolerance.
The rack-and-pinion mechanism eliminates undesirable effects found in belt-driven and free-
wheel systems, such as slippage or belt stretching, ensuring consistent and continuous
traction.
 The motor shaft is coupled to a high-resolution optical encoder that accurately measures the
position of the cart. The angle of the pendulum is also measured by an optical encoder, and
the system employs an LQR controller to stabilize the pendulum rod at the unstable-
equilibrium position.
 Addition of real-time status reporting and visualization of the system.
For the project, the Quanser High Frequency Linear Cart (HFLC) was used [2]. The HFLC
system consists of a precisely machined solid aluminum cart driven by a high-power 3-phase
brushless DC motor. The cart slides along two high-precision, ground-hardened stainless steel
guide rails, allowing for multiple turns and continuous measurement over the entire range of
motion.
Our team implemented a control strategy that consists of a linear stabilizing LQR controller,
proportional-integral swing-up control, and a supervisory coordinator that determines the control
strategy (LQR or swing-up) to be used at any given time. The function of the linear stabilizer is
to stabilize the system when it is in the vicinity of the unstable equilibrium. When the pendulum
is in its natural state (straight-down stable-equilibrium node), the swing-up controller provides
the cart/pendulum system with adequate energy to move the pendulum to the unstable
equilibrium inside the “region of attraction” in which the linearized LQR controller is functional.
Figure 1 shows a real-life example of the application of an inverted pendulum.

3. 2
Figure 1: Example of a real system that behaves like an inverted pendulum [3].
Physical Model
Using free body diagrams for the cart and the rod, the equations of motion of the cart-rod system
can be derived using the Newtonian method [4]:
Horizontal pendulum motion: 𝐻 = 𝑚(𝑦̈ − 𝑙 cos 𝜃𝜃̈ + 𝑙 sin 𝜃 𝜃̇2
) (1)
Vertical pendulum motion: 𝑚𝑔 − 𝑉 = 𝑚𝑙(sin 𝜃 𝜃̈ + cos 𝜃 𝜃̇2
) (2)
Rotational pendulum motion: 𝑙𝑉 sin 𝜃 + 𝑙𝐻 cos 𝜃 = 𝐼𝜃̈ + 𝐵 𝜃 𝜃̇ (3)
Horizontal cart motion: 𝑀𝑦̈ + 𝐵𝑦 𝑦̇ = 𝑢 − 𝐻 (4)
The variables used have the following meanings:
By viscous friction of translational motion
Bθ viscous friction of rotational motion
g acceleration due to gravity
H horizontal reaction force at the pendulum’s pivot
I the rod’s mass moment of inertia (⅓ml2
)
l length from the pivot to the rod’s center of gravity
M mass of cart
m mass of pendulum rod
u input (forcing function pushing the cart in the horizontal direction)
V vertical reaction force at the pendulum’s pivot
y horizontal position of cart
θ angular position of pendulum (top = 0º, CCW = pos.)
The reaction forces are given by:

4. 3
𝐻 = 𝑚(𝑦̈ − 𝑙 cos 𝜃 𝜃̈ + 𝑙 sin 𝜃 𝜃̇2
) (5)
𝑉 = 𝑚𝑔 − 𝑚𝑙(sin 𝜃 𝜃̈ + cos 𝜃𝜃̇2
) (6)
Substituting the reactions into (3) and (4) and rearranging results in the following two equations:
4
3
𝑚𝑙2
𝜃̈ + 𝐵 𝜃 𝜃̇ − 𝑚𝑔𝑙 sin 𝜃 − 𝑚𝑙 cos 𝜃 𝑦̈ = 0 (7)
(𝑀 + 𝑚)𝑦̈ + 𝐵𝑦 𝑦̇ + 𝑚𝑙 sin 𝜃 𝜃̇2
− 𝑚𝑙 cos 𝜃 𝜃̈ = 𝑢 (8)
These equations can also be obtained using a Lagrangian (energy-based) approach. For the
Quanser inverted-pendulum system the variable values are given as [4]: M = 1.79 (kg), m =
0.104 (kg), l = 0.3048 (m), g = 9.8 (m/s2
), Bs = 0.25 (N·s/m), Bθ = 0.02 (N·s/rad).
Choosing state variables as x1 = θ, x2 = 𝜃̇, x3 = y, and x4 = 𝑦̇, the following nonlinear state
equation 𝑥̇ = 𝑓(𝑥, 𝑢) can be formulated:
𝑥̇1 = 𝑥2 (9)
𝑥̇2 =
1
244 − 10 cos2 𝜃
[(5885 sin 𝜃 − 378.8𝜃̇ − 10 sin 𝜃 cos 𝜃 𝜃̇2
− 79.25𝑦̇ cos 𝜃) + 317 cos 𝜃 𝑢]
𝑥̇3 = 𝑥4 (11)
𝑥̇4 =
1
244 − 10 cos2 𝜃
[(−4.083 sin 𝜃𝜃̇2
− 32.2𝑦̇ + 98.5 sin 𝜃 cos 𝜃 − 6.34𝜃̇ cos 𝜃) + 128.8𝑢]
Linearized State Space Model
Jacobian linearization evaluates the Jacobian matrix
𝜕𝑓
𝜕𝑥
at the equilibrium point, which is p = x =
(0, 0, 0, 0) in our case [5]. Then the linearized system and input-coupling matrices, respectively,
can be determined as follows:
𝐴 𝑐 =
𝜕𝑓
𝜕𝑥
|
𝑥=𝑝
(13)
𝐵𝑐 =
𝜕𝑓
𝜕𝑢
|
𝑥=𝑝
(14)
These expressions are applicable in a neighborhood of equilibrium point p. Using Jacobian
linearization and matrix notation, the linearized state-space model can be written as

5. 4
𝑥̇ = [
0 1 0 0
25.15 −1.619 0 −0.3387
0 0 0 1
0.4209 −0.02709 0 −0.1376
] 𝑥 + [
0
1.355
0
0.5504
] 𝑢 (15)
In order to design a linear digital stabilizing controller, the continuous-time (CT) system has to
be converted into a discrete-time (DT) system. The transformation of a CT state equation into a
DT state equation requires the determination of the state transition matrix Φ as an intermediary
step. The state transition matrix is given by
𝛷(𝑡) = e 𝐴 𝑐 𝑡
= ℒ−1{(𝑠𝐼 − 𝐴 𝑐)−1} = ∑
(𝐴 𝑐 𝑡) 𝑘
𝑘!
,
∞
𝑘=0
(16)
where t denotes (continuous) time, ℒ-1
the inverse Laplace transform, and I an identity matrix of
appropriate dimensions. Using the state transition matrix, matrices A and B of the DT system
x[k+1] = Ax[k] + Bm[k] can be computed as [6]
𝐴 = 𝛷(𝑇) (17)
𝐵 = ∫ 𝛷(𝜏)𝑑𝜏𝐵𝑐
𝑇
0
(18)
The variable T stands for the sampling time (here, T = 0.01 s). Using this approach, we obtain the
following discretized state space equation:
𝑥[𝑘 + 1] = [
1.001 0.009926 0 −0.00001680
0.2496 0.9852 0 −0.003359
0.00002088 −0.000001239 1 0.009931
0.004175 −0.0002479 0 0.9986
] 𝑥[𝑘] + [
0.0006719
0.1344
0.0002749
0.05499
] 𝑚[𝑘]
Controller for Region in which Linearized Model is Valid
Infinite-horizon linear-quadratic-regulator optimal control theory is used to design the controller
for the region in which the linearized model is valid (“region of attraction”). The objective is to
minimize the energy of the controlled output z and the cost function (performance index) J [7].
The expressions for z and J are given by [4]
𝑧[𝑘] = [
𝑧1[𝑘]
𝑧2[𝑘]
] = [
𝐶1 𝑥[𝑘]
𝐷12 𝑚[𝑘]
], (20)
where z1[k] denotes the error to be minimized (“energy of the system”), z2[k] the control input
constraint (“energy of the controller”), and C1 and D12 state and control weighting matrices.

6. 5
𝐽 = ∑ 𝑧 𝑇[𝑘]𝑧[𝑘]
∞
𝑘=0
= ∑ [
𝐶1 𝑥[𝑘]
𝐷12 𝑚[𝑘]
]
𝑇
[
𝐶1 𝑥[𝑘]
𝐷12 𝑚[𝑘]
]
∞
𝑘=0
= ∑[𝑥 𝑇[𝑘]𝐶1
𝑇
𝑚 𝑇[𝑘]𝐷12
𝑇 ] [
𝐶1 𝑥[𝑘]
𝐷12 𝑚[𝑘]
]
∞
𝑘=0
= ∑[𝑥 𝑇[𝑘]𝐶1
𝑇
𝐶1 𝑥[𝑘] + 𝑚 𝑇[𝑘]𝐷12
𝑇
𝐷12 𝑚[𝑘]]
∞
𝑘=0
= ∑[𝑥 𝑇[𝑘]𝑄𝑥[𝑘] + 𝑚 𝑇[𝑘]𝑅𝑚[𝑘]] (21)
∞
𝑘=0
The optimization is satisfied by solving the DT algebraic Riccati equation S and determining the
state-feedback gain matrix F. The expressions for these matrices are given as follows:
𝐴 𝑇
𝑆𝐴 − 𝐴 𝑇
𝑆𝐵(𝑅 + 𝐵 𝑇
𝑆𝐵)−1
𝐵 𝑇
𝑆𝐴 + 𝑄 = 𝑆 (22)
𝐹 = (𝑅 + 𝐵 𝑇
𝑆𝐵)−1
𝐵 𝑇
𝑆𝐴 (23)
Choosing C1 = [100 50 100 50] and D12 = [30],
𝑆 = [
7.488×106
1.113×106
−2.43×106
−2.32×106
1.113×106
1.692×105
−3.45×105
−3.35×105
−2.43×106
−3.45×105
8.657×105
8.032×105
−2.33×106
−3.35×105
8.032×105
7.538×105
] (24)
𝐹 = [20.94 3.924 −2.761 −3.796] (25)
The feedback gains were scaled with the factor 6.33 to give a gain vector of F = [132.6 24.84
-17.48 -24.03]. In the source code for the microprocessor, values of 132.88, 23.715, -19.0353,
and -23.7728 are used, based on the results from a previous team [8]. The slight discrepancies
might reflect rounding errors or differences in calculation that we are not aware of. These state-
feedback gain values satisfy the control law m[k] = Fx[k].
Controller for Swing-up Control
The development of the swing-up controller considers the following physical principles [4]:
 Inertia: When the pendulum’s pivot end moves, the top end of the rod lags behind.
 Resonance: The swing amplitude increases when the driving force matches the pendulum’s
natural frequency.
 Conservation of energy: The cart’s travel direction, distance, and speed need to be adjusted to
meet energy requirements. Furthermore, the motion of the cart needs to be synchronized with
the pendulum so that energy is added rather than subtracted.
The cart also needs to stay within the rail-length limit. The length of the rail is 1.1 m [2]. A
proportional-integral (PI) controller was used for the swing-up control problem, based on the
work of a previous team [8]. The swing-up controller is functional up to a distance of 0.38 m to

7. 6
either side from the rail’s center. In other words, if the cart is not positioned within these bounds,
it first has to be moved inside this region before swing-up control can be applied.
The region of attraction of the LQR controller of the linearized model is shown in Figure 22. The
LQR controller can be employed when the rod is in region I of the diagram. Note that the exact
extent of the region of attraction does not only depend on angular position (θ), but also on
angular velocity (𝜃̇). To base the size of the region of attraction on angular position alone would
needlessly restrict the size of the region. The LQR controller can be applied when the angular
velocity of the pendulum is 𝜃̇ ∈ [−22°/𝑠, 22°/𝑠]. If the magnitude of 𝜃̇ is greater than 22
degrees per second, the controller cannot “catch” the pendulum and stabilize it in an upright
position. Instead, the controller just lets it pass and catches it in a subsequent swing, when the
angular-velocity requirement is fulfilled. Depending on angular velocity, the magnitude of the
angular position of the region of attraction lies between 20 and 50 degrees [8].
Figure 2: The various regions of the
Pendulum’s angular-position space [4].
The implementation of the digital PI speed controller is based on the trapezoidal rule for
numerical integration. In the z domain, such a controller is of the form [9]
𝐾(𝑧) =
𝑀(𝑧)
𝐸(𝑧)
= 𝐾𝑝 + 𝐾𝑖
𝑇
2
𝑧 + 1
𝑧 − 1
=
2𝐾𝑝(𝑧 − 1) + 𝐾𝑖 𝑇(𝑧 + 1)
2(𝑧 − 1)
=
2𝐾𝑝(1 − 𝑧−1) + 𝐾𝑖 𝑇(1 + 𝑧−1)
2(1 − 𝑧−1)
𝑋(𝑧)
𝑋(𝑧)
, (26)
where M, E, and X denote the z-domain control input, error, and controlled variable, respectively,
Kp the proportional gain, Ki the integral gain, and T the sampling period. The numerator is M(z) =
2 Kp (1 – z-1
) X(z) + Ki T (1 + z-1
) X(z), which corresponds in the (discrete) time domain to m[k] =
2 Kp (x[k] – x[k-1]) + Ki T (x[k] + x[k-1]).
Here, T = 0.01 s. The gains were chosen as Kp = 0.0192 and Ki = 0.0009. The implementation of
the PI controller in the microprocessor source code looks as follows [8]:

8. 7
The PI controller is active when the pendulum is in region II, III, or IV of the angular-position
space (Figure 2) that is, the controller is active when the pendulum is outside the region of
validity of the LQR controller of the linearized model. The PI controller has a self-adjusting
feature: If the cart’s reference position drifts away from the center of the rail, it is repositioned to
the center [8].
The control strategy is finalized by the inclusion of a supervisory coordinator that switches the
system between LQR control and swing-up control, depending on whether the pendulum is
within or outside of the region of attraction (region I of Figure 2), or puts the system into sleep
(waiting) mode if this enhances system performance [4].
Hardware Troubleshooting
Table 1: Hardware Troubleshooting
Idea Steps Taken Findings Solution
Encoders
Verify encoders are
powered correctly
Voltage was
measured with
multimeter
Incomplete
voltage caused
by open jumper
(JR4)
Jumper made
between JR4 and
+3.3 V
Verify encoders
transmit signal
correctly
Measured encoder
output with
oscilloscope
Verified timing
diagram with logic
analyzer
Signal
transmitted
correctly
N/A
Verify encoder
signals are read
correctly by
microcontroller
Observed CCS-
reported variable
values using
continuous refresh
and real-time mode
Microcontroller
reads correct
signal from
encoder
N/A

9. 8
PWM Verify PWM for
cart actuation
Generated a PWM
signal with
constant duty cycle
and verified this
using oscilloscope
Changed the open-
loop PWM duty
cycles and
observed if the cart
moved in the
correct direction
Cart moves in
only one
direction
Found broken
connections
Fixed broken
connections by
soldering the
joints around
connectors and
insulating using
heat-shrink wrap
Verify correct
polarities of encoder
and PWM signals
Listed every
possible
combination and
conducted
exhaustive (brute-
force) search to
identify the correct
polarity
Wrong pin
combination and
layout
Used the right
pin combination
and polarity
based on the
findings
Software Implementation
The microcontroller software project was created in the Integrated Development Environment
(IDE) of Texas Instrument (TI) Code Composer Studio (CCS) v7.1.0. on students’ computers.
The majority of the IDE project was imported from the previous team’s work in 2013, and minor
changes were applied to the header files so that CCS would recognize and use the correct
software libraries. The main.c source code file was then updated to the 2014’s version with
minor changes in order to work with the rest of the CCS project.
After the software structure was understood by the team, minor modifications and additions were
made in order to enable data capture and reporting in real time. This involved adding code to
implement the TI serial communication interface (SCI), which sends feedback and control values
over an RS-232 serial interface.

10. 9
Real-time Data Capturing and Reporting
Figure 3: Flowchart for Data Capturing and Reporting
In order to record and visualize data on a computer, data need to be both captured and reported.
The flow chart in Figure 3 shows the relevant algorithm. Data capturing is implemented at the
end of a 10-ms timer interrupt, and data reporting is implemented in the main loop. A variable is
used as a flag to synchronize their behaviors: reporting starts only when the previous capture has
completed, and capturing executes only when the previous reporting has finished.
It is worth noting that capturing and reporting are separate concepts. Capturing means copying
data for reporting while preserving such data until the reporting is done. On the other hand,
reporting means transmitting data from the microcontroller to a computer using a certain
protocol.
Data Capturing
Three (3) measurements are attempted to be captured every 50 ms: cart position (feedback),
pendulum angle (feedback), and duty cycle (control). The reasoning of selecting these is that

11. 10
they are the raw measurements and other values can be derived from them, such as angular/linear
velocities and accelerations.
After evaluating different data types, a 32-bit field was used for each value. Such 32-bit fields
will be noted as words in this report. Therefore, a word can be float/FP32 (32-bit signed floating
point), int32 (32-bit signed integer), or uint32 (32-bit unsigned integer).
When data capture is attempted and carried out, each value is type-casted into a 32-bit unsigned
integer (uint32) and stored in the corresponding buffer. Type cast means the straight copy of its
binary data; so 32-bit floating-point values (FP32) can be transmitted as if they were four 8-bit
unsigned integers (uint8), which avoids compromising precision [10].
Data Reporting
A custom protocol was created for the propose of data reporting and consisted of header (1 byte),
and payload (12 bytes). Figure 4 below shows a byte-to-byte structure of this protocol.
Figure 4: Communication Protocol
A header byte is used to ensure that the recipient is aware of the beginning of a message. Also,
the message has a fixed length, so the recipient can tell the end of the message. However, edge
cases exist when one of the bytes has the same value as the header byte. Therefore, padding is
used to distinguish payload from header, as explained below:
 If an octet has the same value as the header octet, then this value will be padded with a
header value.
 If a message ends with an octet that is the same as the header, then a non-header octet will be
padded at the end.
Visualization
Visualization was performed with MATLAB, which receives and parses the data via serial
communication (RS-232). Upon stripping of header padding, the payload was grouped by 32-bit
fields and converted into 32-bit unsigned-integer data type, which was then type-casted to 32-bit
floating point.
role header
octet 1 2 3 4 5 6 7 8 9 10 11 12 13
name header
notes 0x55 MS byte LS byte MS byte LS byte MS byte LS byte
this byte transmits first
word 1 (cart position) word 2 (pendulum angle) word 3 (duty cycle)
payload

12. 11
Figure 5: Screenshot of real-time data visualization during swing up
The MATLAB software was designed to continuously attempt to read and plot received data.
Figure 5 shows an example of real-time data visulization. Note that the duty cycle is bounded by
±65535 in this example.
To summerize the process of data capture, reporting, and visulization, a graphical pipeline is
shown in Figure 6 below.

13. 12
Figure 6: Summary of data capture, data reporting, and data visualization
Software Optimization & Analysis
Existing software source code was reviewed during the process of troubleshooting and
modification, and several opportunities for future improvement were identified. They can be
summarized as readability, initial calibration, and data type.
Due to time constraints, many software-optimization opportunities were identified and solutions
are proposed, but still have to be implemented.
Readability
Currently, the entire source code related to the control law is implemented in one function for
timer interrupt, and multiple nested conditional statements are used to determine and switch
among swing-up and balancing modes. Meanwhile, initialization and signal input are both
implemented in the main loop.

14. 13
In order to improve readability, the software should be divided into multiple roles: power-up
initialization, signal conditioning, signal input, control law, signal output, and data capturing and
reporting. This would allow better human readability.
Signal conditioning should rapidly monitor signals for overflow handling. Signal input should
prepare feedback signals for control-law use. The control law should compute the desired output.
Signal output should perform unit conversion and copy the output signal to the correct registry.
Initial Calibration
Calibration takes place right after power up. Currently, the cart moves in two directions in order
to determine the boundaries of the track, which subsequently determines the position of the cart.
The shortcoming of the current algorithm is that the boundaries are determined by a simple
velocity threshold. When the cart does not accelerate fast enough, the velocity can stay below the
threshold right after the initial actuation. For this reason, the calibration algorithm could place
both (left and right) boundaries at the same location, without any usable range for motion. This
logic could be improved by adding a debouncing feature on velocity, position, and/or time.
Data Type
The current implementation uses floating-point numbers for many data, which may not be the
best choice in many cases. For the purpose of counting quadratures to determine the
position/angle, precision can be negatively affected by floating point data types. This is due to
the nature of the IEEE 754 floating point standard [10].
Floating point numbers are denoted using 3 integer values: sign, exponent, and mantissa. Error
can accumulate when many additions or subtractions with different magnitudes are performed.
Figure 7 shows an example where error is introduced due to floating point addition, where 100 +
0.1 is smaller than 100.1
Figure 7: Example of error due to floating point addition in MATLAB
The software should be examined closely to determine when integer data types can be used, as
integer values tend not to carry such errors when performing integer addition or subtraction. This
property makes the integer data type more suitable for applications such as counting quadratures.

15. 14
Conclusions
This project has helped our team understand the principles involved in controlling an inverted
pendulum. The exercise provided us with a chance to troubleshoot an already existing, however
non-operational, set of equipment and make it functional again. We also added a few new
features to the system. It is now possible to capture and report real-time data and store such data
in computer memory or other storage devices.
The troubleshooting of the system formed a major part of the project, as we discovered a number
of loose or damaged connections. We fixed, properly insulated, and terminated them. Apart from
hardware troubleshooting, we also conducted several open-loop tests to check for the integrity of
the PWM signals.
Using the principles of inertia, resonance, and conservation of energy, it is possible to create a
swing-up control system that moves the pendulum into the region of attraction of the digital
stabilizing LQR controller, which is based on the linearization of a nonlinear state-space system
with state variables angular position, angular velocity, linear position, and linear velocity. The
switching between swing-up and LQR control is orchestrated by a supervisory coordinator.
Minor and moderate modifications were made to the software. We added code for real-time data
capturing and reporting via an RS-232 serial interface. The transmitted data was visualized in
MATLAB, by continuously reading and plotting the data received (Figure 3 and Figure 5). The
microcontroller software project was created and implemented in the CCS v7.1.0 IDE. By
design, the cart position, pendulum angle, and duty cycle were captured every 50 ms, with the
aim of deriving other values such as angular/linear velocities and acceleration. The captured data
was encoded in 32-bit fields and read into the CPU buffer or stored in other storage devices for
present or future data reporting and analysis.
The technology used has mechanical and electrical limitations, such as sensitivity to vibrations
and insufficient current. Several future enhancements could be made. For instance, the software
could be grouped into multiple functional units, to enable a better understanding of its structure.
The initial calibration logic could also be improved by debouncing the cart’s velocity, position,
and/or time, to improve track-length calibration and cart positioning. Finally, the use of integer
data types (rather than floating points) would help increase the accuracy of counting quadrature
pulses to determine cart and pendulum position and angle.
Our project shows that, under ordinary circumstances, the control system is capable of swinging
up the pendulum to the linear digital stabilizing controller’s region of attraction. Moreover, the
system can be termed robust with respect to its response to moderate disturbances [4].

16. 15
References
[1] Khalil Sultan, Inverted Pendulum, Analysis Design and Implementation. Karachi, Pakistan:
Institute of Industrial Electronics Engineering, 2003.
[2] Quanser, Markham, ON, Canada. High Fidelity Linear Cart System. (2013) [Online.]
Available: http://www.quanser.com/Products/Docs/1868/HFLC_System_Specifications.pdf
Accessed on Jun. 12, 2017.
[3] Segway® Personal Transporter (PT), a self-balancing vehicle. Picture from the open image
gallery of Segway Inc. [2011].
[4] H. L. Li, B. C. Chang, C. Jagadish, and H. Kwatny, “A DSP microprocessor hybrid control
of an inverted pendulum,” in 2009 IEEE ICCA, Christchurch, New Zealand, pp. 2317–
2322.
[5] H. K. Khalil, Nonlinear Systems. Upper Saddle River, NJ, USA: Prentice Hall, 2002.
[6] C. T. Chen, Linear System Theory and Design. New York, NY, USA: Oxford University
Press, 2013.
[7] T. Chmielewski, “Optimal Control: Linear Quadratic Regulator Systems.” Lecture notes
for the course ECES 512 Fundamentals of Systems II at Drexel University, Winter 2016.
[8] G. Bertelli, M. Brovedani, M. Bayram, and F. Catpinar, “Control of an inverted
pendulum.” Final Project report for the course MEM 800 at Drexel University, 2014.
[9] B. C. Chang, “Review of Proportional Control and PI Control.” Lecture notes for the
course MEM 459/540 Control Applications of DSP Microprocessors at Drexel University,
Spring 2017.
[10] IEEE Standard for Binary Floating-Point Arithmetic, IEEE Standard 754, 1985.

17. 16
Appendix
Source Code File for Inverted Pendulum Project
// MEM459/MEM800 - Final Project
// Teacher - Dr. Chang
// Group - Gianmarco Bertelli, Massimo Brovedani, Mevlut Bayram, Fatih Catpinar!
//=============================================================================
// Inverted Pendulum
// Version 1.1 - (2009) Original Code Imported from Ho-Lung Li's work for the DSPF2812 Microcontroller
// Version 1.2 - (2013) Group - Chris Braun, Mike Fan, Sam Pagano, Hang Yu
// Version 1.3 - (2014) Implementation of the cart positioning, simplification of the code
//=============================================================================
/*
To create & Run the Inverted Pendulum program in Code Composer Studio v5.5 the following must be
completed:
1) Create 'New CCS project': Name, Family = C2000, Variant = 2833x Delfino / Connection = EZDSPF28335,
Connection = Spectrum Digital DSK-EVM-eZdsp onboard USB Emulator
2) Once created, add this Main file to the project.
To add files, right click the project name and select add files. Browse and add the required files
in addtion to main listed below (this list includes .c .asm and .cmd files):
-DSP2833x_ADC_cal.asm
-DSP2833x_Adc.c
-DSP2833x_CodeStartBranch.asm
-DSP2833x_CpuTimers.c
-DSP2833x_DefaultIsr.c
-DSP2833x_EPwm.c
-DSP2833x_EQep.c
-DSP2833x_GlobalVariableDefs.c
-DSP2833x_Headers_nonBIOS.cmd
-DSP2833x_PieCtrl.c
-DSP2833x_PieVect.c
-DSP2833x_SysCtrl.c
-DSP2833x_usDelay.asm
The two following files should also we listed which are gnerated with the creation of the project file:
-28335_RAM_lnk.cmd
-EZDSPF28335.ccxml
3) Before it can be built and debugged, the includes must be added. To do this, go to 'Show Build
Settings'
under 'Project' on the title bar. Once there under C200 Complier click on 'include'. In the bottom space
under add directory, click 'add', and include the following folders:
-C:tixdais_7_21_01_07packagestixdais
-C:ticcsv5toolscompilerc2000_6.1.0include
-C:tidcsc25DSP2833xv131DSP2833x_commoninclude
-C:tidcsc25DSP2833xv131DSP2833x_headersinclude
4) Build the project by right clicking the project name and clicking 'Build Project'. Also you can build
the
by clicking 'Project' on the title bar and clicking 'Build all', or by simply pressing Ctrl+B.
5) *WARNING* Ensure the track is securely fastened to a hard surface and that the track is free of
obstructions.
Do not go near the track when unit is powered up. *WARNING*
6) Once the system is connected and powered up, the program can be debugged (Please ensure the cart is
started in the
middle of the track, at time of debugging).
7) If an emergency arises, click the pause button. You may also unplug the power supply or turn off the
power
switch in the box that supplies the capacitor or set finito=1 .
*/
//=============================================================================
// Code & Comments
#include "DSP28x_Project.h" // Include the Device Headerfile and Examples
#include "Math.h" // This is needed for the 'fabs' and 'floor' functions
#include "DSP2833x_Sci.h" // for serial (RS232) communication
#define DATA_CAPTURE_HEADER 0x55

18. 17
#define DATA_CAPTURE_BUFFER_SIZE 3
#define BYTE_MASK 0xFF
void InitEPwm1(void);
Uint32 EPwm1TimerIntCount;
void InitQEP1();
void InitQEP2();
void InitTRACKLENGTH();
interrupt void timer0_isr(void);
void scia_echoback_init(void);
void scia_fifo_init(void);
void scia_xmit(int a);
void scia_msg(char *msg);
void data_capture_xmit_all(void);
void data_capture_load_buffer(void);
Uint32 DATA_CAPTURE_buffer[DATA_CAPTURE_BUFFER_SIZE];
int DATA_CAPTURE_tx_flag = 0;
int DATA_CAPTURE_msg_gap = 0;
int DATA_CAPTURE_msg_miss = 0;
// Global variables used in this project
int num_of_ints=0,counter=0,i=0, checco=0,okok=1000, overcounter=0;
int time =0, seconds = 0,finito=0;
float duty=0,angle=0.0,used_angle=0.0,distance
=0.0,old_distance=0.0,old_angle=0.0,lin_speed=5,ang_speed=0.0,used_angle1=0.0,used_angle2=0.0,tmp1=0,tmp2=
1;
long timerValue=0,new_timervalue=0,old_timervalue=0,count=0,max=65536,flag=0,dir_flag=0,dist_flag =0,
salva=0,salva2=0,cabron=0;
float end1=0,end2=0,end3=0,middle=0,dud=1; // end setting, if used
/*
float angular_pos[400]; //for record angle, buffer size max=1756
float cart_pos[400]; //for record distance
float cart_adj[400];
float where_i_want[400];
*/
float k1, k2, k3, k4, k5, req_distance=0,record=0;
float ek=0.0,ekminus=0.0,mk=0.0,mkminus=0.0,kp=0.0,ki=0.0,kih=0.0,yk=0.0;
float track_length=0, donde=0, wanted=0, integral_pos=0;
float dist_control=0, jjj=0, media_new=0, media_old=0, numerator=0, adjust=0, adjust_old=0,
integral_want=0;
// Start main loop
void main()
{
InitSysCtrl(); // Initializes System Control
//
// Step 2. Initialize GPIO:
// This example function is found in the DSP2833x_Gpio.c file and
// illustrates how to set the GPIO to it's default state.
//
// InitGpio(); Skipped for this example
//
// For this example, only init the pins for the SCI-A port.
// This function is found in the DSP2833x_Sci.c file.
//
InitSciaGpio();

19. 18
InitEPwm1Gpio(); /* Initialze GPIO Pins for PWM
GPIO = General Purpose Input Output / PWM = Pulse Width Modulation */
DINT; // Disable CPU interrupts
InitPieCtrl(); /* Initializes the Peripheral Interrupt Expansion (PIE) control registers to their
default state
The default state is all PIE interrupts disabled and flags are cleared */
// Disable CPU interrupts and clear all CPU interrupt flags
IER = 0x0000; // CPU Interrupt Enable Register (IER)
IFR = 0x0000; // CPU level interrupt flag (IFR)
InitPieVectTable(); /* Initialize the PIE vector table with pointers to the shell Interrupt
Service Routines (ISR) */
EALLOW; // This is needed to write to EALLOW protected registers
SysCtrlRegs.PCLKCR0.bit.TBCLKSYNC = 0; /* Peripheral Clock Control 0 Register
Enable ePWM module clocks in the PCLKCR1 register.
Set TBCLKSYNC to 0.
Configure prescaler values and ePWM modes.
Set TBCLKSYNC to 1.
PAGE 35 of SYSTEM CONTROL & INTERUPTS */
EDIS; // This is needed to disable write to EALLOW protected registers
InitEPwm1();
EALLOW; // This is needed to write to EALLOW protected registers
SysCtrlRegs.PCLKCR0.bit.TBCLKSYNC = 1; // See above comments
EDIS; // This is needed to disable write to EALLOW protected registers
// Interrupts mapped to ISR functions found within this file.
EALLOW; // This is needed to write to EALLOW protected registers
PieVectTable.TINT0 = &timer0_isr; // CPU-timer interrupt signals
EDIS; // This is needed to disable write to EALLOW protected registers
InitCpuTimers(); // Initialize CPU Timers - Stopped State
ConfigCpuTimer(&CpuTimer0, 150, 10000); //150MHz, 10ms / Configure CPU Timer
StartCpuTimer0(); // Start the initialized timers
InitQEP1(); // Initialize Enhanced Quadrature Encoder Pulse (eQEP) # 1
InitQEP2(); // Initialize Enhanced Quadrature Encoder Pulse (eQEP) # 2
IER |=(M_INT1|M_INT3|M_INT4); // Set CPU Interrupt Enable Registers
PieCtrlRegs.PIEIER1.bit.INTx7 = 1; /* PAGE 124 of SYSTEM CONTROL & INTERUPTS
PIE MUXed Peripheral Interrupt Vector Table - INTx7 / TINT0 (TIMTER 0) */
EINT; // enable Interrupt Register
ERTM; // enable Global Realtime Tnterrupt DBGM
// PI controller for swing-up (from old DSP F2812 code)
kp=0.0192;//proportional gain value 0.0194, 0.0191
ki=0.0009;//integral gain value 0.00081, 0.0009
// State feedback gains
k1=-19.0353;
k2=-23.7728;
k3=132.8867;
k4=23.7150;
InitTRACKLENGTH(); // call the void that will compute the track length
// -1000 will take into account that on the left side the origin has been set by the tracklength void
at 1000 EQEP on the left
//(for better understanding read the report)
track_length=(salva2)/4096*2.0 * 3.14159 * 0.635;
scia_fifo_init(); // Initialize the SCI FIFO
scia_echoback_init(); // Initialize SCI for echoback
char *msg;
msg = "rnnnHello World!0";
scia_msg(msg);
while (1) //This loop continuously updates the angle and timerValue variables with data from the QEP's
{
angle = ((EQep1Regs.QPOSCNT)/4096.0) * 360.0; // Set current angle value
// is the value that will be converted as the cart position!
timerValue=EQep2Regs.QPOSCNT; // Set current Timer Value
// start of code to take care of timer overflow
new_timervalue=timerValue; // Set new timer value to current timer value
// Code to 'take care of timer value overflow'

20. 19
if(fabs(old_timervalue - new_timervalue)>30000) // If difference in timers is greater than 30000
{
if(timerValue<30000)
{
// Increasing the Overclock counter because of the type of overflow
overcounter++;
}
else if(timerValue>30000)
{
// Decreasing the Overclock counter because of the type of overflow
overcounter--;
}
}
// If I'm not encountering the overflow I'm HERE and flag will decide how to compute the
distance!!!
count=overcounter*max/2+timerValue/2;
old_timervalue=new_timervalue; // set new timer value as old timer value
if (DATA_CAPTURE_tx_flag == 1){
data_capture_xmit_all();
DATA_CAPTURE_tx_flag = 0;
}
}
}
void InitTRACKLENGTH()
{
// while the cart is moving
while(fabs(lin_speed)>0.00005)
{
// power the CMPB
EPwm1Regs.CMPB=10000;
// timerValue is the value that will be converted as the cart position!
timerValue=EQep2Regs.QPOSCNT; // Set current Timer Value
new_timervalue=timerValue; // Set new timer value to current timer value
// Code to 'take care of timer value overflow'
if(fabs(old_timervalue - new_timervalue)>30000) // If difference in timers is greater than 30000
{
// Decreasing the Overclock counter because of the type of overflow
overcounter--;
}
// If I'm not encountering the overflow I'm HERE and flag will decide how to compute the
distance!!!
count=overcounter*max/2+timerValue/2;
old_timervalue=new_timervalue; // set new timer value as old timer value
}
// the cart will be on the left side of the track
count=0;
overcounter=0;
// set the speed to an high value so that in the next cycle Ill move to the right
lin_speed=10;
//Reinitializing the QEP so that that position will be read as 0.
InitQEP2();
// setting the value of the EQep to 0, in that position Ill have the origin of the discontinuous
counter
EQep2Regs.QPOSCNT=0;
old_timervalue=0; //This is necessary to avoid an additional overflow (not real but just due to the
reset of QPOSCNT registry.
//shutting down the cart motor
EPwm1Regs.CMPB=0; // same procedure of above but with the cart moving on the right
while(fabs(lin_speed)>0.00005)
{
EPwm1Regs.CMPA.half.CMPA=10000;
EPwm1Regs.CMPB=0;
timerValue=EQep2Regs.QPOSCNT; // Set current Timer Value
new_timervalue=timerValue; // Set new timer value to current timer value
// Code to 'take care of timer value overflow'
if(fabs(old_timervalue - new_timervalue)>30000) // If difference in timers is greater than 30000
{

21. 20
// Increasing the Overclock counter because of the type of overflow
overcounter++;
}
// If I'm not encountering the overflow I'm HERE and flag will decide how to compute the
distance!!!
count=overcounter*max/2+timerValue/2;
old_timervalue=new_timervalue; // set new timer value as old timer value
}
// saving the value of count! is the track length!!!
salva2=count*2;
// the controller can start
finito=2;
// initial power of the motor=0
EPwm1Regs.CMPB=000;
EPwm1Regs.CMPA.half.CMPA=000;
}
void InitQEP1()
{
// PAGE 30 of ENHANCED QUADRATURE ENCODER PULSE (eQEP) MODULE
EQep1Regs.QEINT.bit.PCO = 0; // Position Counter Overflow Interrupt disEnabled
EQep1Regs.QEINT.bit.PCM = 0; // Position-Compare Interrupt disEnabled
EQep1Regs.QEPCTL.bit.FREE_SOFT=2;
EQep1Regs.QEPCTL.bit.PCRM=01; // Position Counter Reset on Maximum Position PCRM=01 mode
EQep1Regs.QEPCTL.bit.UTE=0; // Unit Timeout Enable
EQep1Regs.QEPCTL.bit.QCLM=0; // Latch on unit time out
// The PCO interrupt is generated for the UPEVNT when QPOSCNT is reset from 1 to 0.
// The values for QPOSMAX and QPOSCMP are arrived at keeping in mind that the position
// counter counts 0-1-0-1-0-1... and so on
//This code does not interrupt on QEP, so interrupts are disabled.
EQep1Regs.QPOSMAX=max; // Maximum Position Count Register
EQep1Regs.QPOSCMP=0x00000000; // Position-Compare Register
EQep1Regs.QEPCTL.bit.QPEN=1; // QEP enable
EQep1Regs.QCAPCTL.bit.UPPS=1; // for UPEVNT Generation
EQep1Regs.QCAPCTL.bit.CCPS=6; // 1/64 for CAP clock (input 40 Hz)
EQep1Regs.QCAPCTL.bit.CEN=1; // QEP Capture Enable
EQep1Regs.QPOSCTL.bit.PCE = 1; // disEnable Position-Compare
EALLOW; // Enable EALLOW
// PAGE 75 of SYSTEM CONTROL & INTERUPTS
// GPIO Pins do not change. Once enabled, they follow the table on page 75.
GpioCtrlRegs.GPAMUX2.bit.GPIO20 = 1; // Assuming GPIO20 is EQEP1A (Signal)
GpioCtrlRegs.GPAMUX2.bit.GPIO21 = 1; // Assuming GPIO21 is EQEP1B (Signal)
GpioCtrlRegs.GPAMUX2.bit.GPIO23 = 1; // Assuming GPIO23 is EQEP1I (Index)
//Index pin is not used in this project because it is not needed. Index
// is designed to take a high input for every one revolution in order
// to reset the counter every rev
EDIS; // Disable EALLOW
}
void InitQEP2()
{
// PAGE 30 of ENHANCED QUADRATURE ENCODER PULSE (eQEP) MODULE
EQep2Regs.QEINT.bit.PCO = 0; // Position Counter Overflow Interrupt disEnabled
EQep2Regs.QEINT.bit.PCM = 0; // Position-Compare Interrupt disEnabled
EQep2Regs.QEPCTL.bit.FREE_SOFT=2;
EQep2Regs.QEPCTL.bit.PCRM=01; // Position Counter Reset on Maximum Position PCRM=01 mode
EQep2Regs.QEPCTL.bit.UTE=0; // Unit Timeout Enable
EQep2Regs.QEPCTL.bit.QCLM=0; // Latch on unit time out
// The PCO interrupt is generated for the UPEVNT when QPOSCNT is reset from 1 to 0.
// The values for QPOSMAX and QPOSCMP are arrived at keeping in mind that the position
// counter counts 0-1-0-1-0-1... and so on
//This code does not interrupt on QEP, so interrupts are disabled.
EQep2Regs.QPOSMAX=max; // Maximum Position Count Register
EQep2Regs.QPOSCMP=0x00000000; // Position-Compare Register
EQep2Regs.QEPCTL.bit.QPEN=1; // QEP enable
EQep2Regs.QCAPCTL.bit.UPPS=1; // for UPEVNT Generation
EQep2Regs.QCAPCTL.bit.CCPS=6; // 1/64 for CAP clock (input 40 Hz)
EQep2Regs.QCAPCTL.bit.CEN=1; // QEP Capture Enable
EQep2Regs.QPOSCTL.bit.PCE = 1; // disEnable Position-Compare
EALLOW; // Enable EALLOW
// PAGE 75 of SYSTEM CONTROL & INTERUPTS

22. 21
// GPIO Pins do not change. Once enabled, they follow the table on page 75.
GpioCtrlRegs.GPAMUX2.bit.GPIO24 = 2; // Assuming GPIO24 is EQEP1A (Signal)
GpioCtrlRegs.GPAMUX2.bit.GPIO25 = 2; // Assuming GPIO25 is EQEP1B (Signal)
GpioCtrlRegs.GPAMUX2.bit.GPIO26 = 2; // Assuming GPIO26 is EQEP1I (Interrupt)
//Index pin is not used in this project because it is not needed. Index
// is designed to take a high input for every one revolution in order
// to reset the counter every rev
EDIS; // Disable EALLOW
}
void InitEPwm1()
{
// PAGE 96 of ENHANCED PULSE WIDTH MODULATOR (ePWM) Module
EPwm1Regs.TBPRD = 0xFFFF; // Period = Total High FFFF
EPwm1Regs.CMPB = 0x0000; // Compare A
EPwm1Regs.CMPA.half.CMPA = 0x0000; // Compare B
// Comparators are set to zeros initially to ensure the motor doesn't move
EPwm1Regs.TBPHS.half.TBPHS = 0x0000; // Set Phase register to zero
EPwm1Regs.TBCTR = 0; // clear TB counter
EPwm1Regs.TBCTL.bit.CTRMODE = TB_COUNT_UP; // Page 98 (ePWM)
EPwm1Regs.TBCTL.bit.PHSEN = TB_DISABLE; // Phase loading disabled
EPwm1Regs.TBCTL.bit.SYNCOSEL = TB_SYNC_DISABLE;
EPwm1Regs.TBCTL.bit.HSPCLKDIV = TB_DIV4; // Clock ratio to SYSCLKOUT
EPwm1Regs.TBCTL.bit.CLKDIV = TB_DIV4;
EPwm1Regs.CMPCTL.bit.LOADAMODE = CC_CTR_ZERO; // load on CTR = Zero
EPwm1Regs.CMPCTL.bit.LOADBMODE = CC_CTR_ZERO; // load on CTR = Zero
EPwm1Regs.AQCTLA.bit.ZRO = AQ_SET; // Page 103
EPwm1Regs.AQCTLA.bit.CAU = AQ_CLEAR; // Page 103
EPwm1Regs.AQCTLB.bit.ZRO = AQ_SET; // Page 104
EPwm1Regs.AQCTLB.bit.CBU = AQ_CLEAR; // Page 104
}
interrupt void timer0_isr(void)
{
CpuTimer0.InterruptCount++;
i++;
if(seconds>20.0) // Set a time period to 20 seconds per cycle
{
time=0;
}
else time++;
seconds = time/100.0;
//(for better understanding read the report)
distance = (((count) * 2)/4096.0)* 2.0 * 3.14159 * 0.635 ; // Calc for distance
distance = distance - track_length/2; //set dist equal to zero in the middle of the track
used_angle = angle - 180.0; // setting the angle 0 when the pendulum is up
ang_speed= (used_angle - old_angle) / 0.01; // backward difference differentiation will give us the
value of velocity
old_angle=used_angle;
//Hacked together modulus division to set range. It is important to do this AFTER speed calcs
tmp1 = (used_angle-180) / 360.0; // Set Range of values to -180 to 180 for the pendulum
used_angle = (tmp1-floor(tmp1))*360 - 180.0; // 0 degrees initially down, reverse to make 0 vertical
//Calculate linear speed
lin_speed=( (distance - old_distance ) / 0.01);
// Computing the integral of the position so that we can control better the positioning of the cart
old_distance = distance;
if(finito==1)
{
EPwm1Regs.CMPB=0x0000;
EPwm1Regs.CMPA.half.CMPA=0x0000;
}
if(finito==2)
{
if(fabs(used_angle)<=24.0) //Balance state feed back control
{ // switch controller when angle is between -24 and 24 degree
// donde represent the (x-xref) error of the position of the cart
donde=(distance-wanted);
// The dist_control variable will be adjusted by the adjusting parameter
// this will give an adaptive feedforward to the system.
dist_control=donde+adjust;
//creating the input (scaling with 100 for the voltage compensator)

23. 22
yk = ((dist_control * k1) + (lin_speed * k2) + (used_angle * k3) + (ang_speed * k4))/100.0;
/*
//Saving important values such as position of the cart position wanted and angle of the
pendolum
//every 10 interrupts
if(i%10==0)
{
if(counter<400)//876
{
angular_pos[counter]=used_angle; // store values to create graph in debug
cart_pos[counter]=distance; // store values to create graph in debug
cart_adj[counter]=adjust;
where_i_want[counter]=wanted;
counter++;
}
else counter=0;
}
*/
// Cycling the wanted position
if(seconds<10)
{
wanted=15.2654;
}
else
{
wanted=-15.2654;
}
// Only move if the ang_speed of Pendulum is less than given value this is in order to respect
the energy requirements
// of the swing up
if(fabs(ang_speed)<178.0) // fabs = absolute value function
{duty= (yk * 10000.0);} // the duty has been scaled to consider the gain associated with the
voltage-input TF
else duty=0.0;
// check on the overflow of the duty (we decided to work only with one of the 2 pwms in order
to achieve a better linearity)
if( fabs(duty)>32767)
{
duty=duty/fabs(duty)*32767;
}
//If I'm not too near to the border
if( fabs(distance) <35.0)
{
EPwm1Regs.CMPB = 32767;
// if duty bigger than 0 the duty cycle of A must be decreased to move on the left!
// A represent the right movement
EPwm1Regs.CMPA.half.CMPA=32767-duty;
}
else if(distance<0)
{
// if I'm near the border I can get stuck and adjust can assume too high value!
adjust=0;
// Power on the left
EPwm1Regs.CMPB=0x0000;
EPwm1Regs.CMPA.half.CMPA=0xffff; // 12288 decimal...
}
else if(distance>0)
{
// if I'm near the border I can get stuck and adjust can assume too high value!
adjust=0;
// Power on the right
EPwm1Regs.CMPB=0xffff;
EPwm1Regs.CMPA.half.CMPA=0;
}
// If I'm around the equilibrium every 10 ms I'll save the values for 1s
if(jjj<100)

24. 23
{
jjj++;
// saving the error (donde) value to compute the mean
numerator+=donde;
}
else
{
// at 1 sec compute the mean and get ready to compute a new one
media_new=numerator/jjj;
jjj=0;
numerator=0;
// if my cart is stuck far from the wanted position adjust the feedforward
if(fabs(media_old-media_new)<0.5 && fabs(donde)>0.5)
{
adjust=(media_new+media_old)/2+adjust_old;
adjust_old=adjust;
}
// save the value of the actual mean
media_old=media_new;
}
}
else // ================== swing-up PI position control with BS algorithm
==============================
{
if(ang_speed==0.0)
{
if(used_angle<-179.8 || used_angle>179.8) // set initial position
{
req_distance=-15.0;
}
}
else if(ang_speed>0.0)
{
if(used_angle>0.0)
{
if(ang_speed<=22.0) //20
{
used_angle1=used_angle;
}
}
if(used_angle1>0.0) // the request distance is relative to used_angle
{
if(used_angle>175.0)
{
if(fabs(used_angle1)<50.0 && fabs(used_angle1)>40.0)
{
req_distance=-(fabs(used_angle1))/3.65;//3.45, 3.7
}
else if(fabs(used_angle1)<40.0 && fabs(used_angle1)>20.0)
{
req_distance=-(fabs(used_angle1))/3.25;//3.25, 3.3
}
else {req_distance=-10.0;}
}
}
}
else if(ang_speed<0.0)
{
if(used_angle<0.0)
{
if(ang_speed>=-22.0)
{
used_angle1=used_angle;
}
}
if(used_angle1<0.0)
{
if(used_angle<-175.0)
{

25. 24
if(fabs(used_angle1)<50.0 && fabs(used_angle1)>40.0)
{
req_distance=(fabs(used_angle1))/3.65;//3.45
}
else if(fabs(used_angle1)<40.0 && fabs(used_angle1)>20.0)
{
req_distance=(fabs(used_angle1))/3.25;//3.25
}
else {req_distance=10.0;}
}
}
}
ek =(req_distance-distance);
//ek/=1.0;
if(ek>30.0)//25
{ek=30.0;}
else if(ek<-30.0)
{ek=-30.0;} // mkminus
kih=ki * 0.005;
mk = mkminus + (kp * (ek-ekminus))+(kih * (ek+ekminus));
ekminus = ek;
mkminus = mk;
//mk/=1;
duty= mk * 65535.0;
//setting PWM duty cycle
if (duty>=0)
{
if(distance<38.0)
{
EPwm1Regs.CMPB=0;
EPwm1Regs.CMPA.half.CMPA = (int)(duty);
}
else
{
EPwm1Regs.CMPB = 0xffff;
EPwm1Regs.CMPA.half.CMPA = 0x0000;
}
}
else if(duty<0)
{
if(distance>-38.0)
{
EPwm1Regs.CMPB = (int)(-duty);
EPwm1Regs.CMPA.half.CMPA=0;
}
else
{
EPwm1Regs.CMPB = 0x0000;
EPwm1Regs.CMPA.half.CMPA = 0xffff;
}
}
}
//}
}
if(i%5==0){
// data capture and transmit if the previous transmission is already done
// otherwise, skip this transmission and count number of miss(es)
if (DATA_CAPTURE_tx_flag == 1){
DATA_CAPTURE_msg_gap++;
DATA_CAPTURE_msg_miss++;
}
else{
data_capture_load_buffer();
DATA_CAPTURE_msg_gap = 0; // clear the gap for missed messages
DATA_CAPTURE_tx_flag = 1;
}
}

26. 25
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1; // PIE Acknowledge Register
// The microcontroller needs to "know" that the program has handled the
// interrupt and is done.
}
//
// scia_echoback_init - Test 1,SCIA DLB, 8-bit word, baud rate 0x000F,
// default, 1 STOP bit, no parity
//
void
scia_echoback_init()
{
//
// Note: Clocks were turned on to the SCIA peripheral
// in the InitSysCtrl() function
//
// 1 stop bit, No loopback, No parity,8 char bits,
// async mode, idle-line protocol
//
SciaRegs.SCICCR.all =0x0007;
//
// enable TX, RX, internal SCICLK,
// Disable RX ERR, SLEEP, TXWAKE
//
SciaRegs.SCICTL1.all =0x0003;
SciaRegs.SCICTL2.all =0x0003;
SciaRegs.SCICTL2.bit.TXINTENA =0;
SciaRegs.SCICTL2.bit.RXBKINTENA =0;
SciaRegs.SCIHBAUD =0x0000; // 38400 baud @LSPCLK = 37.5MHz.
SciaRegs.SCILBAUD =0x0079;
SciaRegs.SCICTL1.all =0x0023; // Relinquish SCI from Reset
}
//
// scia_xmit - Transmit a character from the SCI
//
void
scia_xmit(int a)
{
while (SciaRegs.SCIFFTX.bit.TXFFST != 0)
{
}
SciaRegs.SCITXBUF=a;
}
//
// scia_msg -
//
void
scia_msg(char * msg)
{
int i;
i = 0;
while(msg[i] != '0')
{
scia_xmit(msg[i]);
i++;
}
}
//
// scia_fifo_init - Initialize the SCI FIFO
//
void

27. 26
scia_fifo_init()
{
SciaRegs.SCIFFTX.all=0xE040;
SciaRegs.SCIFFRX.all=0x204f;
SciaRegs.SCIFFCT.all=0x0;
}
// this function transmits the header and payload
void data_capture_xmit_all(){
int dc_i, dc_j, dc_byte;
// transmit header
scia_xmit( DATA_CAPTURE_HEADER );
// TODO: transmit type and miss count
// sweep each 32-bit word
for (dc_i=0; dc_i<DATA_CAPTURE_BUFFER_SIZE; dc_i++){
// sweep each byte inside this word
for (dc_j=24; dc_j>=0; dc_j-=8){
// obtain this byte (8-bit)
dc_byte = (int)( ( DATA_CAPTURE_buffer[dc_i] >> dc_j ) & BYTE_MASK );
// pad itself if it is the same as the header
if (dc_byte == DATA_CAPTURE_HEADER) scia_xmit(DATA_CAPTURE_HEADER);
scia_xmit(dc_byte); // transmit this byte
}
}
// if a message ends with header, transmit a non-header
if (dc_byte == DATA_CAPTURE_HEADER) scia_xmit( (DATA_CAPTURE_HEADER + 1) % 0xFF );
// DATA_CAPTURE_tx_flag = 0;
}
void data_capture_load_buffer(){
// copy to-be-transmitted values to buffer
DATA_CAPTURE_buffer[0] = * ((Uint32 *) & distance );
DATA_CAPTURE_buffer[1] = * ((Uint32 *) & used_angle );
DATA_CAPTURE_buffer[2] = * ((Uint32 *) & duty );
}

28. 27
MATLAB Code for Visualizing Results
close all;
clear;
global buffer MSG_LEN;
% close current com ports
try
comports = instrfind;
closed = cellfun(@(x) strcmp(x, 'closed'), comports.Status);
open_ports = find( ~closed );
if size(open_ports) == 1
fclose( comports(open_ports) );
end
end
baud = 38400
port = 'COM13'
buffer = [];
s = serial(port);
set(s, 'BaudRate', baud);
set(s, 'Terminator', '');
fopen(s);
% clear anything in buffer
if (s.BytesAvailable > 0)
fread(s, s.BytesAvailable);
end
byte_1 = -1;
byte_2 = -1;
byte_3 = -1;
buffer_serial = [];
HEADER = hex2dec('55');
MSG_LEN = 3; % number of 32-bit words
while(true)
cs = s.BytesAvailable;
if (cs > 0)
chunk = fread(s, cs);
byte_1 = -1;
byte_2 = -1;
byte_3 = -1;
for b = chunk'
byte_3 = byte_2;
byte_2 = byte_1;
byte_1 = b;

29. 28
if (byte_1 == HEADER)
if (byte_2 == HEADER)
buffer_serial = [buffer_serial, byte_1];
end
else
if ((byte_2 == HEADER) && (byte_3 ~= HEADER))
buffer_serial = [];
end
buffer_serial = [buffer_serial, byte_1];
end
if (length(buffer_serial) >= (MSG_LEN * 4))
add_frame(buffer_serial(1:(MSG_LEN * 4)));
buffer_serial = [];
end
end
else
pause(0.01);
end
pause(0.01);
end
function [] = add_frame(msg)
global buffer MSG_LEN;
frame = [];
for i = [1 : 4 : (MSG_LEN * 4 - 1)]
word_uint8 = uint32( msg(i : i+3) );
tmp = sum( word_uint8 .* (256.^uint32([3:-1:0])) );
word_fp32 = typecast( uint32(tmp), 'single');
frame = [frame, word_fp32];
end
buffer = [buffer; frame];
if size(buffer, 1) > 300
buffer = buffer(end-299 : end, :);
end
draw( buffer );
end
function [] = draw( f )
subplot(3,1,1);
plot(f(:,1), 'Marker', '.');
ylim([-100 100]); grid on;
title('distance')
subplot(3,1,2);
plot(f(:,2), 'Marker', '.');

30. 29
ylim([-180 180]); grid on;
title('angle')
subplot(3,1,3);
plot(f(:,3), 'Marker', '.');
ylim([-65536 65536]); grid on;
title('duty cycle')
end

31. 30
Pin-out Diagram

32. 31
Project Video Link
https://www.youtube.com/watch?v=YFRkQXO_EEM