Engle

MATLAB Based DC Motor Control Lab
Benjamin J . Engle
Advisor: Dr. J ohn Watkins

Wichita State University
College of Engineering
December 4, 2007

MATLAB Based DC Motor Control Lab Page i
Table of Contents

1.0 Introduction ............................................................................................. 1.1
2.0 Quanser Engineering Trainer Hardware ................................................. 2.1
2.1 PIC 16F877 Microcontroller ................................................................. 2.2
2.2 Position Measurement ......................................................................... 2.3
2.3 Current Measurement .......................................................................... 2.3
3.0 Microcontroller C Code............................................................................ 3.1
3.1 Signal Types ........................................................................................ 3.1
3.2 Angular Rate Calculation ..................................................................... 3.1
3.3 Sample Rate Specification ................................................................... 3.2
3.4 Student Modified Control Function ...................................................... 3.2
4.0 MATLAB Graphical User Interface .......................................................... 4.1
4.1 Graphical User Interface Options ........................................................ 4.1
4.2 Signal Definition ................................................................................... 4.2
4.3 Digital Scopes and Plot Area ............................................................... 4.2
4.4 Operation Buttons ................................................................................ 4.2
4.5 Step Response Calculations ................................................................ 4.2
4.6 Output MATLAB Workspace Variables ................................................ 4.2
5.0 Help File .................................................................................................. 5.1
6.0 Examples ................................................................................................ 6.1
6.1 PID Position Controller ........................................................................ 6.2
6.2 PI Angular Rate Controller ................................................................... 6.5
7.0 Conclusion .............................................................................................. 7.1
8.0 Future Work ............................................................................................ 8.1
9.0 References .............................................................................................. 9.1
Appendix A: PIC Microcontroller C Code ...................................................... 9.1
Appendix B: MATLAB QET Interface Code ................................................. 9.22
Appendix C: PIC C Code Flow Charts ........................................................... 9.1
MATLAB Based DC Motor Control Lab Page 1.1
1.0 Introduction
Wichita State University currently offers a variety of control courses. These
range from introductory courses to those covering advanced topics such as
optimal and robust control. A course in digital control is now being offered and
has received great student interest. In a recent semester the 50 available seats
were reserved on the first day of registration. The number of available seats was
increased to 80, but these were quickly filled and many students still had to be
turned away. This digital control class covers the underlying theory of digital
control and the design of various classical controllers, such as PD, PID, PI, and
lead compensators. However, there is currently not a lab component available to
complement this course.

Many students struggle to understand the abstract concepts used in control. The
understanding of concepts such as frequency response, poles and zeros,
stability, and controller implementation can be greatly enhanced by a lab
component [2]. Schools that have implemented a control laboratory have
reported high student ratings and good industry feedback [2] [3]. In addition, a
lab is of tremendous value when teaching topics such as modeling,
implementation, and operation that are needed to properly prepare a student to
work in industry [1].

In recent years this lack of a control lab has been addressed in various ways at
many universities. For example, the University of Illinois addressed the issue
with a 2000 square foot interdepartmental control lab with a full time lab manager
[2]. The University of North Florida addressed the issue by creating an electrical
engineering specific control lab with 13 lab stations [3]. Work has also been
completed at the University of Minnesota to develop take home lab kits in order
to further reduce the cost and space required for the lab [4].

Wichita State University currently owns a number of Quanser Engineering
Trainers (QET). Among other components, this trainer contains a DC motor with
an optical angle encoder, a programmable microcontroller with a serial port
interface, and the necessary circuitry to power and interconnect these main
components. A review of the QET is available in [8] and further discussion of
the hardware is presented in Section 2 of this report. Custom code can be
created for the installed microcontroller that can interface with the hardware as
well as communicate with a PC over the serial port.

The goal of this project was to write C code for the microcontroller installed in the
QET and to interface with it over a serial port on a standard PC using a graphical
user interface in MATLAB. The purpose of this is to provide a low cost solution
that will allow a student to implement and test a digital controller design. In this
manner the student can obtain hands-on experience with implementing a digital
controller without burden of writing all of the microcontroller and MATLAB code
from scratch.
2.0 Quanser Engineering Trainer Hardware
The Quanser engineering trainer (QET) DC motor control trainer (DCMCT) is a
unit produced by Quanser Consulting of Markham, Ontario, Canada. A picture of
the QET DCMCT is shown in Figure 1. The Wichita State University units come
with an installed QIC (Quanser Integrated Controller) board that contains a PIC
16F877 microcontroller. This unit also comes equipped with a high quality
Maxon DC motor, a linear power amplifier that allows the QIC to apply a control
voltage to the motor, a 1024 line optical encoder to measure the motor angle, a
series resistor to allow the QIC to measure the current being used by the motor,
and the hardware necessary to allow the microcontroller to interface with a PC
through the provided serial port. Figure 2 shows a block diagram of the QET as
it is used in this project for motor angular position control.

The trainer supports other capabilities that are not being used in this project.
These include the ability to interface with Quanser HIL (hardware in the loop)
boards through the provided analog ports, the availability of a potentiometer to
read the angle of the motor, and the availability of a breadboard to prototype
custom circuits.

Figure 1 Quanser Engineering DC Motor Control Trainer
Figure 2 System Block Diagram for Angular Position Control
2.1 PIC 16F877 Microcontroller
The Microchip PIC 16F877 microcontroller installed on the QIC is a high
performance RISC (reduced instruction set computer) CPU (central processing
unit). The microcontroller supports clock speeds up to 20 MHz, hardware
interrupts, and USART (Universal Synchronous/Asynchronous Receiver
Transmitter) for serial port communication [11]. Additionally, as it is installed on
the QIC it supports low power programming through the available serial port.

In this project the maximum clock speed of 20 MHz was used to provide
minimum calculation time and maximum control frequency. For this project the
16F877s 16 bit timer/interrupt is used to run a digital controller at the desired
clock frequency. Information is passed to and from the 16F877 via the serial port
at a baud rate of 115,200 bits/second.

The code written for the 16F877 was written using the PCW PIC C compiler
produced by Custom Computer Services, Inc. (CCS). Using C code provides a
higher level software interface to the hardware than could be accomplished using
assembly language. Also, because the microcontroller code written for the
trainer by Quanser is in C, this allows some of their code to be reused.
Linear
Power
Amplifier
Controller
D(z)
(t)
V(t)
Signal
Generator
-
+
D/A
PC
MATLAB
GUI
z
-1

Differ-
entiator
Serial
Interface
T
[k]
V(t) V[k]
DC Motor
PIC 16F877
E[k]
1024 Line
Quadrature
Encoder
[ ] k
[k-1]
d
[k]
Where:

d
=Desired motor angle
E =Error
V =Motor voltage/control voltage
=Motor angle

=Motor angular rate

2.2 Position Measurement
The angular position of the motor is measured by a 1024 line quadrature angular
encoder with a resolution of approximately 0.0879. The encoders
measurement is made available to the 16F877 as a 24 bit integer value. Note
that the encoder is initialized to zero at the start of the code so that the angle
measured is the difference from the initial motor angle. Due to the resolution of
the encoder and the use of the 24 bit counter, the range of values that can be
read by the encoder is approximately 4096*2.
2.3 Current Measurement
The current being drawn by the motor is read using a series resistor and is
passed to the microcontroller as an analog voltage. This voltage is read using
the 16F877s 10 bit analog to digital converter and is used to calculate the motor
current. The resolution of the current measurement is approximately 0.5 mA
10%.

3.0 Microcontroller C Code
C programming code was written specifically for this application. A listing of this
code is available in Appendix A and the flowcharts for this code are available in
Appendix C. This code implements all of the control logic as well as the
necessary routines to communicate with the MATLAB code described in Section
4.0.

There are many functions in the C code, most of which should not need to be
modified. These functions provide capabilities whose purpose is unlikely to
change, such as serial communication, control timing, reference signal
generation, and hardware interaction. This allows the student to concentrate on
the programming of their control design without the overhead of learning
hardware implementation specific code. The only function that must be changed
is the control function that is described in Section 3.4.
3.1 Signal Types
Various signal types are supported by the code. These signal types are
described in detail along with figures in the help file [12]. The available signal
types are:

Impulse
Step
Ramp
Parabolic
Triangle Wave
Saw Tooth Wave
Square Wave
Sine Wave
Cosine Wave
Stair Step
3.2 Angular Rate Calculation
The angular rate is determined by finding the first derivative of the position
measured by the angular encoder. Two different methods were implemented in
the C code for this. The first uses a simple finite difference calculation as shown
in (3.1).

T
last Now

(3.1)

The second method is the same as that used by Quanser and is derived by
taking the bilinear transform of the continuous high pass filter shown in (3.2).

( )
1 300+
=
s
s
last Now
(3.2)

Both methods are implemented in the code and are made available by the GUI.
Either method may be used by the student. Based upon work with both methods
there is little difference in either the noise or results of the two methods.
3.3 Sample Rate Specification
The top of the C code contains many constants and variable declarations that
should not be modified by the student. However, there is one that must be
modified by the student. This is the constant named fs that defines the
sampling frequency, in Hz. For example, to define a sampling rate of 150 Hz the
fs definition should be modified as follows:

#define fs 150

Due to hardware limitations and the 20 MHz clock rate of the 16F877, the lowest
sample rate that can be used is 10 Hz. In addition, due to the computation time
required to complete a control step, the maximum sample rate is approximately
150 Hz. For simple signal types, such as a step signal, higher sample rates may
be achievable. If sample rates higher than 150 Hz are desired then an
oscilloscope would be necessary to verify that the desired sample rate can be
achieved for a particular signal type and controller.
3.4 Student Modified Control Function
The only function that must be updated by the student to implement their desired
controller is the function named RunController. This function will be called at
the specified sample rate and allows the implementation of digital controllers.
Much of the code in this function does things that will nearly always be required
and thus probably won't need to be changed. This is the code that does things
such as generate the new reference signal, reads the angle from the encoder,
reads the motor current from the analog to digital converter, and calculates the
angular rate.

However, there are areas of this function that will need to be modified to
implement whatever controller the user desires. The first area is labeled
controller parameters and stores any constant parameters that are needed for
the controller. These are typically things such as controller gains, poles, and
zeros.

The second area of the function that will need to be modified is labeled controller
logic and is where the student implements their control logic. These
modifications will depend upon the specific controller being implemented.
Examples of what may need to be done in this area are available in Section 6.0.

4.0 MATLAB Graphical User Interface
A graphical user interface was written to simplify the interface with the QET. A
picture of the GUI is shown in Figure 3. The GUI allows the user to specify the
desired signal type and run options for the QET. The user can also create a
standard set of plots from the GUI. The GUI contains an input area for options,
an input area for the signal definition, an area for the digital scopes, a plot area
where data are plotted while the QET is being run, and four buttons that operate
the form.

Figure 3 MATLAB QET Graphical User Interface

A help file has been written that details the operation of the graphical user
interface. The following sections give a basic overview of the operation of the
GUI. However, additional information and details can be found in the help file.
4.1 Graphical User Interface Options
The options area allows the user to specify various options for the input and
outputs of the GUI. These options are as follows.

Run Time - This is the total time, in seconds, that the QET should run.
Control Type - This tells the GUI what type of control the user has
implemented on the QET. This allows the GUI to place the reference
signal on the proper plot when the Make Plots button is pressed and to
show the correct data in the plot area while the QET is running.
Angle units - This allows the user to specify the angle units to be displayed
on the scopes and in the output plots. Also, if the user has selected the
rate or position control type then this also inputs the units for the input
magnitude.
Frequency units Some signal types require a frequency input (such as a
sine wave). For these signal types this option tells the GUI whether the
input frequency is in Hz or rad/s.
4.2 Signal Definition
The signal definition input area allows the user to specify characteristics of the
desired signal. The inputs are as follows.

Signal Type - This allows the user to specify the desired signal type. The
available signal types are listed in Section 3.1.
Magnitude - This allows the user to specify the signal magnitude or rate,
as applicable. The units of this input (degrees or radians) for position or
rate control are specified in the options frame. Further information about
what this value represents for the various signal types is available in the
help file.
Frequency - This allows the user to specify the signal frequency (if
applicable). If this isn't applicable to the selected signal type then this input
will be disabled. The units of this input are specified in the options frame.
4.3 Digital Scopes and Plot Area
The digital scopes frame contains the current values of angle, voltage, and
current while the QET is running. The plot area displays the current reference
signal. In addition, the actual results from the QET will be displayed on this plot.
The data that is displayed on this plot is determined by the selection made for the
control type in the options input area.
4.4 Operation Buttons
There are four buttons at the bottom of the GUI. The first button is labeled Run
and runs the QET using the current input options and signal definition. The
second button is labeled Make Plots and will create plots of control voltage,
motor current, motor angular rate, and motor angle versus time for the last
completed run. The third button is labeled Help and will display the help file
when pressed. The fourth button is labeled Exit and will close the GUI.
4.5 Step Response Calculations
When the user runs a step signal then the percent overshoot, settling time, and
steady state error will be calculated and displayed in the plot area. Also, if the
user clicks on the Make Plots button then the step response characteristics will
also be displayed on the resulting plots.
4.6 Output MATLAB Workspace Variables
When the user runs the QET through the GUI various output variables are placed
in the base MATLAB workspace. These variables contain the input
characteristics and output data from the QET. This allows the student to do
custom post processing on the control results. Further details of these variables
are available in the help file.

5.0 Help File
An HTML help file has been generated to assist in using the use of the QET and
MATLAB interface [12]. A picture of this help file is available in Figure 4. This
help file contains an overview of the QET hardware, information on running the
QET from MATLAB, an overview of the MATLAB GUI, and some examples on
controller implementation. This help file is available to the user by pressing the
help button on the MATLAB GUI.

Figure 4 Help File

6.0 Examples
This section contains examples of both a proportional integral-derivative (PID)
angular position controller and a proportional integral (PI) angular rate controller.
Only the implementation of these controllers is discussed as the design of these
controllers is outside the scope of this project.

When implementing a controller into the C code the user must first set the
sample rate at the top of the code to the desired value as shown in section 3.3.
Next the user must modify the function named RunController to implement their
control logic. Most of the code in this function should remain unmodified as it
completes calculations that will always be necessary. However, other parts of
the code must be changed to implement the controller and it is these changes
that are discussed in the following examples.

The variables that will always be used in the RunController function are listed
below. The code that calculates these variables should not be modified.

AngleLast The angle of the motor, in radians, at the previous time step
Angle The angle of the motor, in radians, read from the angular encoder
at the current time step
RefSigLast This is the reference signal at the previous time step. The
meaning of the reference signal will depend on the control type that is
being completed. For example, if the implemented controller is intended
to control angular position then this reference signal would be the desired
angular position.
RefSig The reference signal at the current time step
Current The current through the motor, in milliamps, at the current time
step
AngleFD The first derivative of the motor angle, in radians/second, at the
current time step using (3.1)
AngleFDFilt - The first derivative of the motor angle, in radians/second, at
the current time step using the bilinear transform of the continuous time
filter shown in (3.2)
Volt The motor voltage, in volts, that should be applied to the motor at
the current time step

6.1 PID Position Controller
The motor position may be controlled using a proportional integral-derivative
controller. This control should greatly reduce or eliminate any steady state error
that may be present using a gain or proportional derivative controller. The
downside to this method is that adding an integrator will degrade the transient
performance of the system.

Below is the transfer function of a proposed proportional integral-derivative
position controller that was designed for the QET.

( )
( )
( )
( )( )
( ) 1 - z z
a2 - z a1 - z K
= =
z E
z V
z G
c
(5.1)

where
V(z) =Motor voltage
E(z) =Angle Error =Desired Angle - Actual Angle
K =6.6502
a1 =0.8751
and a2 =0.9190

This transfer function can be implemented using the following difference
equation:

[ ] [ ] [ ] ( ) [ ] [ ] { } 2 - k E a2 a1 1 - k E a2 a1 - k E K 1 - k V k V + + + = (5.2)

The MATLAB simulation of this controller predicts a 20% overshoot and a settling
time of 0.64 seconds as shown in the following figure.


Figure 5 Predicted PID Position Control Example Results
To implement this controller on the QET the user should define the controller
gain and zero locations at the top of the RunController function in the "controller
parameters" section. The following code shows an example of this.

//PID controller parameters
const float K =6.6502;
const float a1 =0.8751;
const float a2 =0.9190;

The code that calculates the reference signal, motor angle, and motor angular
rate is already completed for the user as discussed in the introduction to this
section. Thus, once the controller parameters are defined the next step is to
write the code to calculate the error at the current time step and save the
necessary number of error measurements from previous time steps. From (5.2)
it can be seen that for this example the errors from the current time step and
previous two time steps are needed. The following code snippet is an example of
what may be used to store the errors.

Errors[2] =Errors[1]; //Error two time steps ago
Errors[1] =Errors[0]; //Error at last time step
Errors[0] =RefSig - Angle; //Error at current time step

For a controller with an integral term the user must also save the last control
voltage. The following code snippet is an example of storing the last control
voltage.

VoltLast =Volt;

For the previous two code snippets to be valid the user must define both the
three element array Errors and the variable VoltLast at the top of the function.
The following is an example of the necessary variable definitions.

static float Errors[3];
float VoltLast;

Once the controller parameters are defined the user must add the PID control
logic to the "controller logic" section of the RunController function. The following
code shows the implementation of difference equation (5.2).

Volt =VoltLast +K*(Errors[0] - (a1+a2)*Errors[1] +a1*a2*Errors[2]);

Once these code changes have been implemented the code can be compiled
and loaded onto the 16F877 microcontroller as discussed in the help file. The
GUI can then be used to quickly obtain the step response of the closed loop
system. The results obtained should be similar to those shown in Figure 6.
These results show that the overshoot is approximately the 20% that was
predicted. Also, as expected, there is no steady state error. However, the settling
time is more than the 0.64 seconds that was anticipated, likely due to unmodeled
friction in the system.

Figure 6 PID Example Control Results

6.2 PI Angular Rate Controller
The motor angular rate may be controlled using a proportional integral controller.
Below is the transfer function of a proposed proportional integral rate controller
that was designed for the QET.

( )
( )
( )
( )
( ) 1 - z
a - z K
= =
z E
z V
z G
c
(5.3)

where
V(z) =Motor voltage
E(z) =Angle Error =Desired Angle - Actual Angle
K =0.038955
and a =0.7778

This transfer function can be implemented using the following difference
equation:

[ ] [ ] [ ] [ ] { } 1 - k E a - k E K 1 - k V k V + = (5.4)

The MATLAB simulation of this controller predicts a 5% overshoot and a settling
time of 0.74 seconds as shown in the following figure.

Figure 7 Predicted PI Rate Control Example Results
To implement this controller on the QET the user should define the controller
gain and zero locations at the top of the RunController function in the "controller
parameters" section. The following code shows an example of this.

//PI rate controller parameters
const float K =0.038955;
const float a =0.7778;

The code that calculates the reference signal, motor angle, and motor angular
rate is already completed as discussed in the introduction to this section. Thus,
once the controller parameters are defined the next step is to write the code to
calculate the error at the current time step and save the necessary number of
error measurements from previous time steps. From (5.4) it can be seen that
only the error from the current and previous time steps are needed. The following
code snippet is an example of what may be used to store the errors.

Errors[1] =Errors[0]; //Error at last time step
Errors[0] =RefSig - AngleFD; //Error at current time step

For the previous code snippets to be valid the user must define the two element
array Errors at the top of the function. The following is an example of the
necessary variable definition.

static float Errors[2];

Once the controller parameters are defined the user must add the PI control logic
to the "controller logic" section of the RunController function. The following code
shows the implementation of difference equation (5.4).

Volt =VoltLast +K*(Errors[0] - a*Errors[1])

Once these code changes have been implemented the code can be compiled
and loaded onto the 16F877 microcontroller as discussed in the help file. The
GUI can then be used to quickly obtain the step response of the closed loop
system. The results obtained should be similar to those shown in Figure 8. This
figure shows that the overshoot is less than the 5% that was predicted. Also, the
settling time is less than the 0.74 seconds that was anticipated. These better
than expected results are likely due to unmodeled friction in the system. Note
that this step response was run with a magnitude of 16 radians/second (8
revolutions per second) to reduce the impact of the unmodeled friction on the
system.


Figure 8 PID Example Control Results

7.0 Conclusion
Wichita State University currently offers various control courses, including a
digital control course. However, there is currently not an available lab
component for this digital control course. The goal of this project was to create a
system that would allow a student to quickly implement and test a controller
design. Toward this goal custom C code was written for the PIC microcontroller
installed on the Quanser Engineering Trainers currently owned by Wichita State
University. The necessary code modifications to implement a user designed
controller were also discussed. A MATLAB graphical user interface was created
that allows simple interaction with the microcontroller for the purposes of testing
the controller performance. An HTML help file was also generated to assist in
the use of the C code and MATLAB GUI. Finally, examples of the
implementation of PID angular position and PI angular rate controllers were
discussed.

8.0 Future Work
Although much has been accomplished towards the development of a control lab
there is additional work that may prove helpful. The first would be to develop a
lab manual. This manual should have topics in it similar to those covered by
Quanser in their lab manual, such as system modeling and controller design.

Also, in this project only simple controllers were implemented. These were PD
position, PID position, and PI rate controllers. In the future it would be interesting
to develop and implement more advanced controllers such as two degree of
freedom and robust controllers. Even a neural net controller may be
implemented if it is simple enough to fit in the remaining ROM space on the PIC.

Finally, serial port communication errors are only minimally handled in the current
PIC C code. Additional handshaking between MATLAB and the PIC may be
helpful in ensuring more reliable operation. Due to the slow speed of MATLAB
relative to the serial communication it may prove useful to move the serial
communication itself out of MATLAB and into specifically written compiled code.
Microsofts .NET framework provides a class for operating the serial port that
may prove useful for this enhancement.

9.0 References

[1] strm, Karl J ohan. Challenges in Control Education. Lund University,
Sweden, 2006.

[2] Alleyne, Andrew G., Block, Daniel J ., Meyn, Sean P., Perkins, William R., and
Spong, Mark W. An Interdisciplinary, Interdepartmental Control Systems
Laboratory. IEEE Control Systems Magazine, v25, n1, February 2005.

[3] Choi, Chiu H. Undergraduate Controls Laboratory Experience. Proceedings
of the 2004 American Society for Engineering Education Annual Conference and
Exposition, J un 20-23 2004.

[4] Durfeee, William, Li, Perry, and Waletzko, David. Take-Home Lab Kits for
System Dynamics and Controls Courses. Proceedings of the 2004 American
Control Conference, J une 30-J uly 2, 2004.

[5] Dixon, Warren E., Dawson, Darren M., Costic, B. T., Queiroz, Marcio S. A
MATLAB-Based Control Systems Laboratory Experience for Undergraduate
Students: Toward Standardization and Shared Resources. IEEE Transactions
on Education, v 45, n 3, Aug. 2002.

[6] strm, Karl J ., and Apkarian, J acob. A Laptop Servo for Control Education.
IEEE Control Systems Magazine, v24, n5, October 2004.

[7] Products in Control: Engineering Trainer, IEEE Control Systems Magazine,
v24, n2, April 2004.

[8] Berstein, Dennis S. The Quanser DC Motor Control Trainer. IEEE Control
Systems Magazine, v25, n3, J une 2005.

[9] User Guide: Quanser Engineering Trainer DC Motor Control, Quanser
Consulting Inc.

[10] PICmicro Mid-Range MCU Family Reference Manual. Microchip
Technology Inc., December 1997.

[11] PIC16F87X Data Sheet. Microchip Technology Inc., 2001.

[12] Engle, Benjamin. WSU QET Interface Help System. November 2007.

MATLAB Based DC Motor Control Lab Page B.9.1

Appendix A: PIC Microcontroller C Code
Below is a listing of the microcontroller routines developed for this project.
The routines in the header files q_config.h, encoder.h, daout.h, and
init_qmt.h were provided with the QET by Quanser and only slightly modified.
As this code is shown it is setup for the PID position control example of
Section 6.2.

Main Routines

#i ncl ude " 16f 877. h"
#i ncl ude " q_conf i g. h"
#i ncl ude " encoder . h"
#i ncl ude " daout . h"
#i ncl ude " i ni t _qmt . h"

#i ncl ude <mat h. h>
#i ncl ude <st r i ng. h>
#i ncl ude <st dl i b. h>

#ZERO_RAM

#def i ne CLK 20000000 / / Cl ock f r equency

#use del ay( cl ock = CLK) / / Set cl ock f r equency
#use r s232( baud=115200, bi t s=8, par i t y=N, xmi t =PI N_C6, r cv=PI N_C7, er r or s) / / Set up
ser i al por t

/ / User def i ned const ant s
#def i ne f s 50 / / sampl i ng f r equency i n Hz ( 10 Hz i s t he sl owest suppor t ed
f r equency)
#def i ne t s ( 1/ ( f l oat ) f s) / / sampl i ng per i od i n second

/ / Si gnal t ypes
#def i ne SI GI MP 1 / / I mpul se si gnal
#def i ne SI GSTP 2 / / St ep si gnal
#def i ne SI GRMP 3 / / Ramp si gnal
#def i ne SI GPAR 4 / / Par abol i c si gnal
#def i ne SI GTRI 5 / / Tr i angl e wave si gnal
#def i ne SI GSAW6 / / Sawt oot h wave si gnal
#def i ne SI GSQA 7 / / Squar e wave si gnal
#def i ne SI GSI N 8 / / Si ne wave si gnal
#def i ne SI GCOS 9 / / Cosi ne wave si gnal
#def i ne SI GSTR 10 / / St ai r wave si gnal

/ / Si gnal i nf or mat i on
i nt 8 Si gType; / / Cur r ent si gnal t ype ( see above def i ni t i ons)
f l oat Si gMagRat e; / / Si gnal magni t ude or r at e, dependi ng on si gnal t ype
f l oat Si gFr eq; / / Si gnal f r equency ( hz) , i f appl i cabl e t o cur r ent si gnal t ype

/ / Gl obal var i abl es
shor t i nt DoCont r ol ; / / Fl ag as t o whet her t o do cont r ol or not
shor t i nt Runni ng; / / Fl ag as t o whet her we ar e r unni ng or not

l ong EndCnt ; / / Count t o end t he pr ogr amwhen r eached

l ong Ti meCnt ; / / Last cont r ol t i me i ndex
f l oat Ref Si g; / / Cur r ent r ef er ence si gnal ( desi r ed angl e or angul ar vel oci t y)
f l oat Ref Si gLast ; / / Last r ef er ence si gnal
f l oat Angl e; / / Last r ead angl e i n r adi ans
f l oat Vol t ; / / Last set mot or vol t age out put
f l oat Cur r ent ; / / Last r ead mot or cur r ent i n mi l l i amps
f l oat Angl eFD; / / Last cal cul at ed mot or angl e f i r st der i vat i ve ( r ad/ s) usi ng
t he di r ect met hod
f l oat Angl eFDFi l t ; / / Last cal cul at ed mot or angl e f i r st der i vat i ve ( r ad/ s) f r om
t he f i l t er i ng met hod

l ong Squar eCount ; / / Next count t o f l i p squar e wave on
shor t i nt Tr i Sgn; / / Cur r ent si gn of t r i angl e wave ( 0 means negat i ve and 1 means
posi t i ve)

l ong pr eset _count er ; / / Val ue t o set count er t o at each i nt er r upt

#def i ne BUFSI Z 5 / / Reci evi ng buf f er si ze ( DON' T CHANGE! )
char RecDat Buf [ BUFSI Z] ; / / Reci evi ng dat a buf f er
l ong i RecDat a; / / Next avai l abl e l ocat i on i n dat a r eci eve buf f er
shor t i nt RecDat Avai l ; / / Fl ag as t o whet her dat a has been r eci eved
i nt 1 RecEr r ; / / Fl ag as t o whet er a r eci eve er r or has occur ed

i nt 1 Fi r st Ti me;

/ / Set up angul ar r at e f i l t er par amet er s
f l oat Omega = 300. ;
f l oat ca;
f l oat cb;

/ / Funct i on pr ot ot ypes
f l oat Cal cRef Si gnal ( l ong Ti meCnt ) ;
f l oat Get Tr i ( ) ;
f l oat Get Squar e( ) ;

/ / #%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%
/ / The f ol l owi ng r out i ne wi l l be cal l ed at t he sampl e r at e t o gener at e t he new
/ / r ef er ence si gnal as wel l as compl et e t he act ual cont r ol and wr i t e t he out put
voi d RunCont r ol l er ( )
{

f l oat Angl eLast ;
f l oat Vol t Last ;
f l oat Angl eFDFi l t Last ;
st at i c f l oat Er r or s[ 3] ;

/ / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
/ / St ar t of cont r ol l er par amet er s
/ / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

/ / PI D cont r ol l er par amet er s
const f l oat k = 6. 650160854277;
const f l oat a1 = 0. 87508921708793;
const f l oat a2 = 0. 91901606680500;

/ / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
/ / End of cont r ol l er par amet er s
/ / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

DoCont r ol = 0;

/ / Tur n on LED
out put _hi gh( LED3) ;

/ / I ncr ement out put count ( as l ong as t hi s i sn' t t he f i r st t i me)
i f ( ! Fi r st Ti me)
Ti meCnt ++;

/ / Read t he encoder
Angl eLast = Angl e;
Angl e = r ead_encoder ( ENCODER0, X_AXI S) ;

/ / Save cur r ent r ef er ence si gnal
Ref Si gLast = Ref Si g;

/ / Get new desi r ed angl e
Ref Si g = Cal cRef Si gnal ( Ti meCnt ) ;

/ / Read t he cur r ent
set _adc_channel ( 2) ;
del ay_us( 20) ;
Cur r ent = ( ( f l oat ) Read_ADC( ) - 512. 0) *0. 00543*1000; / / Read ADC and conver t t o
mi l l i amper e

/ / Cal cul at e f i r st der i vat i ve of angl e
i f ( ! Fi r st Ti me)
{
Angl eFD = ( Angl e- Angl eLast ) / t s;

Angl eFDFi l t Last = Angl eFDFi l t ;
Angl eFDFi l t = ca*( Angl e- Angl eLast ) - cb*Angl eFDFi l t Last ;
}

/ / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
/ / St ar t of cont r ol l er l ogi c
/ / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

/ / Save cur r ent vol t age
Vol t Last = Vol t ;

/ / Set al l er r or s t o zer o t he f i r st sampl e per i od
i f ( Fi r st Ti me) memset ( Er r or s, 0, si zeof ( Er r or s) ) ;

/ / Save er r or s f or posi t i on cont r ol
Er r or s[ 2] = Er r or s[ 1] ; / / Er r or t wo t i me st eps ago
Er r or s[ 1] = Er r or s[ 0] ; / / Er r or at l ast t i me st ep
Er r or s[ 0] = Ref Si g - Angl e; / / Er r or at cur r ent t i me st ep

/ / Cont r ol l er l ogi c
Vol t = Vol t Last + k*( Er r or s[ 0] - ( a1+a2) *Er r or s[ 1] + a1*a2*Er r or s[ 2] ) ; / / PI D

/ / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
/ / End of cont r ol l er l ogi c
/ / - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -

/ / Oupt ut cont r ol l er vol t age
daout ( Vol t ) ;

/ / Tur n LED of f
out put _l ow( LED3) ;

}

/ / #%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%
f l oat Cal cRef Si gnal ( l ong Ti meCnt )
{

f l oat NewSi g, t ;
f l oat Tr i gX;

t = Ti meCnt *t s;

/ / Fi gur e out t he mul t i pl i er t o t he t r i g si gnal s
i f ( Si gType == SI GSI N | | Si gType == SI GCOS)
{
Tr i gX = t *Si gFr eq;
Tr i gX = Tr i gX - f l oor ( Tr i gX) ;
}

swi t ch ( Si gType)
{

/ / I mpul se si gnal
case SI GI MP:
i f ( Ti meCnt == 0)
NewSi g = ( Si gMagRat e/ t s) ;
el se
NewSi g = 0;

br eak;

/ / St ep si gnal
case SI GSTP:
NewSi g = Si gMagRat e;
br eak;

/ / Ramp si gnal
case SI GRMP:
NewSi g = Si gMagRat e*t ;
br eak;

/ / Par abol i c si gnal
case SI GPAR:
NewSi g = Si gMagRat e*t *t ;
br eak;

/ / Tr i angl e wave si gnal
case SI GTRI :
NewSi g = Get Tr i ( ) ;
br eak;

/ / Sawt oot h wave si gnal
case SI GSAW:

NewSi g = 2*Si gMagRat e*( ( t *Si gFr eq) - f l oor ( ( t *Si gFr eq) +0. 5) ) ;
br eak;

/ / Squar e wave si gnal
case SI GSQA:
NewSi g = Get Squar e( ) ;
br eak;

/ / Si ne wave si gnal
case SI GSI N:
NewSi g = Si gMagRat e*si n( Tr i gX*2*PI ) ;
br eak;

/ / Cosi ne wave si gnal
case SI GCOS:
NewSi g = Si gMagRat e*cos( Tr i gX*2*PI ) ;
br eak;

/ / St ai r wave si gnal
case SI GSTR:
NewSi g = Si gMagRat e*( f l oor ( ( t *Si gFr eq) +0. 0000001) +1) ;

}

r et ur n NewSi g;

}

/ / #%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%
f l oat Get Tr i ( )
{

f l oat NewSi g, Rat e;

NewSi g = 0;
el se
{
Rat e = 4*Si gMagRat e*Si gFr eq;

i f ( ! Tr i Sgn)
Rat e = - Rat e;

NewSi g = Ref Si gLast + Rat e*t s;

i f ( NewSi g > Si gMagRat e)
{
NewSi g = 2*Si gMagRat e - NewSi g;
Tr i Sgn = 0;
}
el se i f ( NewSi g < - Si gMagRat e)
{
NewSi g = - 2*Si gMagRat e - NewSi g;
Tr i Sgn = 1;
}
}
r et ur n NewSi g;
}

/ / #%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%
f l oat Get Squar e( )
{
f l oat NewSi g;

{
Squar eCount = f s/ ( 2*Si gFr eq) ;
NewSi g = Si gMagRat e;
}
el se i f ( Ti meCnt == Squar eCount )
{
Squar eCount = Squar eCount + f s/ ( 2*Si gFr eq) ;
NewSi g = - Ref Si gLast ;
}
el se
NewSi g = Ref Si gLast ;

r et ur n NewSi g;
}

/ / #%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%
voi d Fl oat ToI EE754( f l oat *Val )
{

i nt 1 Tmp;
i nt 8 *Pt r ;

Pt r = Val ;

Tmp = shi f t _l ef t ( &Pt r [ 1] , 1, 0) ;
Tmp = shi f t _r i ght ( &Pt r [ 0] , 1, Tmp) ;
shi f t _r i ght ( &Pt r [ 1] , 1, Tmp) ;

}

/ / #%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%
voi d I EE754ToFl oat ( f l oat *Val )
{

i nt 1 Tmp;
char *Pt r ;

Pt r = Val ;

Tmp = shi f t _l ef t ( &Pt r [ 1] , 1, 0) ;
Tmp = shi f t _l ef t ( &Pt r [ 0] , 1, Tmp) ;
shi f t _r i ght ( &Pt r [ 1] , 1, Tmp) ;

}

/ / #%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%
#SEPARATE voi d Out put Fl oat ( f l oat Val I n)
{

char *c;
f l oat Val ;

Val = Val I n;

/ / Save poi nt er t o f l oat so we can easi l y access t he byt es i ndi vi dual l y
c = &Val ;

/ / Conver t number t o I EEE 754
Fl oat ToI EE754( &Val ) ;

/ / Send dat a ( or der of byt es r ever sed si nce PC i s expect i ng l i t t l eEndi an)
put c( c[ 3] ) ;
put c( c[ 2] ) ;
put c( c[ 1] ) ;
put c( c[ 0] ) ;

}

/ / #%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%
#SEPARATE voi d Out put Long( l ong Val )
{

char *c;

/ / Save poi nt er t o f l oat so we can easi l y access t he byt es i ndi vi dual l y
c = &Val ;

/ / Send dat a
put c( c[ 0] ) ;
put c( c[ 1] ) ;

}

/ / #%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%
#SEPARATE l ong Get Long( )
{

char *c;
l ong NewI nt ;

c = &NewI nt ;

c[ 0] = RecDat Buf [ 1] ;

r et ur n NewI nt ;

}

/ / #%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%
#SEPARATE f l oat Get Fl oat ( )
{

char *c;
f l oat NewFl oat ;

c = &NewFl oat ;


I EE754ToFl oat ( &NewFl oat ) ;

r et ur n NewFl oat ;

}

/ / #%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%
voi d Set Sampl eFr equency( f l oat f r eq) {
set up_t i mer _1( T1_I NTERNAL | T1_DI V_BY_8) ;

/ / The count er i s i ncr ement ed ever y 4 cl ocks.
/ / When t he count er r eset s ( r eaches 2^16 = 65536) i t gener at es an i nt er upt
/ / Thi s i nt er upt cal l s t he RunCont r ol l er f unct i on ( above)
/ / Set t i ng t he f ol l owi ng val ue t o t he t i mer wi l l cause i t t o i nt er upt at t he
/ / desi r ed f r equency.
pr eset _count er = 65536 - f l oor ( ( ( CLK/ 8) / 4) / f r eq ) ;
set _t i mer 1( pr eset _count er ) ;
}

/ / #%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%
voi d mai n( )
{

/ / Local var i abl es
l ong Last Out put Cnt ; / / I ndex of t he l ast dat aset out put
char Cmd;
i nt I nt Val ;

/ / I ni t i al i ze
i ni t _QMT( ) ;

/ / Cal cul at e angul ar r at e f i l t er par amet er s
ca = 2*omega/ ( 2+omega*t s) ;
cb = ( - 2+omega*t s) / ( 2+omega*t s) ;

/ / Choose cur r ent ADC channel
set _adc_channel ( 2) ;

i ni t _encoder ( ENCODER0) ; / / I ni t i al i ze t he encoder

/ / I ni t i al i ze out put s
daout ( 0) ; / / Send 0 vol t age t o DA out put
out put _l ow( LED3) ; / / Set LED3 t o l ow
out put _hi gh( LED2) ; / / Set LED2 t o hi gh
out put _l ow( PI N_B4) ; / / Enabl e ext er nal D/ A

/ / I ni t i al i ze var i abl es
DoCont r ol = 0;
Ref Si g = 0;
Ref Si gLast = 0;
Vol t = 0;
Cur r ent = 0;
Angl eFD = 0;
Angl eFDFi l t = 0;

Squar eCount = 0;
Tr i Sgn = 1;

i RecDat a = 0;
RecDat Avai l = 0;
RecEr r = 0;

EndCnt = 0;
Ti meCnt = 0;
Last Out put Cnt = 0;

Runni ng = 0;
Fi r st Ti me = 1;

enabl e_i nt er r upt s( I NT_RDA) ; / / Tur n on r eci eve dat a avai l abl e i nt er r upt
enabl e_i nt er r upt s( GLOBAL) ; / / Enabl e i nt er r upt s gl obal l y

whi l e( 1)
{

i f ( Runni ng == 1)
{

/ / Check i f we shoul d do cont r ol
i f ( DoCont r ol == 1)
RunCont r ol l er ( ) ;

/ / Check i f we need t o ouput dat a
i f ( ( Last Out put Cnt ! = Ti meCnt ) | | Fi r st Ti me)
{
put c( ' d' ) ;
Out put Long( Ti meCnt ) ;
Out put Fl oat ( Ref Si g) ;
Out put Fl oat ( Angl e) ;
Out put Fl oat ( Vol t ) ;
Out put Fl oat ( Cur r ent ) ;
Out put Fl oat ( Angl eFD) ;
Out put Fl oat ( Angl eFDFi l t ) ;

Last Out put Cnt = Ti meCnt ;
Fi r st Ti me = 0;
}

/ / Check f or end of t i me
i f ( Ti meCnt >= EndCnt )
br eak;

}

/ / Check f or dat a i n r ead buf f er
i f ( RecDat Avai l == 1)
{

RecDat Avai l = 0;

Cmd = RecDat Buf [ 0] ;

i f ( RecEr r )
{
/ / I gnor e bad dat a
RecEr r = 0;
}
el se
{

/ / Run t he desi r ed command
i f ( Cmd == ' S' )
{
/ / Thi s i s t he new si gnal t ype
Si gType = Get Long( ) ;
}
el se i f ( Cmd == ' M' )
{
/ / Thi s t he new si gnal magni t ude or r at e
Si gMagRat e = Get Fl oat ( ) ;
}
el se i f ( Cmd == ' F' )
{
/ / Thi s i s t he new si gnal f r equency
Si gFr eq = Get Fl oat ( ) ;
}
el se i f ( Cmd == ' E' )
{
/ / Thi s i s t he count at whi ch t o end
EndCnt = Get Long( ) ;
}
el se i f ( Cmd == ' G' )
{
/ / Thi s i s t he f l ag t o GO!
out put _l ow( LED2) ; / / Set LED2 t o l ow
Runni ng = 1;

Set Sampl eFr equency( f s) ; / / Set sampl e f r equency
enabl e_i nt er r upt s( I NT_TI MER1) ; / / Tur n on t i mer 1 i nt er r upt
enabl e_i nt er r upt s( GLOBAL) ; / / Enabl e i nt er r upt s gl obal l y
}
el se i f ( Cmd == ' f ' )
{
/ / Sampl i ng f r equency r equest
put c( ' f ' ) ;
Out put Fl oat ( t s) ;
}

}
}
}

/ / Send f l ag t o t el l t he comput er t o st op r eadi ng dat a
put c( ' X' ) ;

/ / St op t he i nt er r upt s
di sabl e_i nt er r upt s( GLOBAL) ; / / Di sabl e i nt er r upt s gl obal l y
di sabl e_i nt er r upt s( I NT_TI MER1) ; / / Tur n of f t i mer 1 i nt er r upt
di sabl e_i nt er r upt s( I NT_RDA) ; / / Tur n of f r ecei ve dat a avai l abl e
i nt er r upt

/ / St op mot or by sendi ng 0 vol t age t o DA out put
daout ( 0) ;

/ / Wai t 1 second f or mot or t o st op spi nni ng
del ay_ms( 1000) ;

r eset _cpu( ) ;

}

/ / #%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%
/ / Use i nt er r upt t o r eci eve ser i al dat a when i t i s avai l abl e
#I NT_RDA
voi d ReadSer i al Dat a( )
{

i f ( ( RS232_ERRORS & 0x7) ! = 0)
{
/ / An er r or ocur r ed
RecEr r = 1;
}
el se
{
/ / Save cur r ent t r ansmi ssi on
RecDat Buf [ i RecDat a] = get c( ) ;
i RecDat a++;

/ / Check f or f ul l buf f er
i f ( i RecDat a >= 5)
{
RecDat Avai l = 1;
i RecDat a = 0;
}
}

}

/ / #%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%#%
/ / Si gnal t he compi l er t hat t he next f unct i on i s t he t i mer 1 i nt er r upt
#I NT_TI MER1
voi d Set Cont r ol Fl ag( )
{

/ / Reset t he count er t o t he cor r ect count
/ / Thi s i s necessar y t o achi eve t he desi r ed sampl e r at e
set _t i mer 1( pr eset _count er ) ;

DoCont r ol = 1;

}

16F877.h Routines

/ / / / / / / / St andar d Header f i l e f or t he PI C16F877 devi ce / / / / / / / / / / / / / / / /
#devi ce PI C16F877 *=16
#nol i st
/ / / / / / / / Pr ogr ammemor y: 8192x14 Dat a RAM: 367 St ack: 8
/ / / / / / / / I / O: 33 Anal og Pi ns: 8
/ / / / / / / / Dat a EEPROM: 256
/ / / / / / / / C Scr at ch ar ea: 77 I D Locat i on: 2000
/ / / / / / / / Fuses: LP, XT, HS, RC, NOWDT, WDT, NOPUT, PUT, PROTECT, PROTECT_5%
/ / / / / / / / Fuses: PROTECT_50%, NOPROTECT, NOBROWNOUT, BROWNOUT, LVP, NOLVP, CPD
/ / / / / / / / Fuses: NOCPD, WRT, NOWRT, DEBUG, NODEBUG
/ / / / / / / /
/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / I / O
/ / Di scr et e I / O Funct i ons: SET_TRI S_x( ) , OUTPUT_x( ) , I NPUT_x( ) ,
/ / PORT_B_PULLUPS( ) , I NPUT( ) ,
/ / OUTPUT_LOW( ) , OUTPUT_HI GH( ) ,
/ / OUTPUT_FLOAT( ) , OUTPUT_BI T( )
/ / Const ant s used t o i dent i f y pi ns i n t he above ar e:

#def i ne PI N_A0 40

#def i ne PI N_B0 48

#def i ne PI N_C0 56

#def i ne PI N_D0 64

#def i ne PI N_E0 72

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / Usef ul def i nes
#def i ne FALSE 0
#def i ne TRUE 1

#def i ne BYTE i nt
#def i ne BOOLEAN shor t i nt

#def i ne get c get ch
#def i ne f get c get ch
#def i ne get char get ch
#def i ne put c put char
#def i ne f put c put char
#def i ne f get s get s
#def i ne f put s put s

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / Cont r ol
/ / Cont r ol Funct i ons: RESET_CPU( ) , SLEEP( ) , RESTART_CAUSE( )
/ / Const ant s r et ur ned f r omRESTART_CAUSE( ) ar e:
#def i ne WDT_FROM_SLEEP 3
#def i ne WDT_TI MEOUT 11
#def i ne MCLR_FROM_SLEEP 19
#def i ne MCLR_FROM_RUN 27
#def i ne NORMAL_POWER_UP 24
#def i ne BROWNOUT_RESTART 26

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / Ti mer 0
/ / Ti mer 0 ( AKA RTCC) Funct i ons: SETUP_COUNTERS( ) or SETUP_TI MER_0( ) ,
/ / SET_TI MER0( ) or SET_RTCC( ) ,
/ / GET_TI MER0( ) or GET_RTCC( )
/ / Const ant s used f or SETUP_TI MER_0( ) ar e:
#def i ne RTCC_I NTERNAL 0
#def i ne RTCC_EXT_L_TO_H 32
#def i ne RTCC_EXT_H_TO_L 48

#def i ne RTCC_DI V_1 8

#def i ne RTCC_8_BI T 0

/ / Const ant s used f or SETUP_COUNTERS( ) ar e t he above
/ / const ant s f or t he 1st par amand t he f ol l owi ng f or
/ / t he 2nd par am:

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / WDT
/ / Wat ch Dog Ti mer Funct i ons: SETUP_WDT( ) or SETUP_COUNTERS( ) ( see above)
/ / RESTART_WDT( )
/ /
#def i ne WDT_18MS 8

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / Ti mer 1
/ / Ti mer 1 Funct i ons: SETUP_TI MER_1, GET_TI MER1, SET_TI MER1
/ / ( or ( vi a | ) t oget her const ant s f r omeach gr oup)
#def i ne T1_DI SABLED 0
#def i ne T1_I NTERNAL 0x85
#def i ne T1_EXTERNAL 0x87
#def i ne T1_EXTERNAL_SYNC 0x83

#def i ne T1_CLK_OUT 8

#def i ne T1_DI V_BY_1 0
#def i ne T1_DI V_BY_2 0x10

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / Ti mer 2
/ / Ti mer 2 Funct i ons: SETUP_TI MER_2, GET_TI MER2, SET_TI MER2
#def i ne T2_DI SABLED 0

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / CCP
/ / CCP Funct i ons: SETUP_CCPx, SET_PWMx_DUTY
/ / CCP Var i abl es: CCP_x, CCP_x_LOW, CCP_x_HI GH
/ / Const ant s used f or SETUP_CCPx( ) ar e:
#def i ne CCP_OFF 0
#def i ne CCP_CAPTURE_FE 4
#def i ne CCP_CAPTURE_RE 5
#def i ne CCP_CAPTURE_DI V_4 6
#def i ne CCP_CAPTURE_DI V_16 7
#def i ne CCP_COMPARE_SET_ON_MATCH 8
#def i ne CCP_COMPARE_CLR_ON_MATCH 9
#def i ne CCP_COMPARE_I NT 0xA
#def i ne CCP_COMPARE_RESET_TI MER 0xB
#def i ne CCP_PWM 0xC
#def i ne CCP_PWM_PLUS_1 0x1c
l ong CCP_1;
#byt e CCP_1 = 0x15
#byt e CCP_1_LOW= 0x15
#byt e CCP_1_HI GH= 0x16
l ong CCP_2;
#byt e CCP_2 = 0x1B
#byt e CCP_2_LOW= 0x1B
#byt e CCP_2_HI GH= 0x1C
/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / PSP
/ / PSP Funct i ons: SETUP_PSP, PSP_I NPUT_FULL( ) , PSP_OUTPUT_FULL( ) ,
/ / PSP_OVERFLOW( ) , I NPUT_D( ) , OUTPUT_D( )
/ / PSP Var i abl es: PSP_DATA
/ / Const ant s used i n SETUP_PSP( ) ar e:
#def i ne PSP_ENABLED 0x10
#def i ne PSP_DI SABLED 0

#byt e PSP_DATA= 8

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / SPI
/ / SPI Funct i ons: SETUP_SPI , SPI _WRI TE, SPI _READ, SPI _DATA_I N
/ / Const ant s used i n SETUP_SSP( ) ar e:
#def i ne SPI _MASTER 0x20
#def i ne SPI _SLAVE 0x24
#def i ne SPI _L_TO_H 0
#def i ne SPI _H_TO_L 0x10
#def i ne SPI _CLK_DI V_4 0
#def i ne SPI _CLK_T2 3
#def i ne SPI _SS_DI SABLED 1

#def i ne SPI _SAMPLE_AT_END 0x8000
#def i ne SPI _XMI T_L_TO_H 0x4000

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / UART
/ / Const ant s used i n set up_uar t ( ) ar e:
/ / FALSE - Tur n UART of f
/ / TRUE - Tur n UART on
#def i ne UART_ADDRESS 2
#def i ne UART_DATA 4

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / ADC
/ / ADC Funct i ons: SETUP_ADC( ) , SETUP_ADC_PORTS( ) ( aka SETUP_PORT_A) ,
/ / SET_ADC_CHANNEL( ) , READ_ADC( )
/ / Const ant s used f or SETUP_ADC( ) ar e:
#def i ne ADC_OFF 0 / / ADC Of f
#def i ne ADC_CLOCK_DI V_2 0x100
#def i ne ADC_CLOCK_I NTERNAL 0xc0 / / I nt er nal 2- 6us

/ / Const ant s used i n SETUP_ADC_PORTS( ) ar e:
#def i ne NO_ANALOGS 7 / / None
#def i ne ALL_ANALOG 0 / / A0 A1 A2 A3 A5 E0 E1 E2
#def i ne AN0_AN1_AN2_AN4_AN5_AN6_AN7_VSS_VREF 1 / / A0 A1 A2 A5 E0 E1 E2 VRef h=A3
#def i ne AN0_AN1_AN2_AN3_AN4 2 / / A0 A1 A2 A3 A5
#def i ne AN0_AN1_AN2_AN4_VSS_VREF 3 / / A0 A1 A2 A5 VRef h=A3
#def i ne AN0_AN1_AN3 4 / / A0 A1 A3
#def i ne AN0_AN1_VSS_VREF 5 / / A0 A1 VRef h=A3
#def i ne AN0_AN1_AN4_AN5_AN6_AN7_VREF_VREF 0x08 / / A0 A1 A5 E0 E1 E2 VRef h=A3 VRef l =A2
#def i ne AN0_AN1_AN2_AN3_AN4_AN5 0x09 / / A0 A1 A2 A3 A5 E0
#def i ne AN0_AN1_AN2_AN4_AN5_VSS_VREF 0x0A / / A0 A1 A2 A5 E0 VRef h=A3
#def i ne AN0_AN1_AN4_AN5_VREF_VREF 0x0B / / A0 A1 A5 E0 VRef h=A3 VRef l =A2
#def i ne AN0_AN1_AN4_VREF_VREF 0x0C / / A0 A1 A5 VRef h=A3 VRef l =A2
#def i ne AN0_AN1_VREF_VREF 0x0D / / A0 A1 VRef h=A3 VRef l =A2
#def i ne AN0 0x0E / / A0
#def i ne AN0_VREF_VREF 0x0F / / A0 VRef h=A3 VRef l =A2
#def i ne ANALOG_RA3_REF 0x1 / / ! ol d onl y pr ovi ded f or compat i bi l i t y
#def i ne A_ANALOG 0x2 / / ! ol d onl y pr ovi ded f or compat i bi l i t y
#def i ne A_ANALOG_RA3_REF 0x3 / / ! ol d onl y pr ovi ded f or compat i bi l i t y
#def i ne RA0_RA1_RA3_ANALOG 0x4 / / ! ol d onl y pr ovi ded f or compat i bi l i t y
#def i ne RA0_RA1_ANALOG_RA3_REF 0x5 / / ! ol d onl y pr ovi ded f or compat i bi l i t y
#def i ne ANALOG_RA3_RA2_REF 0x8 / / ! ol d onl y pr ovi ded f or compat i bi l i t y
#def i ne ANALOG_NOT_RE1_RE2 0x9 / / ! ol d onl y pr ovi ded f or compat i bi l i t y
#def i ne ANALOG_NOT_RE1_RE2_REF_RA3 0xA / / ! ol d onl y pr ovi ded f or compat i bi l i t y
#def i ne ANALOG_NOT_RE1_RE2_REF_RA3_RA2 0xB / / ! ol d onl y pr ovi ded f or compat i bi l i t y
#def i ne A_ANALOG_RA3_RA2_REF 0xC / / ! ol d onl y pr ovi ded f or compat i bi l i t y
#def i ne RA0_RA1_ANALOG_RA3_RA2_REF 0xD / / ! ol d onl y pr ovi ded f or compat i bi l i t y
#def i ne RA0_ANALOG 0xE / / ! ol d onl y pr ovi ded f or compat i bi l i t y
#def i ne RA0_ANALOG_RA3_RA2_REF 0xF / / ! ol d onl y pr ovi ded f or compat i bi l i t y

/ / Const ant s used i n READ_ADC( ) ar e:
#def i ne ADC_START_AND_READ 7 / / Thi s i s t he def aul t i f not hi ng i s speci f i ed
#def i ne ADC_START_ONLY 1
#def i ne ADC_READ_ONLY 6

/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / I NT
/ / I nt er r upt Funct i ons: ENABLE_I NTERRUPTS( ) , DI SABLE_I NTERRUPTS( ) ,
/ / EXT_I NT_EDGE( )
/ /
/ / Const ant s used i n EXT_I NT_EDGE( ) ar e:
#def i ne L_TO_H 0x40
#def i ne H_TO_L 0
/ / Const ant s used i n ENABLE/ DI SABLE_I NTERRUPTS( ) ar e:
#def i ne GLOBAL 0x0BC0
#def i ne I NT_RTCC 0x0B20
#def i ne I NT_RB 0x0B08
#def i ne I NT_EXT 0x0B10
#def i ne I NT_AD 0x8C40
#def i ne I NT_TBE 0x8C10
#def i ne I NT_RDA 0x8C20
#def i ne I NT_TI MER1 0x8C01
#def i ne I NT_TI MER2 0x8C02
#def i ne I NT_CCP1 0x8C04
#def i ne I NT_CCP2 0x8D01
#def i ne I NT_SSP 0x8C08
#def i ne I NT_PSP 0x8C80
#def i ne I NT_BUSCOL 0x8D08
#def i ne I NT_EEPROM 0x8D10
#def i ne I NT_TI MER0 0x0B20

#l i st
q_config.h Routines
/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / / / / / / / / / / / / / / /
/ /
/ / q_conf i g. h
/ /
/ / Thi s f i l e does t he gener al set up f or t he QI C Pr ocessor Cor e. Thi s f i l e shoul d be t he
i ncl uded
/ / on t he f i r st l i ne of any pr ogr amwr i t t en by t he user .
/ / The user may add addt i onal l i ne t o t hi s f i l e as necessar y, but t hey shoul d not e t hat
t he CCS
/ / compi l er l oads l i nes i n or der . The compi l er essent i al l y " i nl i nes" t he f i l es i n t he
mai n sour ce
/ /
/ / Thi s f i l e i s desi ged t o be compi l ed wi t hout change usi ng t he CCS compi l er , ver si on
3. 051
/ / The devel opment envi r onment used was t he Mi cr ochi p MPLAB I DE, wi t h t he CCS compi l er
l i nked
/ / as t he C compi l er .
/ /
/ / Aut hor : A. D.
/ / Copyr i ght 2002, Quanser Consul t i ng I nc.
/ /
/ / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / / /
/ / / / / / / / / / / / / / / /

#f uses HS, NOWDT, NOPROTECT, PUT, NOLVP / / hi gh speed osci l l at or opt i on
HS
/ / wat chdog t i mer i s DI SABLED
/ / t he code pr ot ect i on bi t s ar e
DI SABLED

#DEVI CE ADC=10 / / set t he ADC t o 10 bi t mode
#ZERO_RAM / / i ni t i al i ze al l RAM l ocat i ons
t o zer o by def aul t

#def i ne PUSHBUTTON PI N_B0 / / def i ne PUSHBUTTON as PI N_B0
#def i ne LED3 PI N_C0 / / def i ne LED as PI N_C0
#def i ne LED2 PI N_C1 / / def i ne LED as PI N_C1

/ / The f ol l owi ng l i nes set up t he por t addr ess so t hat we can use di r ect r eads and
/ / wr i t es t o memor y l ocat i ons t o r ead and wr i t e por t s di r ect l y.
/ / t hi s i s t he most ef f i ci ent way of r eadi ng / wr i t i ng 8 bi t s ( a byt e ) of dat a
/ / t o / f r oma por t .

#byt e PORTA = 5
#byt e PORTB = 6
#byt e PORTC = 7
#byt e PORTD = 8
#byt e PORTE = 9

/ / Speci f y t he por t s t o use t he f ast _i o opt i on. car e must be t aken when usi ng t hi s opt i on
as
/ / t he compi l er does not add any ext r a code t o set up t he por t . The bi t s i n a por t
/ / must be set up ahead of t i me t o be ei t her i nput s or out put s.

#use f ast _i o( a)
/ / #use f ast _i o( b)
#use f ast _i o( c)
#use f ast _i o( d)
#use f ast _i o( e)

encoder.h Routines

#byt e PORTD = 8

#def i ne ALL_OUT 0x00
#def i ne ALL_I N 0xFF

#def i ne SLAVE0 0x00
#def i ne SLAVE1 0x01

#def i ne READ_ENC PI N_A4
#def i ne WRI TE_ENC PI N_B3

/ / macr os t o enabl e / di sabl e t he encoder
#def i ne DI SABLE_DEV out put _hi gh( PI N_B1) ; out put _hi gh( PI N_B2) ;
#def i ne ENABLE_ENC out put _hi gh( PI N_B1) ; out put _l ow( PI N_B2) ;
#def i ne DI SABLE_ENC out put _l ow( PI N_B1) ; out put _l ow( PI N_B2) ;
#def i ne ENABLE_DA out put _hi gh( PI N_B2) ; out put _l ow( PI N_B1) ;

#def i ne ENC_AXI S PI N_C5 / / ' 0' = X axi s ; ' 1' = Y axi s
#def i ne ENC_DC PI N_C4 / / ' 0' = DATA ; ' 1' = COMMAND

#def i ne DA_BYTE_A0 PI N_C4 / / DA Ni bbl e sel ect A0
#def i ne DA_BYTE_A1 PI N_C5 / / DA Ni bbl e sel ect A1

#def i ne ENCODER0 0x00
#def i ne ENCODER1 0x01
#def i ne X_AXI S 0x00
#def i ne Y_AXI S 0x04
#def i ne ENC_DATA 0x00 / / dat a r egi st er of ENCODER CHANNEL
#def i ne ENC_CONTROL 0x08 / / cont r ol r egi st er ENCODER CHANNEL
#def i ne CLOCK_DATA 0 / / FCK f r equency di vi der
#def i ne CLOCK_SETUP 0X98 / / t r ansf er PR0 t o PSC ( x and y)
#def i ne I NPUT_SETUP 0XC1 / / enabl e i nput s A and B ( x and y)
#def i ne QUAD_X1 0XA8 / / quadr at ur e mul t i pl i er t o 1 ( x and y)
#def i ne QUAD_X2 0XB0 / / quadr at ur e mul t i pl i er t o 2 ( x and y)
#def i ne QUAD_X4 0XB8 / / quadr at ur e mul t i pl i er t o 4 ( x and y)
#def i ne BP_RESET 0X01 / / r eset byt e poi nt er
#def i ne BP_RESETB 0X81 / / r eset byt e poi nt er ( x and y)
#def i ne CNTR_RESET 0X02 / / r eset count er
#def i ne CNTR_RESETB 0X82 / / r eset count er ( x and y)
#def i ne TRSFRPR_CNTR 0X08 / / t r ansf er pr eset r egi st er t o count er
#def i ne TRSFRCNTR_OL 0X90 / / t r ansf er CNTR t o OL ( x and y)
#def i ne EFLAG_RESET 0X86 / / r eset E bi t of f l ag r egi st er
#def i ne ENABLE_I NDEX 0xE7 / / RCNTR pi n i s i ndexed
#def i ne DI SABLE_I NDEX 0xE6 / / RCNTR pi n i s not i ndexed

/ / gl obal var i abl es
f l oat cal x1, cal x1_mi d, cal x1_hi gh, of f set _x1;

/ / dat a i s r et ur ned i n t hese var i abl es.
/ / see t he exampl e f i l e encoder . c on how t o r econst r uct t hem
/ / t o f or man angul ar measur ement

f l oat cal cul at e_angl e( ) ;
voi d cal i br at i on_par amet er s( ) ;

st at i c i nt encoder _dat a[ 3] ;

/ **
* The i np( addr ess) f unct i on r eads f r omt he pr oper encoder r egi st er based on
* what addr ess val ue i s passed i n. The f unct i on uses a bi t mask t o det er mi ne
* whi ch r egi st er i s bei ng r ead and set s up t he addr essi ng cor r ect l y t o r et ur n
* t he pr oper dat a.
* Val i d val ues f or addr ess shoul d be X_AXI S and Y_AXI S, or ed wi t h t he
* t he encoder dat a r egi st er addr ess ( ENC_DATA) as def i ned i n t he #def i ne
* st at ement s of t he header .
* Ret ur ns: t he val ue r ead f r omt he encoder
**/
i nt i np( i nt addr ess) {
i nt dat a;

/ / PORT D i s t he bus connect ed t o t he encoder
set _t r i s_D( ALL_I N) ; / / set PORT D t o I NPUT
out put _l ow( READ_ENC) ; / / set t he READ ( / RD) l i ne l ow
ENABLE_ENC; / / set t he ENABLE ( / CS) l i ne l ow
/ / t he chi p i s now r eady t o r ead
out put _bi t ( ENC_DC, bi t _t est ( addr ess, 3) ) ; / / set t he D/ C bi t t o r ead DATA or
/ / cont r ol based on t he addr ess
passed i n
out put _bi t ( ENC_AXI S, bi t _t est ( addr ess, 2) ) ; / / set t he appr opr i at e axi s bi t ( X or
Y)
/ / based on t he addr ess passed i n
del ay_cycl es( 1) ; / / wai t f or t he dat a t o be r eady,
same as NOP
dat a = PORTD; / / r ead i n t he dat a f r omPORT D
out put _hi gh( READ_ENC) ; / / r et ur n t he / RD l i ne hi gh
set _t r i s_D( ALL_I N) ; / / set PORT D t o I NPUT
DI SABLE_DEV; / / di sabl e t he encoder ( / CS hi gh)
r et ur n( dat a) ;
}

/ **
* The out p( addr ess, val ue) f unct i on sends a val ue out t o t he pr oper r egi st er of t he
* encoder I C. The f unct i on r ecei ves t wo par amet er s, t he addr ess of t he r egi st er t o
* wr i t e t o, and t he val ue t o be sent t o t hat r egi st er .
* The f unct i on uses a bi t mask t o det er mi ne t he pr oper r egi st er t o wr i t e t o and
* t o set t he appr opr i at e addr ess bi t s.
* Not e t hat when wr i t i ng t o t he pr eset r egi st er s of t he encoder I C, 3 consecut i ve
* wr i t es ar e needed t o f i l l t he i nt er nal r egi st er .
**/
voi d out p( i nt addr ess, i nt val ue) {
set _t r i s_D( ALL_OUT) ; / / set PORT D t o out put
out put _l ow( WRI TE_ENC) ; / / set t he / WR l i ne l ow
ENABLE_ENC; / / enabl e t he encoder I C
out put _bi t ( ENC_DC, bi t _t est ( addr ess, 3) ) ; / / set t he D/ C bi t appr opr i at el y
based on t he
/ / t he addr ess ar gument
out put _bi t ( ENC_AXI S, bi t _t est ( addr ess, 2) ) ; / / set t he axi s bi t appr opr i at el y
based on
/ / t he addr ess ar gument
del ay_cycl es( 1) ; / / wai t f or t he oper at i on t o compl et e
( NOP)
PORTD = val ue; / / set PORT D t o t he dat a val ue
passed i n
out put _hi gh( WRI TE_ENC) ; / / r et ur n t he / WR l i ne hi gh
set _t r i s_D( ALL_I N) ; / / set PORT D t o out put
DI SABLE_DEV; / / di sabl e t he encoder I C
}

/ **
* Thi s f unct i on i s desi gned t o set up t he encoder f or t he most common f unct i ons.
* Thi s f unct i on *must * be cal l ed pr i or t o per f or mi ng any r eads of t he encoder
* t o ensur e t hat i t i s set up pr oper l y.
* I n most i nst ances, t hi s f unct i on can be l ef t as i s, but i t can be modi f i ed
* by t he user t o meet t hei r needs.
**/

voi d i ni t _encoder ( i nt addr ess)
{

/ *i ni t i al i ze t he ENCODER CHI P*/
cal i br at i on_par amet er s( ) ;

/ / whi l e( 0) / / f r omMQ3. DRV

out p( addr ess| ENC_CONTROL, EFLAG_RESET) ; / / r eset E bi t of f l ag r egi st er
out p( addr ess| ENC_CONTROL, BP_RESET) ; / / r eset byt e poi nt er ( x and y)
out p( addr ess| ENC_DATA, CLOCK_DATA) ; / / FCK f r equency di vi der
out p( addr ess| ENC_CONTROL, CLOCK_SETUP) ; / / t r ansf er PR0 t o PSC ( x and y)
out p( addr ess| ENC_CONTROL, I NPUT_SETUP) ; / / enabl e i nput s A and B ( x and y)
out p( addr ess| ENC_CONTROL, QUAD_X4) ; / / quadr at ur e mul t i pl i er t o 4 ( x and
y)
out p( addr ess| ENC_CONTROL, CNTR_RESET) ; / / r eset count er ( x and y)

whi l e( 0)
{
out p( addr ess | ENC_CONTROL, EFLAG_RESET) ; / / r eset t he er r or bi t i n t he FLAGs
r egi st er
out p( addr ess | ENC_CONTROL, BP_RESETB) ; / / r eset t he byt e poi nt er
out p( addr ess | ENC_DATA| X_AXI S, CLOCK_DATA) ; / / set up t he cl ock f or t he X axi s
out p( addr ess | ENC_DATA| Y_AXI S, CLOCK_DATA) ; / / set up t he cl ock f or t he Y axi s
out p( addr ess | ENC_CONTROL, BP_RESETB) ; / / r eset t he byt e poi nt er
out p( addr ess | ENC_CONTROL, CLOCK_SETUP) ; / / t r ansf er PR0 t o PSC ( x and y)
out p( addr ess | ENC_CONTROL, QUAD_X4) ; / / r ead t he encoder i n quadr at ur e ( x4
mode)
out p( addr ess | ENC_CONTROL, I NPUT_SETUP) ; / / enabl e i nput s A and B ( X and Y)
out p( addr ess | ENC_CONTROL, CNTR_RESETB) ; / / r eset t he count er s f or X and Y
axi s

/ / added by A. D. t o di sabl e i ndex pul se
out p( addr ess | ENC_CONTROL, 0xE0) ;

}

}

f l oat pr eset _encoder ( i nt addr ess, i nt axi s, i nt l ow, i nt mi d, i nt hi gh)
{

out p( addr ess| ENC_CONTROL| axi s, BP_RESETB) ; / / r eset byt e poi nt er
out p( addr ess| ENC_DATA| axi s, l ow ) ;
out p( addr ess| ENC_DATA| axi s, mi d ) ;
out p( addr ess| ENC_DATA| axi s, hi gh) ; / / out put pr eset val ue
out p( addr ess | ENC_CONTROL | axi s , TRSFRPR_CNTR) ; / / t r ansf er t he count er s t o t he ouput
l at ches

}

/ **
* Thi s f unct i on r ead_encoder ( axi s) i s used t o r ead t he encoder and r et ur n t he val ue
* of t he r egi st er s st or ed i n t he ar r ay encoder _dat a. The user wi l l t hen need t o
* r econst r uct t he dat a t o f or ma 24 bi t wor d. encoder _dat a[ 0] i s t he l owest byt e, and
* encoder _dat a[ 2] i s t he hi ghest byt e. To
*
*/
f l oat r ead_encoder ( i nt addr ess, i nt axi s) {
out p( addr ess | ENC_CONTROL, BP_RESETB) ; / / r eset t he byt e poi nt er s so t hat we
r ead t he

/ / r egi st er s i n t he pr oper or der
out p( addr ess | ENC_CONTROL | axi s , TRSFRCNTR_OL) ; / / t r ansf er t he count er s t o t he ouput
l at ches
encoder _dat a[ 0] = i np( ENC_DATA | axi s) ; / / r ead t he f i r st byt e, st or e i t .
encoder _dat a[ 1] = i np( ENC_DATA | axi s) ; / / r ead t he second byt e, st or e i t .
encoder _dat a[ 2] = i np( ENC_DATA | axi s) ; / / r ead t he t hi r d byt e, st or e i t .

r et ur n cal cul at e_angl e( ) ;

/ / t he dat a i s st or ed i n t he var i abl es encoder _dat a[ 0- 2] as
/ / i ndi vi dual byt es t o make up a 24 bi t wor d
}

f l oat cal cul at e_angl e( )
{

f l oat hi gh_m, angl e_r ;

/ / Mask of f t he si gn bi t t o do angul ar cal cul at i on
hi gh_m= encoder _dat a[ 2] &0x7f ;
angl e_r = cal x1 * encoder _dat a[ 0] + cal x1_mi d * encoder _dat a[ 1] + cal x1_hi gh * hi gh_m;

/ / comput e t he of f set i f t he angl e was negat i ve
i f ( ( encoder _dat a[ 2] &0x80) == 0x80)
{
angl e_r = angl e_r - of f set _x1;
}

r et ur n angl e_r ;
}

voi d cal i br at i on_par amet er s( )
{

/ / cal cul at e t he cal i br at i on par amet er s - r adi ans
cal x1 = 0. 00153398078789 ; / / 2pi / 4096. ; / / conver si on f r omencoder count s t o
degr ees

cal x1_mi d = 0. 39269908169872; / / cal x1*256; / / mul t i pl i cat i on f act or f or t he
mi ddl e byt e ( i n degr ees)

cal x1_hi gh = 1. 005309649148734e+002; / / cal x1_mi d*256; / / mul t i pl i cat i on f act or f or
t he upper byt e ( i n degr ees)

of f set _x1 = 1. 286796350910379e+004; / / cal x1_hi gh*128. ; / / f act or t o account
f or negat i ve val ues

}

daout.h Routines

f l oat vol t ageToI nt eger Val ue( f l oat ) ;

/ / Di gi t al t o Anal og out put
voi d daout ( f l oat vol t age)
{
i nt l ow_byt e, hi gh_ni bbl e;
l ong val ue;
val ue = vol t ageToI nt eger Val ue( vol t age) ;

l ow_byt e = val ue&0xf f ;
set _t r i s_D( ALL_OUT) ; / / set PORT D t o out put
ENABLE_DA;
out put _bi t ( DA_BYTE_A0, 0 ) ; / / Br i ng A0 l ow
out put _bi t ( DA_BYTE_A1, 0 ) ; / / Br i ng A1 l ow
/ / enabl e t he D/ A Chi p
PORTD = l ow_byt e; / / set PORT D t o t he val ue of t he l ow 8 bi t s
del ay_cycl es( 1) ;

/ / ENABLE_DA;
hi gh_ni bbl e = val ue>>8;
hi gh_ni bbl e = hi gh_ni bbl e&0x0f ;

PORTD = hi gh_ni bbl e; / / set PORT D t o t he dat a val ue of t he hi gh
4 bi t s
out put _bi t ( DA_BYTE_A0, 1 ) ; / / Br i ng A0 hi gh
out put _bi t ( DA_BYTE_A1, 1 ) ; / / Br i ng A1 hi gh

/ / del ay_cycl es( 1) ; / / wai t f or t he oper at i on t o compl et e ( NOP)
set _t r i s_D( ALL_I N) ; / / set PORT D t o i nput - must do ot her wi se i t l oads
t he syst em
DI SABLE_DEV;
}

f l oat vol t ageToI nt eger Val ue( f l oat vol t age)
{
l ong i nt eger _da=0;

vol t age = vol t age*0. 33;

i f ( vol t age >= 4. 99)
vol t age = 4. 99;
i f ( vol t age <= - 4. 99)
vol t age = - 4. 99;

i nt eger _da = ( 2047 + ( l ong) ( ( vol t age / 5. 0) * ( f l oat ) 2047) ) ; / / Conver t vol t age t o
equi v i nt eger val ue

r et ur n i nt eger _da; / / Ret ur n i nt eger val ue

}

init_qmt.h Routines

voi d i ni t _QMT( )
{
set _t r i s_a( 0b1101111) ; / / RA0- 3 anal og i nput s, RA4 out put , RA5 i nput
set _t r i s_b( 0b11000001) ; / / B al l out except f or B0 - connect ed t o swi t ch
por t _b_pul l ups( TRUE) ;
set _t r i s_c( 0b10001100) ; / * PI N RC7 i s ser i al RX f r omHOST */
set _t r i s_d( ALL_I N) ; / * PORTD i s al l di gi t al OUTPUTS */
set _t r i s_e( ALL_I N) ; / * PORTE i s al l di gi t al OUTPUTS */

out put _hi gh( PI N_B3) ; / / set wr i t e and r ead hi gh
out put _hi gh( PI N_A4) ;

out put _hi gh( PI N_C4) ; / / set dat a r egi st er s sel ect s hi gh
out put _hi gh( PI N_C5) ;

out put _hi gh( PI N_B2) ; / / chi p sel ect s t o hi gh t o sel ect i nexi st ent devi ce
out put _hi gh( PI N_B1) ;

out put _l ow( PI N_C0) ; / / t ur n of f bot h LED' s
out put _l ow( PI N_C1) ;

out put _hi gh( PI N_B4) ; / / Tur n of f t he ext r enal D/ A
daout ( 2047) ; / / put out zer o vol t s

out put _l ow( PI N_B6) ; / / St r eami ndi cat or

set up_adc_por t s( ANALOG_RA3_REF) ; / * RE2 - RE0 ar e DI GI TAL I / O and RA3 i s REF*/
set up_adc( ADC_CLOCK_I NTERNAL) ;
}

Appendix B: MATLAB QET Interface Code

The following is a listing of the MATLAB code that uses the serial port to interface
with the QET. This function is called by the GUI to complete the user specified
control task.

f unct i on [ Dat a, Dat aCount , Sampl ePer i od, Er r or ] = . . .
RunQET( Si gType, Si gMagRat e, Si gFr eq, RunTi me, hScopeCal l back, handl es)

%RunQET - Runs a case on t he Quanser engi neer i ng t r ai ner and r et ur ns t he
%r esul t s
%
% [ Dat a, Dat aCount , Sampl ePer i od, Er r or ] = RunQET( Si gType, Si gMagRat e,
% Si gFr eq, RunTi me, hScopeCal l back, handl es) r uns t he speci f i ed case on
% t he Quanser Engi neer i ng Tr ai ner and r et ur ns a mat r i x of r esul t s al ong
% wi t h t he number of t i me sampl es, t he sampl e per i od, and an er r or f l ag.
%
% The i nput Si gType speci f i es t he si gnal t ype t o r un. Thi s can be one of
% t he f ol l owi ng val ues:
% 1 - I mpul se si gnal
% 2 - St ep si gnal
% 3 - Ramp si gnal
% 4 - Par abol i c si gnal
% 5 - Tr i angl e wave si gnal
% 6 - Sawt oot h wave si gnal
% 7 - Squar e wave si gnal
% 8 - Si ne wave si gnal
% 9 - Cosi ne wave si gnal
% 10 - St ai r st ep si gnal
%
% The i nput Si gMagRat e def i nes t he si gnal magni t ude or r at e dependi ng on
% t he si gnal t ype. For a r amp or par abol i c si gnal i t speci f i es a r at e.
% For al l t he ot her si gnal t ypes i t speci f i es a magnui t ude. For mor e
% i nf or mat i on pl ease see t he associ at ed hel p f i l e.
%
% The i nput Si gFr eq speci f i es t he si gnal f r equency i n Hz, i f appl i cabl e
% t o t he spei ci f ed si gnal t ype. I f t he f r equency i s not appl i cabl e t o
% t he speci f i ed si gnal t ype ( such as f or a st ep si gnal ) t hen t hi s shoul d
% be set t o 0.
%
% The i nput RunTi me speci f i es t he t ot al r un t i me i n seconds.
%
% The i nput hScopeCal l back speci f i es t he handl e t o t he scope cal l back
% pr ocedur e. I f t he cal l back i sn' t desi r ed t hen t hi s val ue shoul d be set
% t o zer o.
%
% The i nput handl es i s t he handl es ar r ay t hat wi l l be passed t o t he scope
% cal l back f unct i on. I f t he scope cal l back val ue i s set t o zer o t hen
% t hi s shoul d al so be set t o zer o.
%
% The out put Dat a cont ai ns al l of t he r et ur ned dat a. Thi s ar r ay wi l l
% have Dat aCount r ows i n i t and wi l l cont ai n seven col umns. The col umns
% ar e:
% 1 - The dat a i ndex f or t hi s r ow r et ur ned by t he QET
% 2 - The r ef er ence si gnal
% 3 - The act ual angl e of t he mot or
% 4 - The vol t age bei ng appl i ed t o t he mot or
% 5 - The cur r ent bei ng dr awn by t he mot or
% 6 - The f i r st der i vat i ve of t he mot or angl e usi ng t he si mpl e met hod
% 7 - The f i r st der i vat i ve of t he mot or angl e usi ng t he f i l t er i ng met hod
%
% The out put Sampl ePer i od i s t he sampl e per i od, i n seconds, bei ng used by t he QET
%
% The out put Er r or i s a bool ean f l ag as t o whet her an er r or occur ed.
%
% For addi t i onal i nf or mat i on pl ease see t he hel p f i l e.

Dat a = [ ] ;
Dat aCount = 0;
Sampl ePer i od = 0;
Er r or = t r ue;

i f nar gout ~= 4
msgbox( ' I nval i d number of r et ur n ar gument s f or RunQET! ' )
r et ur n
end

i f nar gi n < 4
msgbox( ' I nval i d number of ar gument s f or RunQET! ' )
r et ur n
end

%Set cur r ent el apsed t i me
Ti meNow = 0;
Dat aCount = 0;

%Cl ose al l ser i al por t obj ect s
Cl oseAl l Ser i al Por t s

%Cr eat e ser i al por t obj ect
S = ser i al ( ComPor t Name, ' BaudRat e' , 115200, ' Par i t y' , ' none' , ' Dat aBi t s' , 8,
' St opBi t s' , 1, ' I nput Buf f er Si ze' , 50000, ' Ti meout ' , 1) ;

%Open ser i al por t
f open( S) ;

%Get sampl i ng f r equency
pause( 0. 1) ;

i f ~Wr i t eToQET( S, ' f ' , ' uchar ' ) f cl ose( S) ; r et ur n; end;
i f ~Wr i t eToQET( S, 0, ' f l oat 32' ) f cl ose( S) ; r et ur n; end;

PauseCount = 0;
PauseSt ep = 0. 1;
whi l e S. Byt esAvai l abl e < 5
pause( PauseSt ep) ;
PauseCount = PauseCount + 1;

i f PauseCount > 2/ PauseSt ep
S
msgbox( ' The QET i s not r espondi ng. Pl ease ver i f y i t i s r eset and t r y agai n' )
f cl ose( S) ;
r et ur n
end

end

Cmd = f r ead( S, 1, ' uchar ' ) ;

i f Cmd ~= ' f '
di sp( ' Unexcect ed r et ur n val ue f or f r equency' ) ;
r et ur n
end

Sampl ePer i od = f r ead( S, 1, ' f l oat 32' ) ;

%Det er mi ne number of si gnal s t o r ead
EndCnt = cei l ( RunTi me/ Sampl ePer i od- 0. 0001) ;

i f ( EndCnt > 2^16)
msgbox( ' Too l ong of a r unt i me was ent er ed f or t he sampl e r at e. Pl ease choose a
shor t er r un t i me or l ower t he sampl e r at e. Reset t he QET and t r y agai n. ' )
r et ur n
end

%Set t he end of t i me count on QET
i f ~Wr i t eToQET( S, ' E' , ' uchar ' ) f cl ose( S) ; r et ur n; end;
i f ~Wr i t eToQET( S, EndCnt , ' i nt 16' ) f cl ose( S) ; r et ur n; end;
i f ~Wr i t eToQET( S, 0, ' i nt 16' ) f cl ose( S) ; r et ur n; end;

%I ni t i al l y si ze dat a ar r ay
Dat a = zer os( EndCnt , 7) ;

%Set r ef er ence si gnal t ype and dat a
i f ~Wr i t eToQET( S, ' S' , ' uchar ' ) f cl ose( S) ; r et ur n; end;
i f ~Wr i t eToQET( S, Si gType, ' i nt 16' ) f cl ose( S) ; r et ur n; end;
i f ~Wr i t eToQET( S, 0, ' i nt 16' ) f cl ose( S) ; r et ur n; end;

i f ~Wr i t eToQET( S, ' M' , ' uchar ' ) f cl ose( S) ; r et ur n; end;
i f ~Wr i t eToQET( S, Si gMagRat e, ' f l oat 32' ) f cl ose( S) ; r et ur n; end;

i f ~Wr i t eToQET( S, ' F' , ' uchar ' ) f cl ose( S) ; r et ur n; end;
i f ~Wr i t eToQET( S, Si gFr eq, ' f l oat 32' ) f cl ose( S) ; r et ur n; end;

%Send st ar t f l ag
i f ~Wr i t eToQET( S, ' G' , ' uchar ' ) f cl ose( S) ; r et ur n; end;
i f ~Wr i t eToQET( S, 0, ' f l oat 32' ) f cl ose( S) ; r et ur n; end;

%Read dat a
Cmd = 0;
Byt esReq = 0;

RunLoop = t r ue;
whi l e RunLoop

%di sp( S. Byt esAvai l abl e) ;
i f Cmd == 0

%Read command char act er i f i t i s avai l abl e
i f S. Byt esAvai l abl e >= 1

Cmd = f r ead( S, 1, ' uchar ' ) ;

%Det er mi ne number of byt es r equi r ed t o compl et e t hi s command
i f Cmd == ' X'
%Thi s i s t he st op f l ag
Cmd = 0;
Byt esReq = 0;
RunLoop = f al se;
br eak;
el sei f Cmd == ' d'

%Thi s i s t he dat a f l ag ( 1 l ong i nt eger and 4 f l oat i ng poi nt number s)
Byt esReq = 2 + 4*4;

Dat aCount = Dat aCount + 1;

end

end

end

%Check f or command dat a avai l abl e
i f Cmd ~= 0 && Byt esReq ~= 0

i f S. Byt esAvai l abl e >= Byt esReq

i f Cmd == ' d'

%Read dat a
QETDat aI ndex = f r ead( S, 1, ' ui nt 16' ) ;
Ref Si g = f r ead( S, 1, ' f l oat 32' ) ;
Act Ang = f r ead( S, 1, ' f l oat 32' ) ;
Vol t = f r ead( S, 1, ' f l oat 32' ) ;
Cur r ent = f r ead( S, 1, ' f l oat 32' ) ;
AngFD = f r ead( S, 1, ' f l oat 32' ) ;
AngFDFi l t = f r ead( S, 1, ' f l oat 32' ) ;

%Save dat a
Dat a( Dat aCount , 1) = QETDat aI ndex;
Dat a( Dat aCount , 2) = Ref Si g;
Dat a( Dat aCount , 3) = Act Ang;
Dat a( Dat aCount , 4) = Vol t ;
Dat a( Dat aCount , 5) = Cur r ent ;
Dat a( Dat aCount , 6) = AngFD;
Dat a( Dat aCount , 7) = AngFDFi l t ;

i f nar gi n == 6 && i sa( hScopeCal l back, ' f unct i on_handl e' )
hScopeCal l back( QETDat aI ndex*Sampl ePer i od, Ref Si g, Act Ang, Vol t ,
Cur r ent , AngFD, handl es) ;
end

end

Cmd = 0;

end

end

end

%Cl ose ser i al por t and cl ean up
f cl ose( S) ;
del et e( S) ;
cl ear S;

Er r or = f al se;

f unct i on Cl oseAl l Ser i al Por t s

Por t s = i nst r f i nd( {' Por t ' }, {ComPor t Name}) ;

f or i = si ze( Por t s, 1) : - 1: 1
f cl ose( Por t s( i ) ) ;
del et e( Por t s( i ) ) ;
end

f unct i on [ Por t Name] = ComPor t Name( )

i f eval i n( ' base' , ' exi st ( ' ' QETPor t Name' ' ) ' )
Por t Name = eval i n( ' base' , ' QETPor t Name' ) ;
el se
Por t Name = ' COM1' ;
end

f unct i on [ Success] = Wr i t eToQET( S, Val , For mat )

%Si nce MATLAB spor adi cal l y t i mes out on t hi s st at ement we' l l t r y i t up t o 5 t i mes
%KNOWN BUG: see ht t p: / / www. mat hwor ks. com/ suppor t / bugr epor t s/ det ai l s. ht ml ?r p=250986
Success = f al se;
f or i =1: 5

Er r or ed = f al se;
t r y
f wr i t e( S, Val , For mat ) ;
cat ch
Er r or ed = t r ue;
end

i f ~Er r or ed
Success = t r ue;
br eak;
el sei f i == 5
di sp( ' Unabl e t o wr i t e t o QET devi ce. Pl ease r eset i t and t r y agai n. ' ) ;
r et ur n;
end

end

MATLAB Based DC Motor Control Lab Page C.9.1

Appendix C: PIC C Code Flow Charts

Flowchart Discussion
Flowcharts of the pertinent PIC C code are available in Figure 9 through Figure
10. Figure 9 contains a flowchart of the main function that is run whenever the
PIC is started (power is applied or the reset button is pressed). Figure 10
contains a flowchart of the function that is run whenever the Run Control
process block is encountered in the main function.

While the main function is executing there are two other functions that may be
run when specific events occur. These events are the timer interrupt and the
serial RDA (received data available) interrupt. The timer interrupt occurs
whenever the amount of time specified by the sample rate (variable name fs)
has passed since the last timer interrupt (or since the time when the interrupt was
enabled). The serial RDA interrupt occurs whenever a single character (1 byte)
has been received over the serial port. When these events occur the execution
of the main function is stopped (interrupted) and the appropriate function is
executed. A flowchart of the function that is executed when the timer interrupt
occurs is shown in Figure 11. A flowchart of the function that is executed when
the serial RDA interrupt occurs is shown in Figure 12.

Figure 9 Main Function Flowchart
Initialize Hardware
Initialize Variables
Enable Interrupts
Running?
Do Control?
Yes
No
Output Data?
End of Time?
Yes
Run Control
Send Output
No
Yes
No
Yes
No
Received Data?
Process received data request and
set running flag if run command received
Yes
Send PC end flag
Disable interrupts
Set motor voltage
to zero
1 second
Delay
Reset CPU

Figure 10 Run Control Function

Figure 11 Timer Interrupt Function Flowchart

Figure 12 Serial Received Data Available Interrupt Function Flowchart
Read angular
encoder
Get new reference
signal
Read motor current
Calculate angular
rate
Run control logic
Output motor control
voltage
Set DoControl flag
to true
Save received Data
Set received data
flag to true if
complete command
has been received

Engle

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Engle

Uploaded by

Copyright:

Available Formats

MATLAB Based DC Motor Control Lab

=Motor angular rate

You might also like