UNIVERSITY OF NOTRE
DAME
Pendulum Project
AME 30315
Joshua Szczudlak
Firas Fasheh
5/2/2012
For me, I am driven by two main philosophies, know more today about the world than I knew yesterday. And
lessen the suffering of others. You'd be surprised how far that gets you.

Neil deGrasse Tyson
University of Notre Dame
AME 30315 Pendulum Project
1

P a g e
Abstract
The purpose of this project was to design a controller that stabilizes an inverted pendulum. The
first step in designing the controller was to identify the system. Through the process of system
identification it was found that
and
Next, a transfer function was
derived for the system using the governing equations of motion. This transfer function was found
for
an output position,
in terms of an
input torque,
Additionally, lead and lag compensators
were created to help stab
ilize the system. Two lead

lag compensators were designed. One
controller used the assigned parameters of
and a lag gain of 92. The other controller was
designed for optimal performance including a half second rise time
, quick settling time, and
small
stead

state error
. These parameters were evaluated using root locus plots and the used of Simulink
to predict performance.
University of Notre Dame
AME 30315 Pendulum Project
2

P a g e
Table of Contents
1 System
................................
................................
................................
................................
...........
4
1.1 System Identification
................................
................................
................................
......
4
1.1.1 Design Parameters
................................
................................
................................
.
5
2 Control Design
................................
................................
................................
..............................
5
2.1 Continuous Transfer Function
................................
................................
........................
5
2.1.1 Hanging Pendulum
................................
................................
................................
5
2.1.2 Inverted Pendulum
................................
................................
................................
6
2.2 Design Parameters
................................
................................
................................
..........
7
2.3 Lead Control
................................
................................
................................
..................
7
2.3.1 Design
................................
................................
................................
...................
8
2.3.2 Lead Calculation
................................
................................
................................
....
8
2.4 Lag Control
................................
................................
................................
....................
9
2.4.1 Design
................................
................................
................................
...................
9
2.4.2 Lag Calculation
................................
................................
................................
......
9
2.5 Discrete

time Transfer Function
................................
................................
...................
10
2.5.1 Conversion to Discrete

time
................................
................................
................
10
3 Controller Implementation
................................
................................
................................
..........
10
3.1 Controller Implementation
................................
................................
...........................
10
3.1.1 Non

dimensionalization
................................
................................
.......................
10
3.1.2 Error Calculation
................................
................................
................................
.
11
3.1.3 Transfer Function Implementation
................................
................................
......
11
5 System Evaluation
................................
................................
................................
.......................
11
5.1 Theoretical Modeling
................................
................................
................................
....
11
5.2 Transient Evaluation
................................
................................
................................
....
12
5.2.1 Rise Time Evaluation
................................
................................
...........................
13
5.2.2 Overshoot Evaluation
................................
................................
..........................
13
5.3 Steady

State Evaluation
................................
................................
................................
13
6 System Verification
................................
................................
................................
.....................
14
6.1 Position Verification
................................
................................
................................
.....
14
7 Conclusions
................................
................................
................................
................................
.
15
Appendix A:
Matlab Code
Appendix B: C Code
Appendix C: Discretization of Transfer Function
Appendix D: Derivations of Transfer Function
Appendix E: Lead Compensator Calculations
Appendix F: Iterations Table
List of Tables
Table 1. Steady

state error in the controller at various desired angles.
................................
.............
14
University of Notre Dame
AME 30315 Pendulum Project
3

P a g e
List
of Figures
Figure 1. Measured damped frequency response
................................
................................
...............
5
Figure 2. Root locus of the hanging pendulum transfer function
................................
......................
6
Figure 3. Comparison of measured damped frequency response a
nd derived transfer function
........
6
Figure 3. Root locus of the inverted pendulum transfer function
................................
......................
7
Figure 4. Root locus with effect of the lead compensator
................................
................................
.
9
Figure 5. Root locus of the transfer function with the lead

lag compensator
................................
..
10
Figure 6. Representati
ve Simulink block diagram
................................
................................
............
12
Figure 7. Response predicted by Simulink
................................
................................
......................
12
Figure 8. Response of pendulum
................................
................................
................................
....
13
Figure 9. Response of pendulum at angles of

30º to 30º
................................
................................
14
University of Notre Dame
AME 30315 Pendulum Project
4

P a g e
1
System
1.1 System Identification
The first step to designing a controller is to determine what sort of system you are dealing with and
what parameters you need to model the system accurately.
Determining the parameters to the
pendulum system is fairl
y simple because
when a step input is sent to the pendulum it responds in
an easily understood sinusoidal manner. The parameters necessary for the modeling of the
pendulum are
, the damping ratio,
, the natural frequency, and
F
a scale factor.
A series of
simple equations can be used to determine these
expressions
. The first equation can be used to find
the damping ratio,
(
)
√
(1)
w
here
is the logarithmic decrement,
is the damping ratio of the system, and
and
are the
distance from the
steady

state
value of the second and third peaks respectively
.
This equation can
be used to determine the damping ratio of the pendulum system.
Knowledge of the period and the damping ratio allow
s
us to find the damped natural frequency of
the system.
⠲
)
where
is the damped natural frequency, and
is the period. Using the damped natural
frequency and the damping ratio the natural frequency can be determined.
√
(
3
)
where
is the natural frequency.
A final scale factor was determined by multiplying the
steady

state
value by the square of the natural frequency and dividing by the value of the applied torque.
Figure 1 shows the damped frequency response of the pendulum.
University of Notre Dame
AME 30315 Pendulum Project
5

P a g e
Figure 1.
Measured d
amped frequency response
The parameters derived from this plot are outlined in section 1.1.1 Design Parameters.
1.1.1
Design
Parameters
The design parameters were determ
ined through the system identification process outlined above.
The parameters used in the design of the pendulum controller were found by averaging data taken
by testing multiple pendulums at various torques.
Doing this ensured that any pendulum could be
used with relative accuracy.
This process yielded the following parameters:
was 6.28 Hz
,
was
0.055, and the scale factor
F
was 37.2.
Additional system identification plots can be found in the
Matlab code in Appendix A.
2
Control Design
2
.
1
Continuous Transfer Function
A transfer function
in the continuous time domain was derived first for the
hanging
pendulum
system. This
system is then inverted for uses in the inverted pendulum system.
The inverted
pendulum transfer function is then
disc
retized f
or use in the microcontroller.
2
.1.1
Hanging Pendulum
The transfer function of the pendulum is shown in the f
ol
lowing equation,
(
)
(
)
(4)
where
R
is the input position error and
T
i
s the output torque. Appendix D
shows the calculations
necessary to obtain the transfer function for the hanging pendulum.
Figure 2 is the root
locus plot of the hanging pendulum system.
University of Notre Dame
AME 30315 Pendulum Project
6

P a g e
Figure
2
.
Root locus of the hanging pendulum transfer function
Figure 3
shows the measured
response of the hanging pendulum system and the hanging pendulum
transfer function after it has been subject to a step input. The similarity between the two helps to
verify the accuracy of the model as well as the accuracy of the values obtained from the
system
identification.
Figure 3
.
Comparison of measured damped frequency response and derived transfer function
2
.1.2 Inve
rted Pendulum
After obtaining the transfer function for the hanging pendulum the transfer function for the
inverted system is
almost trivial.
The difference
is in
a sign difference in the equations of motion.
This transfer function is shown in the following equation,
University of Notre Dame
AME 30315 Pendulum Project
7

P a g e
(
)
(
)
(
5
)
The only difference between t
he two transfer functions is that
the pendulum responds to the force
of gravity. When the hanging pendulum is displaced in the positive direction, the gravitational force
opposes it. However, in the inverted pendulum system the gravitational force works with
displacement. This differen
ce causes the sign change on the
term.
Figure 3 is the root locus of the invert pendulum transfer function.
Figure
3
.
Root locus of the inverted pendulum transfer function
Of interest to us at this point is the location of the poles because
these will help to determine many
of the characteristics of our lead and lag controllers. The
se
poles are at 5.93 and

6.62.
2.2 Design Parameters
The design of the controller is dictated mostly by the desired response characteristics. Therefore
before we
can begin to design the controller, it is useful to specify a few parameters. The
parameters
are:
(1)
Rise time of 0.5 seconds or less
(2)
Damping ratio of 0.32
(3)
Lag gain of 92
These parameters will
be
used as a guide to the design of a lead and lag compensator.
2.3
Lead Control
University of Notre Dame
AME 30315 Pendulum Project
8

P a g e
Lead compensator is a fairly easy and effective means to approximate
Proportional

Derivative,
PD
,
control. The idea behind PD control is that the control system should reflect the derivative of the
error of the system. Quite simply how l
arge the
control input should be should depend upon
whether the error is increasing or decreasing.
The lead compensator provides phase lead. This
shifts the poles to the left, which enhances
the stability and performance of the system.
2.3
.1 Design
The
lead compensator is of the form
(
)
⠶
)
where
is the location of the lead zero and
is the location of the lead pole.
The angle to the compensator pole must be
(
)
⠷
)
because
points on⁴heootocus satisfy
(
)
, we can use the angles from the two poles
and one zero to the desired point to compute what the angle from the compensator pole must be.
2.3.2 Lead Calculation
Calculations for the lead controller
are
done using the Matlab function
pole_loca
shown in
Appendix E
.
This function takes the location of the transfer function poles as well as a desired zero
and outputs a minimum lead pole location
.
A large part of the lead design
is
dictated by the assigned
damping ratio value. This is because,
⠸
)
where
is given by the root locus plot.
A small amount of control over the lead design
is
exercised
in the placement of the lead zero. This zero was placed as close as possible to the plant pole in
order to mitigate any adverse effects the zero would have on the response of the system.
Using a
zero at

7 a pole of

8.5
is needed to meet the
minimum design specifications.
The gain needed to
obtain the correct damping ratio was then
.
However this design did not seem optimal. To
optimize the design it was decided that the lead pole needed to be moved farther left in order to
increase it
s effect on
the
root locus. The actual placement of the pole was found through an
iterative process. This process and comments on response can be f
ound in Appendix F
. The final
design of the lead compensator is shown in Equation 9.
⠹
)
The
rootocus of the plant transferunction with⁴he effects of⁴heea搠compensator ishownn
䙩gure‴.
University of Notre Dame
AME 30315 Pendulum Project
9

P a g e
Figure 4.
Root locus with effect of the lead compensator
2.4
Lag Control
The downside to PD control and thus lead compensation is that it tends to
offset from the target
value. To account for this an addition
al
lag compensator is needed. A lag compensator
approximates
Proportional

Integral,
PI
,
control to reduce the steady

state error.
2.4
.1 Design
A lag compensator is of the same for
m
as the le
ad compensator. See Equation 6.
The pole and zero
of the lag compensator should be close together so as not to cause the poles to shift right, which
could cause instability or slow convergence. Additionally, since their purpose is to affect the low
frequ
ency range they should be near zero.
2.4.2 Lag Calculation
The lag compensator was designed to be a balance between rise time effects and stability issues. The
closer the lag compensator values were to zero the less effect they had on stability. However
, if
these values were too close to zero, they negatively affected rise time. The placement of the lag
zero, and thus the lag pole, was also determined through an iterative process. A lag zero was chosen
and then a lag pole was calculated using the lag ga
in ratio. The final design of the lag compensator is
shown in Equation 10.
(10
)
Theootocus of the transferunction with theead

lag compensator is shown⁆ gure‵. ⁔he
pointhownorrespon摳⁴o⁴heptimal gain value use搮
University of Notre Dame
AME 30315 Pendulum Project
10

P a g e
Figure 5.
Root locus of the transfer function with the lead

lag compensator
2
.
5
Discrete

time Transfer Function
Up to this point the entire controller design has been in continuous

time. However, the
microcontroller only works in discrete

time. Therefor
e the controller must be converted from
continuous

time to discrete

time.
2
.
5
.1
Conversion to Discrete

time
The Tustin method allows us to switch from continuous time to discrete time by substituting in the
following equation for
,
⠷
)
where
is the integration step size.
The Matlab c2d command can be used to make this
subst
itution
. For completeness hand substitutions for a single lead

lag control
ler were also done.
Appendix C
shows these substitutions.
3 Controller
Implementation
3.1
Controller Implementation
The controller
is
implemented in discrete

time using the substitution described above. Additional
steps are described below.
3.1.1 Non

dimensionalization
All terms relevant to the control system
are
non

dimens
ionalized. This was done for two reasons.
(1) It allow
s
for units to be taken in to account at the end of the program and (2) It allow
s
for easier
debugging because all important parameters ha
ve
to be between 0 and 1. Because we had no real
University of Notre Dame
AME 30315 Pendulum Project
11

P a g e
sense of wha
t sorts of values we should expect from the torque at various positions, it
i
s much easier
to catch an error this way.
3.1.
2
Error Calculation
The error
is
calculated by subtracting the current position from the desired position and then
multiplying by a s
cale factor
which include
s
the gain
. This value
i
s then divided by the approximate
maximum position which non

dimensionalized the error.
(
)
(
)
⠸
)
where
is the error in the current system,
and
is the gain.
3.1.
3
Transfer Function Implementation
The transfer function
i
s implemented by solving for the output value, the torque.
This torque
i
s a
function of the
current error in the system, the previous loop’s error value, the error two loops
previ
ous, the previous loop’s torque, and the torque two loops previous all scaled
by coefficients
obtained from the discrete transfer function.
For example the implemented transfer function
look
ed
something like this,
⠹
)
where
and
are the coefficients obtained from the discrete transfer function,
is the
error in the system,
is the output torque and
prev
and
prev2
denote the previous and twice previous
values,
respectively.
Additionally, a code needed to be implemented that kept the applied torque
between

400 and 400. This restriction was caused by supplied PWM.
5
System Evaluation
When the program was run the following errors were displayed
:
“
filename.c: In
function `main':
filename.c:65: warning: unused variable `i'
C:
\
usr
\
bin
\
..
\
lib
\
gcc

lib
\
m6811

elf
\
3.3.6

m68hc1x

20060122
\
..
\
..
\
..
\
..
\
m6811
elf
\
bin
\
ld.exe:ldscripts/m68hc11elfb.xbn:264:
warning: memory region eeprom not declared
”
The first warning, “unused
variable” comes from a counter that was occasionally used to stall the
program while evaluating system performance. Neither warning affects the programming of the
controller.
5
.1
Theoretical Modeling
A theoretical model of the controller was
implemented
using
the Simulink block diagram shown in
Figure 4.
University of Notre Dame
AME 30315 Pendulum Project
12

P a g e
Figure 6
.
Representative Simulink block diagram
The response of the system
is
compared against the results presented in Simulink to get a more
thorough understanding of system performance.
Figure 7 s
hows the response predicted by
Simulink.
Figure 7.
Response predicted by Simulink
5.2
Transient Evaluation
The transient response was evaluated by looking at the rise time and the overshoot. Figure 8 shows
the response of the pendulum.
University of Notre Dame
AME 30315 Pendulum Project
13

P a g e
Figure 8
.
Resp
onse of pendulum
5.2
.1 Rise Time Evaluation
Simulink predicts about a 0.2 s rise time. The pendulum itself has a rise time of approximately 0.6 s.
Although these values differ, the pendulum almost perfectly meets the designed for rise time of 0.5
s.
Th
is difference in rise time is mostly likely attributed to neglected values in the derivation of the
transfer function.
Slipping of the pendulum arm at the point of contact of the motor is not
considered. The elasticity of the pendulum
is
also not considered. Both of these factors could
contribute to a longer rise time.
5.2
.2 Overshoot Evaluation
Simulink predicts an overshoot of about 45%. The
actual
pendulum, however, has an overshoot of
nearly
150%
. Part of this error can be
attribu
ted
to the fact that the microcontroller can only take
values for torque within the 400/

400 range. The overshoot could be further exasperated by the
fact that the microcontroller only takes integer values for input torque. A more precise system that
tak
es
decimal inputs could decrease this error.
The errors are compounded by the discretized
controller system. The torque is computed by using the previous two error and torque values. If
these values
are
themselves in error then the torque could overcompe
nsate and therefore increase
the overshoot value.
5.3
Steady

State Evaluation
To evaluate the steady

state response of the system a program
is
run
and
swept through a variety of
angles. The approximate
errors at these angles are
tabulated in Table 1.
This table shows that as the
displacement increases, the error increases. This result is most likely due to the small angle
approximation made during the derivation of the transfer function. Additionally, the error seems
greatest when the desired value
is negative. This error is most likely caused because of the way the
motor applies the torque. More than likely the motor has a certain direction that it prefers to apply a
University of Notre Dame
AME 30315 Pendulum Project
14

P a g e
torque. This direction would have a stronger and more constant value than the ‘r
everse’ direction.
This is probably what is causing the greater error on the negative displacement side.
Table 1.
Steady

state error in the controller at various desired angles.
Desired Angle [
]
Error [
]
Desired Angle [
]
Error [
]

30

1.5
0
1

20

1.5
10
0.5

10

1
20
0
0
1
30
1
Figure 9 shows the response of the system as it
is
being swept through the various angles. It’s
interesting to note that although there
i
s a small amount of steady

state error associated with the
system, the rise times
and overshoots stayed relatively constant.
Additionally, the settling time of the
system can be computed from Figure 9. Simulink predicts a settling time of about 6 s. The actual
pendulum settles in about 4 s.
This lowering of
the
settling time of the
actual pendulum could be
attributed
to the increase in overshoot, by overshooting so much the controller requests additional
torque from the motor. This additional torque acts to quickly forces the pendulum to its steady

state value.
Figure 9.
Response
of pendulum at angles of

30º to 30º
6
System Verification
6.1 Position Verification
Two v
i
deos were taken of the pendulum. The first simply tests how the pendulum responds to
being displace
d
. The second video
i
s used to verify the pendulum at a
variety of different positions.
A video of the pendulum in operation can be found at the following link:
University of Notre Dame
AME 30315 Pendulum Project
1
5

P a g e
http://www.youtube.com/watch?v=Z4SMefuY2cM&feature=youtu.be
A video of the
pendulum operating at a variety of different angles can be found at:
http://www.youtube.com/watch?v=E_No3QtmH2g&feature=youtu.be
7 Conclusions
In conclusion, the controller worked
for the values assigned in the project document. However it
is
found that by increasing the gain and moving the lead pole farther left, increased
a smaller overshoot
could be attained. The theoretical model of the pendulum did a very good job of predicti
ng rise
time and settling time. However the Simulink model way under predicted the amount of overshoot
experienced by the system.
This difference in overshoot can be explained in the way that the
microcontroller inputs torque as well as assumptions made
in the creation of the transfer function.
To improve the accuracy of the controller, a few things could be done. A different microcontroller
could be used that accepts decimal inputs. Additionally, the efficiency of the evaluation of the
system could be
increased by purchasing a microcontroller that uploads significantly faster. A
majority of the analysis time for the pendulum was spent waiting for the program to upload.
University of Notre Dame
AME 30315 Pendulum Project
16

P a g e
Appendix A
University of Notre Dame
AME 30315 Pendulum Project
17

P a g e
University of Notre Dame
AME 30315 Pendulum Project
18

P a g e
Appendix B
/* Basic code skeleton for AME 30315
Project Invert
ed Pendulum

Real Code

Authors:
Bill Goodwine, April 6, 2009.
Raymond Le Grand, May 26, 2010.
Blair Rasmus, Derek Wolf, John Gallagher, November 13, 2011
*/
#include "hc11.h"
#include "mc.h"
#include <math.h>
#include "vectors.c"
#include "serial.c"
// the next two lines setup constants for direction of pendulum
#define CW 0
#define CCW 1
#define OFFSET 1777
//This is the difference between the encoder zero and the pendulum straight
up posi
tion.
//This may change slightly with each pendulum.
#define SCALE 18
/* the SCAlE constant represents the scale of the position decoder,
which is degrees per signal tick, but since the microcontroller only does
integer math,
we will define the scale as
an integer and divide by 100 every time.
*/
#define MAX_U 400
/*
The MAX_U constant represents the maximum amount of PWM signal that the
system can handle,
without the signal being so fast that there are current/voltage spikes.
It is strongly
recommended that this value not be changed.
*/
#define CONTROL_LOOP_FREQ 20 //Frequency of control loop calculations in Hz
#define CLOCK_FREQ 9830400
#define PWM_FREQ 880
// Initializing controller variables
long pos=0, R=0, R_prev=0, R_prev2=0, T_prev=0,
T_prev2=0;
//int pos_deg=0; //keeps track of current angle. 'pos' is in encoder counts,
and 'pos_deg' is in degrees*100
// Initializing PWM variables
unsigned int counts_total=((long)CLOCK_FREQ/4)/PWM_FREQ;
// counts_total is used for the counter for the
PWM interrupt.
This should
give a 880Hz interrupt.
// We divide the clock frequency by 4 because the counter increments every
fourth clock cycle when using a prescale of 1
unsigned int counts_high;
University of Notre Dame
AME 30315 Pendulum Project
19

P a g e
unsigned int counts_low;
// Initializing Timing Variab
les
unsigned int PWM_interrupt_scale=PWM_FREQ/CONTROL_LOOP_FREQ;
//Sets the
ratio of PWM interrupts to control loop interrupts
unsigned int PWM_interrupt_counter = 0; //this keeps track of ticks from
control loop interrupt, used for timing
// My variable
s
unsigned int time=0;
long k=6;
long posD=0;
int main(void)
{
//Initializing controller variables
long u=0; //This is what we use to store the calculated value for torque that
we need
int i;
// Initialize hardware
init_ports();
init_interrupts(
); //this also init's the interrupts for tracking position
set_torque(0); //starts out at 0% torque
pause(brief); // power

on delay
init_serial(); //Initialize serial communication
welcome(); //Display welcome message
pause(brief);
set_zero();
//Example
of how to write to serial port
//out_string("Here is an example of a printed number: ");
//out_string("

");
//out_unsigned_dec(24);
while(1)
{
//this checks to see if the pendulum is in top position,
//which allows for greater position accuracy
if(check
_encoder_top()){
pos = OFFSET/18; //pendulum has reached center, so reset position to zero +
OFFSET.
}
PORTA ^= 0x10; //0b01000000; //toggle pinA.4 on/off to show user that
interrupt is 20Hz with blinking LED
if(PWM_interrupt_counter>=PWM_interrupt_scale/*
control_loop_limit*/){ //this
checks to see if it is time to do 20Hz control calculations
PWM_interrupt_counter=0;
PORTA ^= 0x40; //0b01000000; //toggle pinA.6 on/off to show user that
interrupt is 20Hz with blinking LED
//////////////////////// 20 Hz Oper
ations/////////////////////////////////
//////This where you need to calculate/set the torque////////////////////
//pos_deg=pos*((int)SCALE); //calculate the current position in degrees*100
// Print position to find damping
out_unsigned_dec(time);
out_string("
");
University of Notre Dame
AME 30315 Pendulum Project
20

P a g e
if(pos<0) {
out_string("

");
out_unsigned_dec(

pos);
}
else {
out_unsigned_dec(pos);
}
carriage_return();
R = (posD

pos)*(1000/378)*k;
// R =
non

dimensionalized error in the current position
u = (8416*R

13840*R_prev + 5565
*R_prev2 +13910*T_prev

3910*T_prev2)/10000;
// T = non

dimensionalized torque
//u = 400;
if(u>1000) {
u = 1000;
}
if(u<

1000) {
u =

1000;
}
//set_torque(250);
set_torque(

u*400/1000);
R_prev2 = R_prev;
R_prev = R;
T_prev2 = T_prev;
T_prev = u;
// for (i=0;i<10;i++){
// pause(SECOND);
// }
// set_torque(

u);
// for (i=0;i<10;i++){
// pause(SECOND);
// }
///////////////End of 20Hz Operations////////////////////////////////////////
time = time+50;
if(time==7000) {
posD =

173;
}
if(time==14000) {
posD =

117;
}
if(time==21000) {
posD =

61;
}
if(time==28000) {
posD = 0;
}
if(time==35000) {
posD = 53;
University of Notre Dame
AME 30315 Pendulum Project
21

P a g e
}
if(time==42000) {
posD = 104;
}
if(time==49000) {
posD = 160;
}
}
}
}
/**** This begins the Interrupt Code ****/
// Programing interupts for PWM
void OC3_handler(void){
if(!(PORTA & OC3)){
//(portA.5==low) so set the TOC3 to the time at which we
want to end the low part of the PWM cycle
TOC3 = TOC3 + counts_low;
}else{
TOC3 = TOC3 + counts_high; // Set TOC3 to the time at which we want to end
th
e high part of the cycle
PWM_interrupt_counter++;
}
TFLG1 = OC3;
}
// Programming Interrupts for Tracking Movement
void PAI_handler(void)
{
//this checks direction of pendulum, then increments position variable
if((PORTA & 0x02 /*0b00000010*/) ==
0){
pos++;
}else{
pos

;
}
TFLG2 = PAIF; //reset interrupt flag
}
/* default interrupt handler (empty, just returns) */
void default_handler(void) {}
/**** End of Interrupt Code
****/
// Function to initialize PWM
void init_interrupts(void){
// this a
lso initializes position encoder
asm(" sei"); //disable interrupts
BAUD=BAUD9K_Turbo; //Use BAUD38K for non

turbo mode of
microcontroller
// set register to next time for each interrupt
TOC3 = TCNT + counts_total;
// arm all interrup
ts
TMSK1 = 0x0;
TMSK1 = OC3;
University of Notre Dame
AME 30315 Pendulum Project
22

P a g e
//pulse accumulator setup: used to receive signal from decoder that gives
pendulum angle
TMSK2 = 0x10; //0b00010000; this enables pulse accumulator interrupt
// acknowldege
all interrupts, in case they were already triggered
TFLG1 = OC3; //flag for pulse accumulator
TFLG2 = PAIF; //flag for pulse accumulator
PORTA = (OC3); //start off both PWM ports high
TCTL1=OL3; /*want PORTA.5 to toggle every time there's an interrupt
, but
nothing else*/
TCTL2=0xC0; //0b11000000; // this turns on error checking from h

bridge on
pinA3
asm(" cli"); //enable interrupts
}
// Function to set PWM duty cycle, which changes torque
void set_torque(long p_rate){
// Accepts desired Torq
ue percentage as an input, and uses the global
direction flag to know which direction to apply torque
while((PORTA & OC3)); //while (portA.5==high) do nothing,
//b/c want to wait until low cycle has started,
//which means that we can load next high

low
cycle without messing up PWM
period
// This calculation is 50% + (p_rate%).
// 50% PWM = 0torque, and 95% PWM is Max torque in CW direction.
p_rate=((unsigned int)(counts_total/10)*p_rate)/(unsigned int)10;
counts_low=(unsigned int)counts_total/(unsigned
int)2

p_rate/(unsigned
int)10; //divide p_rate by ten to get it as 0

40 instead of 0

400
counts_high = counts_total

counts_low;
}
// Read direction signal
unsigned char check_encoder_dir(void){
unsigned char dir_flag;
if((PORTA & 0x02/*0b00000010*/) ==
0){
dir_flag=0;
}
else{
dir_flag=1;
}
return(dir_flag);
}
// Read vertical position sensor (tells when pendulum is vertical)
int check_encoder_top(void){
int top_flag;
//this checks pin A.2 to see if pendulum is vertical
if(!(PORTA & 0x04/*0b00000100*/)
){ //note that this line was inverted to
account for top signal being inverted
top_flag=0;
University of Notre Dame
AME 30315 Pendulum Project
23

P a g e
}
else{
top_flag=1;
}
return top_flag;
}
// Pause function waits for specified number of clock cycles before
continuing
void pause(unsigned int duration)
{
unsigned int time;
time=duration;
// small delay routine
while(time>0)
time

;
}
// Initialize ports
void init_ports(void)
{
/* enable pulse accumulator on PA7, falling edge
PA3 is input capture IC4 */
PACTL = 0x40; //0b01000100;
//PACTL = 0b00000100
; //this is the code to disable it
PORTA = 0xCF; //0b11001111; // disable H

bridge, photointerrupter
DDRD =
0x07; //0b00000111; // sets D0

D2 as outputs, the rest are inputs
PORTD = 0x04; //0b00000010; // clears Port D
PACNT = 0x00; //0b00000000; // clear
Pulse Accumulator
}
// Function to prompt user to move pendulum through zero to initialize angle
counter
void set_zero(void){
int de_ch=0,index=0;
while((PORTD & 0x08 /*0b00001000*/) == 0);
// make sure ok button has been
released
out_
string("Move Through Vertical Position");
carriage_return();
pause(SECOND);
pause(SECOND);
pos=0;
out_string("DIR
POS ");
carriage_return();
while(check_encoder_top()==0){ //checks to see if pendulum is at top
position
//if pendulum not
at the top, then keep looping
de_ch=check_encoder_dir(); //check the direction of the pendulum
University of Notre Dame
AME 30315 Pendulum Project
24

P a g e
if(index==1000){ // only updates screen every 1000 iterations
out_unsigned_dec(de_ch); //print out direction
//output position, account for positive/negative nu
mbers
if(pos>=0){
out_string(" ");
out_unsigned_dec(pos*SCALE/100);
}else{
out_string("

");
out_unsigned_dec(

pos*SCALE/100);
}
out_char(NEWLINE); //go back to column zero, but same line
index=0;
}else{
index++;
}
}
//pendulum has reached the top,so
stop looping
pos = OFFSET/18;
carriage_return();
out_string("you finished!");
carriage_return();
pause(SECOND);
}
// Displays Welcome message
void welcome()
{
pause(brief);
out_string("AME 30315");
out_string(" Pendulum Project ");
carriage_return();
car
riage_return();
}
// initialize MicroStamp 11.
This function is called by _start, which is
defined in crs0.s
// A __premain() is created by default by GCC compiler, but we have
overwritten the default
// so that we can move the register block, which mus
t be done within first 64
bus cycles
void __premain(void)
{
*(unsigned char volatile *)(0x3D) = 0x01; //Register block will start at
0x1000 instead of default 0x0000
TMSK2 = 0x0C; //0x0D; // =1101b, a prescale of 4 for the output compare,
// which
must be set within 64 cycles of microcontroller reset,
// which is why we set it here
CONFIG = 0x04;
// disable COP timer
}
Σχόλια 0
Συνδεθείτε για να κοινοποιήσετε σχόλιο