\

= 
= =

=

= =
= = =
Gyroscope
The gyroscope is another important inertial sensor that is needed to achieve
successful balancing control of two wheel robots. The gyroscope measures angular rate
and determines how fast the two wheel robot is falling in either side. The units of angular
rate can be expressed as either degrees/sec or radians/sec. For the autonomous self
balancing two wheel robot, the gyroscope of choice that was implemented onto the
project was the IDG300. Figure 5 shows the IDG300 gyroscope. When the IDG300
gyroscope is not moving the analog outputs will output a voltage value of 1.5V. Only one
of the analog outputs (Y
out
) of the IDG300 was connected to one of the onboard analog
to digital converters on the PIC32 microcontroller. The microcontroller will read the
analog voltage from gyroscope output. The features on the IDG300 include;
 Dual Axis Outputs X
out
/Y
out
 Full Scale Range: +/ 500 /s
 Sensitivity: 2.0 mV//s
22
Y
IDG300
X
Vcc Gnd Xout Yout ST
Fig. 5. IDG300 gyroscope.
 Static Output (Bias): 1.5V
 Operating Voltage: V
cc
=3.3V
To calculate the angle rate from the IDG300 voltage output, the following
steps need to be executed. Steps 15 are coded into the PIC32 microcontroller software to
get the measured angle rate from the gyroscope.
Assume: V
cc
=3.3V, nbits=10, V
static
=1.5V
( )
( ) ( ) ( )
s
rad
rate Angular
s
rad
Gyro offest gyro Zero ADC
offset gyro Zero
V
V
Vcc
V
s
rad
Gyro
radians
ree
Gyro
s
Gyro
ree
s mV y Sensitivit Gyro
ADC
ADC
resol result
nbitd
static
resol resol
resol
o
resol
resol
_ ) _ _ )( 5 (
_ _ 396
3 . 3
) 1024 5 . 1 ( ) 2 (
) 4 (
sec
028123 . 0 )
180
(
sec
deg
61133 . 1 )
180
( ) 3 (
deg
sec
deg
61133 . 1
/ / 2
)
bit
mV
0.003223 (
_
) (
) 2 (
bit
mV
0.003223
2
(3.3V)
2
(Vcc)
) 1 (
10 nbits
= 
= =

=

= =

.

\

=

.

\

= = =
= = =
t t
23
DC Motors
In order for the autonomous self balancing two wheel robot to remain in a
vertical stable state, selection of good DC motors is important. DC motors with high
torque output and fast RPM make them ideal to be used on a two wheel robot system.
Selection of proper DC motors was an important great consideration for the robot system
used as part of this masters project. The motors used for the autonomous self balancing
two wheel robot are the popular Tamiya Gear head 380k75 DC motors. Tamiya motors
are very popular in control remote RC cars, because of there high torque output and high
RPM. Figure 6 displays the motor with the foam wheel attached. Characteristics of the
Tamiya gear head 380k75 DC motors include:
 Gear Ratio: 75:1
 Max. Voltage Supply: 7.2V
 RPM at no Load: 246 RPM
 Torque at Best Efficiency: 0.49Nm (0.4949kgm)
 Running Speed: 1m per 1.35sec
 Torque Constant: K
torque
=0.0739 kgm/A
Logomaticv2
The Logomatic v2 is a module that allows data to be saved onto a microSD
memory card. Logomatic serial data logger will be used to capture and save real time
control data from the two wheeled robot. The data logger is connected to one of PIC32
microcontroller USART transmit pin. The communication protocol between the data
logger module and the PIC32 is standard serial port setup (8 data bits, one stop bit, no
24
Fig. 6. Tamiya DC Motor with foam wheel.
parity, and data rate). The Logomatic module can handle microSD memory cards up to 2
GB. Once the data is saved onto the microSD memory card, the card will be placed in a
microSD carder reader. The data can also be downloaded from the module to a personal
computer via USB cable. When the USB cable is attached to the data logger, the
computer will see it as a removed mass storage device. The data saved on the memory
card will then be used to plot the control system output response using either Microsoft
Excel or MATLAB. Figure 7 shows an image of the Logomatic v2 serial microSD data
logger module. The module can also be used to charge a single cell lithium polymer
battery. Sparkfuns website, where the module was purchased, provides a sheet that
explains how to properly configure the Logomatic v2 module. The only limitation that
comes with using the Logomatic module is the write speed to the microSD card. The
longest write cycle speed to the microSD is 42.5 ms. If logging occurs faster than 42.5
ms, there is a risk for data loss. This can be a major issue if the module is set to
communicate with the PIC32 microcontroller at baud rate speeds of 115Kbps. To resolve
25
Fig. 7. Logomatic serial data logger.
this write speed issue, the data logger will communicate with the PIC32 at a lower baud
rate.
DC Motor Drivers
A set of DC motor drivers are needed to support the Tamiya gear head 380k75
DC motors, to successfully work on the two wheel robot. The motor drivers are critical in
getting any type motor to function properly. The drivers provide the high voltage and
current levels outputs necessary to drive the motor. Some available motor drivers have
inputs that allow the user to control the motors speed and direction.
The chosen DC motor driver for this masters project is the VNH2SP30 from
STMicroelectronics. Figure 8 show s an image of the VNH2SP30 DC motor driver. The
VNH2SP30 is a dual DC motor driver that is able to drive two independent DC motors.
Each DC motor driver has inputs that vary the motors speed and direction. The motor
speed is controlled by a PWM input. Directions are determined by toggling the direction
26
2

E
N
2

I
N
A
2

I
N
B
2

P
W
M
2

C
S
V
I
N
G
N
D
V
C
C
G
N
D
V
I
N
1

E
N
1

I
N
A
1

I
N
B
1

P
W
M
1

C
S
Fig. 8. VNH2SP30 DC motor driver.
inputs to either a 0 (0V) or 1 (5V). Some of the characteristics of the VNH2SP30 DC
motor driver include:
 Maximum PWM Frequency: 20 KHz
 Maximum Output Current (continuous): 30A
 5V Logic Level Compatible Inputs.
 Maximum Operating Supply Voltage: 16V
 MOSFET onresistance (per leg): 19 m
Power Supply
The power supply is another important component of this project. There are
many types of batteries that have different chemical makeup. Different battery types have
advantages or disadvantages over one another in terms of power capacity. For the two
27
wheel robot implementation on this masters project a compact yet with high power
capacity battery was chosen. The battery type of choice for this project is the Nickel
Metal Hydride batteries. NiMH batteries will provide the required power needed to drive
all of the electrical devices on the two wheel robot. The NiMH battery to be used on the
robot has a power rating of 4.8V at 2000mAh. Physical NickelMetal Hydride battery is
shown on Figure 9.
Fig. 9. NickelMetal Hydride battery.
Three NiMH batteries will be used and hooked in a series configuration.
When the three batteries are configured in a series configuration, there will be an increase
in voltage up to 14.4V. The current will remain constant at 2000mAh. Figure 10 shows
an image of a series battery configuration. The two wheel robot will run from +3.3V, 5V,
and 9V power supplies. These three voltage values will run the onboard electronics on
the robot. The three NiMH battery packs will provide the total of 14.4 voltages. The
onboard voltage regulators will provide the three stepped down voltage values. When
28
Fig. 10. Nickelmetal hydride cell batteries.
using NiMH batteries proper caution needs to be exercised. If not properly charged with
the correct NiMH charger, the batteries will swell up and can catch on fire.
29
CHAPTER IV
METHODOLOGY
The following sections will present and provide in depth details on the
methods used to implement both the Kalman filter and the PID controller. The
methodology section will start talking about the Kalman Filter and its algorithm and
corresponding equations. The purpose of implementing the Kalman filter is that it
provides sensor fusion for the autonomous selfbalancing two wheel robot. The other half
of the methodology section will discuss the theory behind the PID controller and its
implementation in this project.
Kalman Filter
Since its introduction in the early 1960s, the Kalman filter has being widely
used in the control engineering community. The Kalman is a recursive digital filter that
provides a very effective means of estimating the state of any process. The Kalman filter
can be thought of being a state estimator. Kalman filtering can be used as a tool to
provide a reliable state estimate of the process. Another important feature of the Kalman
filter is its ability to minimize the mean of the square error and provide a solution for the
least square method. The Kalman filter can be used on a control system that is exposed to
noisy environments because it minimizes the square error. The filter can reduce noisy
measurements from sensors data before its fed into any control system. Noisy
30
measurements from the sensors will not allow the system to reach stability nor get a
reasonable output response. As stated by Rich Ooi, the Kalman filter does not require all
previous data to be kept in storage and reprocessed every time a new measurement is
taken [7, p. 13]. Having this filter characteristic, the Kalman filter can be implemented
on a microcontroller and in sofware.
Discrete Kalman Filter Algorithm
On this section, both the algorithm and corresponding Kalman filter equations
will be discussed. Before the Kalman filter can be used to get rid of noise from a sensor
signal. The process or system that is being measured must be modeled by linear system.
A linear system can best be described by the following two state space representation
equations (6.0) and (6.1).
(6.0)
(6.1)
Equation 6.0 represents the process state equation. A, B, and H represent the
state matrices, the w
k1
represents the process noise, k is the time index, x
k1
is the state of
the process, and u
k1
is the known input to the process. Equation 6.1 above represents the
output state equation. The z
k
represents the measured process output and the v
k
represents
the measurement noise. The sensor data for the system are taken at discrete time sample
points. The measurements are dependent on the state of the process or system.
There is no association between the measurement noise (v
k
) and the process
noise (w
k1
), there are independent random noise variables. The process noise and
k k k
k k k k
v Hx z
w Bu Ax x
+ =
+ + =
1 1 1
31
measurement noise can be best described by there covariance matrices Q
w
and R
v
. The
covariance matrices are then written as
(6.2)
(6.3)
Where T represents a matrix transpose and E represent the expected estimated
value. Both the process noise covariance (Q
w
) matrix and the measurement noise
covariance (R
v
) matrix play a vital role in the overall output performance of the Kalman
filter. These two matrices values can be either calculated using advance statistics
equations or can be manually tuned until the desired filter output response is achieved.
The noise covariance matrices (Q
w
) and (R
v
) can also be adjusted dynamically. But for
the autonomous self balancing two wheel robot project, the noise covariance matrices
will remain constant and be adjusted manually.
The Kalman filter estimates the process by using a feedback scheme. First the
filter estimates the systems state at some time step and then gets noisy measurements in
the form of feedback. Therefore, the equations for the Kalman filter fall into two categories,
the time update equations and measurement update equations. The time update equations
can be thought as the predictor equations. The measurements update equations can also
be thought as corrector equations. These two equation types form the basis for the
Kalman filter algorithm, which resemble a predictorcorrector estimation algorithm.
Figure 11 shows the Kalman filter predictorcorrector estimation algorithm cycle.
Since the Kalman filter algorithm is based on a predictorcorrector scheme, its
equations can now be described based on that. The predict stage displays the current state
] [
] [
T
k k v
T
k k w
v v E R
w w E Q
=
=
32
Fig. 11. Kalman filter estimation algorithm cycle.
and error covariance to the next time step. The correct stage is responsible for adjusting
the expected estimate from the prediction; by incorporating a new measurement at the
current time step. Figure 12 shows the complete Kalman filter algorithm with equations
and how it relates to the predictorcorrector cycle.
x

k
=Ax

k1
+Bu
k1
P

k
=AP
k1
A
T
+Q
Time Update Step
[ Predict State ]
(1) Project State Ahead
(2) Project Error Convariance Ahead
Measurement Update Step
[ Correct State ]
K
k
=P

k
H
T
(HP

k
H
T
+R)
1
x
k
= x

k
+ K
k
(z
k
 Hx

k
)
P
k
=(I K
k
H)P

k
(1) Calculate Kalman Gain
(2) Update Estimate with Measurement
(3) Update Error Covariance
Fig. 12. Kalman filter algorithm equations.
33
Sensor Fusion Using the Kalman Filter
The reason for using sensor fusion is stated by Rich Ooi as follows, the
accuracy and reliability of information regarding its operating environment for these
mobile robots is critical, as these systems are usually autonomous. These requirements
call for highly accurate sensors which are very expensive. Sensor fusion technology,
where signal from several sensors are combined to provide an accurate estimate, is the
most widely used solution. The Kalman filter is used in a number of multisensor systems
when it is necessary to combine dynamic lowlevel redundant data in real time [7, p. 5].
Examples of multisensor systems include navigation systems and inertial
measurement units. The Kalman, filter uses the statistical characteristics of a
measurement model to recursively determine estimates for fused data that are optimal in
a statistical sense. The recursive nature of the filter makes it appropriate for use in
systems without large data storage capabilities [7, p. 5], such as the PIC32
microcontroller.
This section on the sensor fusion, using the Kalman filter, details on the
experiment conducted on the inertial sensors used as part of the project. Figure 13
illustrates the test bed for both the IDG300 gyroscope and ADXL203 accelerometer.
The image shows how an angle finder was used as a visual angle reference on the
autonomous selfbalancing two wheel robot. The figure also shows how the two wheel
robot was programmed using a laptop and a Microchip ICD 2 programmer.
34
Fig. 13. Gyroscope and accelerometer sensor test setup.
The digital IDG300 gyroscope can only provide a measure of angular change.
Angular change or rate refers to how fast an object is rotating in radians per second. The
output of the gyroscope can be considered the derivative of the measured output angle
from the accelerometer. The gyroscope tends to have a faster reaction to change as
compared to an accelerometer. A very unique feature of the gyroscope is that it has a rest
average value, also referred to as a bias. The average value is given when the gyroscope
is motionless. The bias has to be corrected at every measurement to get accurate velocity
data. Gyroscopes do not hold onto the angular rate output change. When an object is
tilted, the angular rate output changes and then quickly returns to its resting average
35
value. Since a gyroscope does not hold on to the output value, it cant be used as a tilt
sensor. One of the major problems that most gyroscopes have is that output average value
tends to drift with time when the gyroscope is motionless. The output drift can be cause
by changes of device operating temperature or the internal physical properties of the
gyroscope itself. This drift can introduce significant errors in the angular measurements.
Figure 14 shows the plot of the raw gyroscope data.
Fig. 14. Gyroscope measurements with drift.
Observed from Figure 14 that as time progresses the gyroscope starts to drift
away from its average resting value of 1.45 rad/s.
36
The ADXL203 accelerometer is the other sensor that is implemented on this
robot project. The accelerometer is a sensor that measures acceleration in a predefined
axis. The sensor can provide a handy reference point to determine which way is up versus
down orientation. The ADXL203 accelerometer mounted on the robot was chosen
because of it can be used as a tilt sensor. When configured as a tilt sensor, the
accelerometer can measure static accelerations. The ADXL203 accelerometer uses
earths downward gravity force of 1G as its reference. So the sensor has a range of +/
1G, which can be converted to a range in degrees of +/ 90
o
. An advantage of using the
ADXL203 accelerometer as a tilt sensor is that it can hold on to its output angle value. It
will remain at that angle until it gets disturbed by an external force. Since the ADXL203
accelerometer can be used as a tilt sensor, it seems that only sensor that is needed. There
are some issues that may arise if the accelerometer is only sensor used as the input sensor
to the robot control system. One issue that most accelerometer sensors have is that the
output angle tends to have a very slow reaction to change. Another problem is that the
sensor is very sensitive to noisy environments and vibrations. The accelerometer output is
usually corrupted with noise. Figure 15 shows the results of non filtered accelerometer
raw measurements.
From Figure 15, it can be observed that an accelerometer used alone on the
robot cannot get a reliable tilt angle data. The figure also indicates the need to implement
a Kalman filter on the robot.
Since both the gyroscope and accelerometer have problems in the outputs, the
sensors used alone cant provide very reliable data. If the sensor information is not
reliable, the two wheel robot will have problems achieving a stable state. To overcome
37
0 10 20 30 40 50 60 70 80 90
33.6
33.8
34
34.2
34.4
34.6
34.8
35
Accelerometer Raw Sensor Data
Time(sec)
A
n
g
l
e
u
[
r
a
d
]
Accelerometer Data
Fig. 15. Noisy accelerometer measurements.
the gyroscope and accelerometer output data problems, a signal level sensor fusion
technique can be used. Sensor fusion can be achieved thru the use of a Kalman filter
algorithm. This fusion technique combines the output signals of the sensors with the
objective of providing an output signal in the same form as the original signal, but with
better quality. The sensor fusion was used on this project to provide means to overcome
some of problems associated with the sensors outputs. In this case, the ADXL203
accelerometer is used to eliminate the drift problem from the gyroscope signal. The IDG
300 gyroscope eliminates the corrupt accelerometer output data by using its clean stable
resting average value. As a result of using the sensor fusion with the Kalman filter, a
proper estimate of the tilt angle and its derivative term is obtained.
38
Kalman Filter Process Model
In order for the Kalman filter to successfully be implemented onto the two
wheel robot, an accurate model needs to be developed. The process also needs to be
modeled as a linear system for Kalman filter implementation to be successfully. As
mention in the previous section the Kalman filter will use the data from the accelerometer
to eliminate the drift problem from the gyroscope output signal. In the process of using
the filter, unwanted noise from the accelerometer will either be minimized or completely
eliminated. The Kalman filter process model will be modeled as a single dimensional
inertial measurement unit. For the single dimensional inertial measurement unit, a two
state Kalman filter is implemented to track the angle of the two wheeled robot and
gyroscope bias value. The simple process model using the gyroscope input data can be
modeled in state space representation as shown on equation 6.4.
(6.4)
The single dimensional inertial measurement unit process model will be used
to implement the Kalman filter algorithm into the two wheel robot.
k
k k
k
k k
k k k
u
dt dt
u
dt
angle dot
angle dt
angle dot
angle
Bu Ax x
(
+
(
=
(
+
(
=
(
+ =
0 * 1 0
1
*
0 _ 1 0
1
_
1
1
1 1
u
u
u
u
39
PID Controller
The control algorithm that was used to maintain it balance on the autonomous
selfbalancing two wheel robot was the PID controller. The proportional, integral, and
derivative (PID) controller, is well known as a three term controller. Since its
introduction, more than sixty years ago, the PID controller has been the popular choice in
industrial process control. Due to the simplicity of the controller, it has become the basis
for many advanced control algorithms and strategies. The PID controller was first
introduced as a purely mechanical controller. Being used as a mechanical controller, it
saw its first mechanical implementation on controlling pneumatic systems. After being
successfully being used to control mechanical systems, it started to be implemented in
electrical analog circuits. PID implementation in analog circuits has made it possible to
control systems such as house heaters to chemical process plants. As microprocessors and
microcontrollers have become popular in control engineering, the PID controller has
become a popular embedded software implementation. This PID controller being
implemented in software has out performed the analog and mechanical versions of the
controller. The controller can now be programmed onto a single integrated circuit chip.
PID Controller Algorithm
In this section the PID controller algorithms software implementation on a
microcontroller and tuning will discussed. The PID control algorithm is a very straight
forward algorithm that provides the necessary output system response to control a
process. One unique feature of the PID controller is that it is capable of manipulating the
process inputs based on the history and rate of change of the signal. The algorithm is best
suited when the process under control is modeled as a linear system. This gives more
40
accurate and stable control. The PID controller consists of proportional, integral, and
derivative terms. The terms of the PID controller are summed up to create a controller
output signal. Each term performs a different task in the control process. The controller
terms also have different effects on the system output response. Figure 16 shows the
classical continuous time PID controller.
Fig. 16. Continuous PID controller.
As shown from the figure, the input to the controller is the error from the
system and the u is the output controller signal. The K
P
, K
I
, and K
D
are referred as the
proportional, integral, and derivative constants. The PID controllers are widely used on
closed loop control systems, where the process output measurement is fed back to the
system and gets processed by the controller. Figure 17 shows the integration of the PID
controller on a general close loop control system.
The closeed loop control system shown on figure 17 is also referred to as a
negative feedback system. The basic idea of a negative feedback system is that it
measures the process output y from a sensor. The measured process output gets
subtracted from the reference setpoint value to produce an error. The error is then fed
into the PID controller, where the error gets managed in three ways. The error will be
41
Fig. 17. Closed loop control system with PID controller.
used on the PID controller to execute the proportional term, integral term for reduction of
steady state errors, and the derivative term to handle overshoots. After the PID algorithm
processes the error, the controller produces a control signal u. The PID control signal then
gets fed into the process under control. The process under PID control is the two wheeled
robot. The PID control signal will try to drive the process to the desired reference set
point value. In the case of the two wheel robot, the desired setpoint value is the zero
degree vertical position. The PID control algorithm can be modeled in a mathematical
representation. Figure 18 shows the algorithm in its time domain mathematical form.
Equation 6.7 represents the continuous time domain of the PID controller.
Both the T
D
andT
I
represent the time constants for both the derivative and integral terms.
Since the PID controller algorithm will be implemented on a PIC32 microcontroller,
equation 6.7 cant be programmed in the time domain. The solution to implementing the
PID controller will be discussed later in the section.
The K
P
, K
I
, and K
D
will then be examined to show how the three term gain
constants affect the system output response performance. The first gain term to be
examined is the proportional K
P
constant. Setting both the K
I
and K
D
term gains from
42
dt
t derror
K dt t error K t error K t u
T K K
T
K
K
where
dt
t derror
T dt t error
T
t error K t u
y ref error
D
t
I P
D P D
I
P
I
D
t
I
P
) (
) ( ) ( ) (
) (
) (
1
) ( ) (
0
0
+ + =
=
=


.

\

+ + =
=
}
}
Fig. 18. PID algorithm in time domain representation.
equation 6.7 to zero, gives a proportional control algorithm. The proportional K
P
gain
gives a process control that is the proportional to the error. The error of the system is
multiple by the K
P
gain. One of the major benefits of the K
P
gain is that it improves the
rise time of the system. An improved rise time gives the system under PID control a
faster recovery response from a disturbance. The disadvantages of using only the K
P
gain
is that a K
P
gain that is too high of a value will cause system to be unstable A low K
P
gain will cause the system to drift away and never reach the reference set point value.
The next PID gain to be examined is the integral K
I
term constant. Using only
the integral K
I
gain in the system will never achieve the desire system output response.
The system will have a slow response to system disturbances. It will also cause the
system to oscillate and become unstable. In common practice the integral K
I
gain is used
together with the proportional K
P
gain. The resulting control algorithm is the PI, where
the K
D
gain term in equation 6.7 is set to zero. The unique feature of the integral gain is
43
that it provides a way to sum or add the system errors. The summing of the process errors
will continue until the output of the process reaches the desire set point value. Including
the integral K
I
gain along with the K
P
gain helps eliminate or reduce the steady state
error. The steady state error is the offset that can occur between the desired value and the
process output over time. The major disadvantage of the integral K
I
gain in the control
algorithm is that it is subject to integral windup or integral saturation. Integral windup is
caused when there is to large of an error between the reference setpoint value and the
measured process. A large error might come from a disturbance that lasts a long time
which the system can not handle. The disturbance causes the integral gain term to add the
error continuously. Integral windup prevents the error signal from reaching a steady state
zero value. A quick solution to prevent integral windup on a PI or PID controller is to set
the integral term between a maximum and a minimum value. Once the integral term
reaches the maximum and minimum value, the integral term gets reset to a predefined
value.
The derivative K
D
gain term is mostly used along with the controller
algorithm. The derivative K
D
gain is commonly used with a proportional K
P
gain which
makes up the PD control algorithm. The PD algorithm is derived by setting the K
I
gain
term in equation 6.7 to zero. The derivative gain term can also be used on the full PID
controller. When used on a PD or PID controller, the derivative term can help increase
the systems rise time. The main function of the derivative term is used to improve the
response to a sudden change in the systems state. The derivative term also improves the
stability of the system, reduces the overshoot, and improves the transient response. The
K
D
gain term can be thought as a damper on the systems output. The damping effect can
44
help the system reach stability more rapidly. The major drawback of using the derivative
term is that it is subject to signal noise and that in a control system an improper K
D
value
can lead to system instability. It is possible for the noise to come from noisy sensor
measurements. If signal noise becomes an issue, a software or hardware filter needs to be
adapted. A Kalman filter may be implemented in software to reduce the signal noise.
Figure 19 shown below summarizes the effects of the proportional, integral, and
derivative terms on the system output response.
Fig. 19. PID controller characteristics.
When using the PID control algorithm to control a system, the control
engineering designer needs to be aware that not all parts of the system that need to be
controlled need the full three term PID controller. If the system has a good output
response with only the PI or PD controllers, there is no need to implement the full PID
control algorithm. When using only the PI or PD controller, the implementation of
control algorithm will be kept simple.
Discrete PID Controller Implementation
Implementing the PID control algorithm in the continuous time domain, as
shown in equation 6.7, isnt practical in industry. Trying to develop a PID software
45
algorithm that follows the time domain notation is impossible. A common approach used
to implement the PID algorithm in software is to approximate the PID terms in equation
6.7. The approximation of the controller terms is achieved when the PID terms are
rewritten in discrete form. When the PID control algorithm is written in discrete form,
software implementation of the PID controller is possible. The discrete representation of
the controller makes it easier to implement the PID algorithm on either a microcontroller
or microprocessor. Figure 20 shows the discrete representation of the PID control
algorithm.
( ) ) 1 ( ) ( ) ( ) ( ) (
)) 1 ( ) ( ( ) ( ) ( ) (
) 1 ( ) ( ) (
) ( ) (
1
1
1
0
+ + =
=
=


.

\

+ + =
~
~
}
=
=
=
k error k error K k error K k error K k u
T
T K
K
T
T K
K
where
k error k error
T
T
k error
T
T
k error K k u
T
k error k error
dt
t derror
k error T dt t error
D
k
I P
S
D P
D
I
S P
I
S
D
k I
S
P
S
k
S
t
Fig. 20. PID controller discrete representation.
46
Figure 20 shows how the continuous time domain PID control algorithm can
be approximated and represented in discrete form. The T represents the sample time
while the k represents the sample number. Both equations 6.8 and 6.9 show how the PID
terms can be approximated in discrete form. The final discrete form, of the PID control
algorithm that will be implemented in software is shown in Equation 6.11.
ZieglerNichols PID Tuning
In order for the closed loop system to work correctly, the PID controller must
be correctly tuned. The constant values for the PID controller play a very vital role in the
system output response. To find the PID constant gain values, a tuning procedure on the
PID needs to be carried out. The sample time in the algorithm is also another important
parameter when tuning the PID controller. There are numerous ways to tune the PID
controller, to achieve desire output response. The widely used tuning algorithm is the
ZieglerNichols which was introduced back in the 1940s. The ZieglerNichols PID tuning
algorithm is the preferred choice to manually tune any PID controller used in industry
today.
To carryout the procedure to tune the PID controller using the ZieglerNichols
is very straight forward. Since the self balancing two wheel robot is classified as a closed
loop system. The close loop form of the ZieglerNichols tuning algorithm will be used to
find the controller gain values. The following steps describe the procedure needed to
apply the closed loop PID tuning method.
 First both the integral K
I
and the derivative K
D
gains from equation 6.7 need to
be set to zero. The controller will have only the proportional K
P
gain action on the closed
loop.
47
 Second carry out a set point test on the system and observe the system output
response.
 Repeat the set point test by either incrementing or decrementing the
proportional K
P
gain, until a stable oscillation is achieved at the output.
 The stable oscillation observed at the system output needs to be of equal
amplitude and period. The gain that produced the output oscillation is called the ultimate
gain K
U
.
 Read and record the period of the steady output oscillation. This period is the
ultimate period P
U
.
Once the ultimate gain K
U
and the ultimate period P
U
are recorded, the final
step is to calculate the PID controller gain values. Figure 21 lists the ZieglerNichols
tuning formulas used to calculate the PID controller gain values.
Controller K
P
T
I
T
D
P
PD
PI
0.50*K
U
0.65*K
U
0.45*K
U


0.85*P
U

0.12*P
U

PID 0.65*K
U
0.50*P
U
0.12*P
U
Fig. 21. ZieglerNichols PID tuning parameters.
48
There are four different types of controller configurations that can be used
with the ZieglerNichols tuning algorithm as shown in table 6.1. The time constants T
I
and T
D
are referred to as the integral and derivative time constants. Both of these time
constants are used to calculate the K
I
and K
D
controller gains. The following example
demonstrates the use of the ZieglerNichols tuning algorithm to find the PID controller
gains. Figure 22 shows the ultimate gain Ku and ultimate period P
U
values that will be
used in the ZieglerNichols tuning example,
Fig. 22. ZieglerNichols close loop tuning test.
For the example, the controller to be designed is the PID controller. Figure 23
shows an example using the values from Figure 22.
From the example, the PID controller gains were calculated using the Ziegler
Nichols tuning method. After obtaining the PID gain values, the gains need to be plugged
back into equation 6.11. The main idea behind using the ZieglerNichols tuning method
49
( )( )
( )( )
0 . 663
529 . 1
0 . 65
0 . 663
050 . 0
51 . 0 0 . 65
529 . 1
125 . 2
050 . 0 0 . 65
51 . 0 25 . 4 120 . 0 120 . 0
125 . 2 25 . 4 50 . 0 50 . 0
0 . 65 0 . 100 65 . 0 65 . 0
sec 050 . 0
25 . 4
0 . 100
=
=
=
= = =
= = =
=  = =
=  = =
=  = =
=
=
=
D
I
P
S
D P
D
I
S P
I
U D
U I
U P
S
U
U
K
K
K
T
T K
K
T
T K
K
P T
P T
K K
T
P
K
Fig. 23. Example.
is to first force the system to be unstable in order to record both ultimate gain K
U
and
ultimate period P
U
. Then use Figure 21 to design the PID controller.
The ZieglerNichols tuning method is one of many ways to tune a PID
controller. When using the ZieglerNichols tuning method, the designer needs to setup a
test plan to observe the system output response. The PID controller designer needs to
understand the specifications that are part of the output response. Figure 24 shows a
typical closed loop system output response waveform.
50
T
P T
R
T
S
t
y
% OS
1
0.9
0.1
Fig. 24. Typical close loop output response.
Figure 24 shows the typical system response terms. Each of the parameters
shown are close loop specifications that are taken into account when designing any
control system. The response specifications allow the control engineer to analyze the
performance of the system under control. The close loop specifications are defined as
follows:
 Peak Time T
P
: The peak time is the time it takes to reach the first maximum
peak of the output response.
 Settling Time T
S
: The settling time is the time required for the transient
response to be damped down to a steady state value.
 Rise Time T
R
: The rise time is the time needed for a waveform to from 0.10
to 0.90 of the final value.
51
 Present Overshoot %OS: The present overshoot is the amount the waveform
overshoots the desire final value.
Sampling Rate
The sample rate, in a discrete PID control algorithm, plays an important role
in the execution of the closed loop system. The sampling rate in a discrete time controller
is both affected by the software and hardware structure of the controller. Choosing the
right sampling rate in a discrete time controller determines its overall performance. Some
considerations when selecting a suitable sampling rate include the following:
 First, if the sampling rate is too large, the closed loop system might
experience excessive overshoot oscillations. These overshoot oscillations can lead to
system instability.
 Next, if the sampling rate is too low, the discrete time system will start to
resemble the same behavior as a continuous time system.
 Finally when choosing the proper sampling rate, the time should be long
enough for the proper execution of the PID control algorithm. A 50 ms sample rate was
chosen to run the PID control algorithm.
The goal is to have a sampling period for a discrete time system that
resembles the same response as that of a continuous time system. A disadvantage of
having too short of a sampling period is that it can lead a closed loop system to have
signal delays.
52
CHAPTER V
RESULTS AND PERFORMANCE
EVALUATION
This section talks about the results and the performance evaluation of the
balancing robot system. Followed by discussing the implementation and testing
completed in the duration of the project. The results were logged by the serial data logger
and plotted using MATLAB. The Kalman filter and the PID controller were programmed
in C language. A P1C32MX360F512L microcontroller was programmed to implement
the filter and the closed loop PID control algorithms for the balancing robot system. A
built in timer from the P1C32MX360F512L microcontroller was used to provide an
accurate sample time for both of the algorithms. A system block diagram representing the
implemented robot components is shown in Figure 25.
Figure 25 shows a block diagram that details the data flow between the
electrical components. The block diagram also displays the actual balancing robot system
implementation. The balancing robot system results were collected using the block
diagram layout. The first results presented will be the Kalman filter results, followed by
the PID controller results.
53
u

u
u
Fig. 25. Functional block diagram for balancing system.
Kalman Filter Results
As previously described, the main objective of using the Kalman filter in an
application is to eliminate most of measurement noise from the accelerometer sensor. The
filter also helps to reduce the drift problem that the gyroscope sensor experiences. The
Kalman filter algorithm will provide a better angle estimate to the true angle from the
accelerometer. For the filter to work properly, a manual tuning method needs to be
carried out. The tuning filter parameters that needed to be tuned are the covariance
matrices Q
w
and R
v
. The Q
w
matrix consists of the process noise and the R
v
matrix is the
measurement noise. Both matrices take the form as shown on Figure 26.
Values of Q_accelerometer and Q_gyroscope are set depending on how much
confidence one puts on the particular sensor. A higher value indicates that the particular
sensor is trusted less as compared to the other sensor. The value of the R_measurement
matrix indicates the amount of noise expected from the measurement, a low value
indicates that the measurement is less corrupted with noise.
54
  t measuremen R R
gyroscope Q
ter accelerome Q
Q
_
_ 0
0 _
=
(
=
Fig. 26. Process and measurement noise covariance matrices.
The filter tuning method used on this project was done by trial and error. Once
the tuning covariance matrices Q
w
and measurement matrix R
v
constant parameters
provided the desire output response, the constant values were kept. The simple test
platform shown in Figure 13 was used for the Kalman filter experimentation. Both the
gyroscope and the accelerometer were rotated at angles between +1 10 degrees. Before
the Kalman filter can be implemented and tested, the accelerometer and gyroscope raw
sensor data has to be scaled. The accelerometer data was scaled in radians, while the
gyroscope data was scaled in radians per second. If the senor data is not scaled properly,
the digital filter will not work as expected. The Kalman filter algorithm was set to run
every 50 ms. The following Kalman filter waveforms demonstrate the output response
when the covariance matrix Q
w
values get varied and R
v
remains constant. The red
waveform represents the noisy accelerometer angle data and the blue waveform
represents the Kalman filter angle data. Figure 27 shows the output response when the
filter is not properly tuned.
From Figure 27 it is clearly shown that the filter output response is not tuned
properly. The filter resembles a saw tooth waveform. The filter doesnt track the true
accelerometer angle in real time. The output of the Kalman filter has a very slow
5
5
Fig. 27. Not properly tuned Kalman filter.
56
response to changes from the accelerometer data. This large delay in Kalman filter can be
a major problem when the estimated accelerometer angle is fed into the PID controller.
From figure 24, it can be observed that the process noise covariance matrix parameter
Q_gyroscope was set to have a lower value than the Q_accelerometer. A lower
Q_gyroscope value means that the gyroscope sensor data is trusted more than the
accelerometer data. From the Kalman filter results it seems that the Q_gyroscope value
controls the filter response to changes causing the saw tooth like waveforms. Also from
figure 24, the disturbances towards the end of the waveform were cause by shaking the
robot. The shaking disturbance shows how the Kalman responses to external forces. The
same disturbance was injected to the system for the remaining Kalman waveforms below.
To solve the slow output response of the Kalman filter, the Q_accelerometer was set to
have a lower value than the Q_gyroscope. In this case the accelerometer sensor data will
be trusted more than the gyroscope data. Figure 28 demonstrates this Kalman filter case.
From Figure 28, it is observed that the filter output response has improved.
The Kalman filter tracks the accelerometer sensor data in real time. A problem was
observed in Figure 28, when the filter is initialized; the filter tends to have a dramatic
overshoot. After approximately 5 seconds, the filter recovers from the overshoot. The
other problem that was observed, from the figure, is that every time there is a change in
angle there are overshoots that follows. The Kalman filter does an excellent job
eliminating the overshoots when the filter is operating. These overshoots can become an
issue if the estimated Kalman angle gets fed into the PID controller.
The solution to the overshoot problem can be solved by adjusting the process
noise covariance matrix parameter Q_gyroscope. After numerous trials and errors, the
5
7
Fig. 28. Kalman filter output response varying Q matrix.
58
optimal covariance matrix Q
w
values were set as Q_accelerometer=0.001 and
Q_gyroscope=0.002. These process noise covariance matrix values offer the best filter
output response. Figure 29 shows the results for these matrix values.
Figure 29 shows a very clear observation that the Kalman filter output
response has reduced or eliminated the overshoots. The figure also illustrates how rapidly
the filter tracks the accelerometer angle data. The initial overshoot that the Kalman filter
experiences gets reduced and dies off after about 3 seconds. After the initial overshoot,
the filter operates normally as expected.
The measurement noise covariance matrix R
v
, determines much the
measurements should be trusted. Setting the covariance matrix R
v
to a high value means
that the sensor measurements are too noisy. Too low of a covariance matrix R
v
value
means that the measurements are less noisy. The Kalman filter parameter also plays a
major role in the performance of the filter. Figure 30 shows when the covariance matrix
R
v
value is set too low.
In Figure 30, the R_measurement was set to 0.000059. The waveform results
clearly show that the Kalman filter has an output response that is similar to the
accelerometer waveform. This type of Kalman filter response has the same amount of
noise as the raw accelerometer data. The noisy filter data cannot be used in the balancing
robot system. To solve the noisy Kalman filter data, the measurement noise covariance
matrix was set to 0.59. Figure 29 displays the waveform when the measurement noise
covariance matrix is set to 0.59.
When using the Kalman filter, numerous trial and error need to be carried out
to find the optimal values. The Kalman filter covariance matrix values that were chosen
5
9
Fig. 29. Optimal Kalman filter Q matrix values.
6
0
Fig. 30. Kalman filter output response varying R matrix.
61
to give the best results were hard coded into software. The measurement noise and the
process noise covariance matrix values are listed as follows:
 R_measurement=0.590
 Q_accelerometer=0.001
 Q_gyroscope=0.002
These filter parameters offered the best performance and results. Figure 29
shows the Kalman filter results when using the parameters listed.
The pervious results showed how the Kalman filter helped to reduce or
eliminate most of the noise from accelerometer. The other results also captured the
performance of the Kalman filter in the gyroscope sensor data. As mentioned before, the
gyroscope has the problem of its steady state value drifting away over time. The Kalman
filter algorithm helps eliminate the drift problem on a motionless gyroscope. Figure 31
shows the results for a motionless gyroscope. The blue waveform represents the Kalman
filter data while the red waveform represents the raw gyroscope sensor data. Figure 31
shows how well the filter performed in preventing the drift problem caused by the
motionless gyroscope. The Kalman filter average drift value remained within 0 rad/s. The
spikes in the waveforms were caused when the robot chassis was moved. After passing
the gyroscope raw sensor data thru the Kalman filter, the estimated velocity can be used
along with estimated angle.
PID Controller Results
The objective of the PID controller is to provide control and stability. The PID
consists of three gain terms that affect the output response of the system. The
6
2
Fig. 31. Kalman filter motionless gyroscope result.
63
proportional K
P
gain determines the rise time of the system. The integral K
I
gain affects
the steady state error. The derivative K
D
gain controls the system response overshoot and
helps with its stability. Figure 32 displays the output response result captured for the self
balancing robot.
Fig. 32. Measured system output response.
Results from Figure 29 show the estimated Kalman filter angle measurement.
This plot also displays the closed loop output response for the robot system. Figure 29
displays how robot tried to maintain a stable state by hovering around the zero radian
value. The measured angle hovered around the zero radian value. The figure also shows
64
that the first few seconds the system response had an overshoot. This initial overshoot is
caused by the initial Kalman angle estimate, but the system soon recovers from the
overshoot. The output response of the system still suffers from the noise generated by the
sensors. All efforts were made to minimize as much of the signal noise as possible.
Results were also captured for both the input and output of the PID controller.
The input to the controller is the error. The output of the controller provides the drive
signal used to throttle up or down the motor speed. Figure 33 shows the input to the PID
controller.
Fig. 33. PID controller input error.
65
The goal or the ideal case is to drive the error signal input to zero. As
mentioned before, error signal from the system also suffers from an initial overshoot. The
overshoot quickly dissipated within a few seconds. The output to the PID controller is
shown on Figure 34. The PID controller output signal gets fed into VNH2SP3O DC
motor driver. The signal determines how fast the motors are driven to prevent the robot
chassis from tipping over. The objective is to have the drive signal get as close as
possible to zero.
Fig. 34. PID controller output.
66
The PID gain values that were used for the PID controller are:
 P= 1800.001
 I= 0.10250
 D= 95.7501
67
CHAPTER VI
SUMMARY, CONCLUSION, AND
RECOMMENDATIONS
Summary
The autonomous selfbalancing robot project was very successful in achieving
its goal. The robot maintained its vertical balance position. The overall robot control
system is classified as a single input single output, also known as SISO. Two sensors
were used to provide tilt angle and angular velocity information. Only the accelerometer
tilt angle data was used in the closed loop system. The accelerometer measures the
inclination of the robot structure, while the gyroscope provides the angular rate. To attain
stability, a digital filter and a control algorithm were implemented as part of the project.
The Kalman filter was the filter of choice to code in software. The Kalman filter fused
both of the sensors to provide reliable estimated angle information. The Kalman filter
was successful in removing most of the noise from the sensor measurements. The
Kalman filter needs tuning to operate properly. The tuning of the filter was done by trial
and error. The estimated filter data was then fed onto the PID control algorithm.
For the selfbalancing robot project, stability was successfully achieved by
implementing the PID controller. The controller works by subtracting the desired set
point from the Kalman estimated tilt angle. The subtraction generates an error, which was
then fed onto the PID control algorithm. The control algorithm processed the error to
68
generate a pulse width modulation drive signal. The signal drive is used to drive the DC
motor accordingly. Before the PID controller can provide system stability, it needs to be
tuned. There many ways to tune the PID controller, to get the desired output response.
One of the tuning methods that was used on this project was the ZieglerNichols tuning
algorithm. The ZeiglerNichols tuning method makes it very simple to properly tune the
PID controller. The tuning method also requires very little process knowledge. The other
method for tuning the PID controller was by trial and error. Tuning the controller by trial
and error is not the best choice, but it provided the desired output response.
Conclusion
Over the past few years, selfbalancing robots have become a popular topic of
research. Most of the research and development involves control algorithms and system
dynamics. Advanced controllers provide robust and optimal control for self balancing
robots. The two wheeled selfbalancing robots are excellent examples of the classical
inverted pendulum problem. In recent years microcontrollers have become the popular
choice to implement control theory at lower costs. Making microcontrollers ideal cheap
components to use on self balancing platforms.
One of the goals for the project was to implement a closed loop control system
using low cost components. Using lost cost components in this type of project can
sometimes lead to drawbacks, in this case from both the accelerometer and the
gyroscope. Both sensors experience initial problems that made them difficult to use in
this type of project. The accelerometer data was highly corrupted with unwanted noise.
The gyroscope suffered from a problem that made its bias value drift away with time. A
69
solution to the problems associated with these sensors outputs can be found in the use of
a Kalman filter. This filter is used to minimize and totally eliminate the unwanted output
results from the sensors. The PID controller relies on the input signal to be almost free of
noise.
The project demonstrated that a fully functional self balancing robot can be
built with off the shelf components. Being based on low cost components, the project is
an ideal demonstration of control theory for a classroom setting. The project can also
serve as a test platform to write the necessary software control algorithms to implement
for commercial products.
Recommendations
This masters project provides the basis for future research on Kalman filter
applications and implementations. Also to research more advanced control systems. As
stated by Rich Ooi, more research should be conducted to exploit the Kalman filter
technology and its application for other projects that require sensor fusion technology
[7, pg. 54]. Such sensor fusion technology can be used in personal navigation systems.
The tuning of the Kalman filter needs to be studied more in depth. Tuning the Kalman
filter by trial and error is time consuming. With more research a better method to tune the
Kalman filter covariance matrices Q
w
and R
v
may be discovered.
The linear PID control system developed in this project was able to establish
that the robot was able to balance under some minimal disturbances. The robustness of
the system is not fully tested and more testing is needed to assess the robustness of the
system. Further fine tuning of the PID control algorithm is required for to achieve better
70
output performance. Further research can be conducted to implement intelligent
controllers on balancing robot system. Such intelligent controllers might be a fuzzy PID
controller or a self tuning PID controller.
Another future improvement to the selfbalancing two wheel robot might be to
add encoders. The encoders may be placed along side the motors to provide motor
rotation information. Having the motor rotation information from the encoders, the robot
will have better locomotion control. Improved locomotion control will allow the robot to
move precise distances.
REFERENCES
72
REFERENCES
[1] D. Ibrahim, D Microcontroller Based Applied Digital Control. West Sussex, England:
J ohn Wiley & Sons Ltd, 2006.
[2] R. Hollis, BallBots, Scientific American, October 2006. Retrieved February 4, 2009
from the World Wide Web: http://www.sciam.com/article.cfm?id=ballbots
[3] T. Atwood, VECNAs Battlefield ExtractionAssist Robot, Robot Magazine, April
25, 2007. Retrieved February 6, 2009 from the World Wide Web:
http://www.botmag.com/articles/042507_vecna_bear.shtml
[4] G. Welch and G. Bishop, Kalman Filter. An Introduction to the Kalman Filter,
2007. Retrieved February 16, 2009 from the World Wide Web:
http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
[5] D. Simon, Kalman Filtering. Embedded Systems Programming, 2001. Retrieved
J anuary 18, 2009 from the World Wide Web:
http://www.embedded.com/9900168?_requestid=8285
[6] V.J . VanDoren, PID: Still the One, Control Engineering, October 2003. Retrieved
J anuary 19, 2009 from the World Wide Web:
http://www.controleng.com/article/CA325983.html
[7] R.C Ooi, Balancing a TwoWheeled Autonomous Robot, 2003. Retrieved J anuary
18, 2009 from the World Wide Web:http://robotics.ee.uwa.edu.au/theses/2003Balance
Ooi.pdf>.
APPENDIX A
74
APPLICATION SOURCE CODE
main_pid.c
/ / ====================================================================
/ *
Pr ogr ammer : J ose L. Cor ona
Pr oj ect Name: PI D Test Sof t war e f or PI C32
Dat e: 2/ 19/ 09
MCU: PI C32MX360F512L
Compl i er : MPLAB C32
*/
/ / ====================================================================
/ / #i ncl ude <pr oc/ p32mx360f 512l . h>
#i ncl ude <p32xxxx. h>
#i ncl ude <mat h. h>
#i ncl ude <st di o. h>
#i ncl ude <st dl i b. h>
#i ncl ude <pl i b. h>
#i ncl ude " conf i g_adc. c"
#i ncl ude " conf i g_pwm. c"
#i ncl ude " conf i g_t i mer _1. c"
#i ncl ude " conf i g_t i mer _2. c"
#i ncl ude " kal man. c"
#i ncl ude " USART_32. c"
#i ncl ude " pi d. c"
#i ncl ude " accel . c"
#i ncl ude " gyr o. c"
#i ncl ude " kni ght _r i der . c"
/ /                         Conf i g. set t i ngs                         
/ / POSCMOD = HS, FNOSC = PRI PLL, FWDTEN = OFF
/ / PLLI DI V = DI V_2, PLLMUL = MUL_20
/ / PBDI V = 8 ( def aul t )
/ / Mai n cl ock = ( 8MHz/ 2) * 20/ 1 = 80MHz
/ / Per i pher al cl ock = 80MHz / 8 = 10MHz
/ / Conf i gur at i on Bi t set t i ngs
/ / SYSCLK = 80 MHz >( 8MHz Cr yst al / FPLLI DI V * FPLLMUL / FPLLODI V)
/ / PBCLK = 40 MHz >SYSCLK/ PBDI V
/ / Pr i mar y Osc w/ PLL ( XT+, HS+, EC+PLL)
/ / WDT OFF
/ / Ot her opt i ons ar e don' t car e
/ *************************  Descr i pt i on ****************************
*
* Osci l l at or Conf i gur at i on Bi t Set t i ngs:
*  Osci l l at or Sel ect i on Bi t s = Pr i mar y Osc w/ PLL ( XT+HS+EC+PLL)
*  Pr i mar y Osci l l at or Conf i g = HS osc mode
*  PLL I nput Di vi der = 2x Di vi der
*  PLL Mul t i pl i er = 20x Mul t i pl i er
*
75
* Not es:
*  Set PBCLK di vi sor = 1: 8 t o achi eve a 10 MHz PBCLK.
*  To cal . baudr at e ( SYS_FREQ/ PB_DI V) / ( 16*baudr at e_2)  1
********************************************************************/
#pr agma conf i g FPLLMUL = MUL_20, FPLLI DI V = DI V_2, FPLLODI V = DI V_1, FWDTEN =
OFF
#pr agma conf i g POSCMOD = HS, FNOSC = PRI PLL, FPBDI V = DI V_8
/ /     Let compi l e t i me pr e pr ocessor cal cul at e t he PR1 ( per i od)    
#def i ne SYS_FREQ 80000000L
#def i ne PB_DI V 8
#def i ne PRESCALE 256
#def i ne SAMPLES_PER_SEC 20
#def i ne T1_TI CK_RATE ( SYS_FREQ/ PB_DI V/ PRESCALE/ SAMPLES_PER_SEC)
/ /     Kal man Fi l t er Par amet er s    
/ *
#def i ne R_mat r i x 0. 690
#def i ne Q_Gyr o_mat r i x 0. 002
#def i ne Q_Accel _mat r i x 0. 001
*/
#def i ne R_mat r i x 0. 590
#def i ne Q_Gyr o_mat r i x 0. 002
#def i ne Q_Accel _mat r i x 0. 001
/ / LED' s
#def i ne LED_1 PORTAbi t s. RA0
#def i ne LED_2 PORTAbi t s. RA1
#def i ne LED_3 PORTAbi t s. RA2
#def i ne LED_4 PORTAbi t s. RA3
#def i ne LED_5 PORTAbi t s. RA4
#def i ne LED_6 PORTAbi t s. RA5
#def i ne LED_7 PORTAbi t s. RA6
#def i ne LED_8 PORTAbi t s. RA7
/ /     Const ant s    
#def i ne pi 3. 1415926535898
#def i ne degr ees ( 180. 0/ pi )
/ / Bool ean val ues used on Ti mer 1 I nt er r upt
/ / #def i ne FALSE 0
/ / #def i ne TRUE 1
/ /     Var i abl es Decl ar at i ons    
char *message_t est = " \ r \ n     >>> USART 2 Up and WORKI NG <<<     \ r \ n" ;
char *car r _r et ur n= " \ r " ;
/ / buf f er of f set t o poi nt t o t he base of t he i dl e buf f er
unsi gned i nt of f set = 0;
/ /     Var i abl e Decl ar at i ons    
doubl e angl e_est = 0. 00;
/ / doubl e gyr o_r at e= 0. 0;
doubl e accel _x, r at e_y= 0. 00;
i nt adc_accel = 0;
i nt adc_gyr o= 0;
doubl e dr i ve_pwm= 0. 00;
doubl e r ef er ence= 0. 0010;
76
doubl e syst em_er r or = 0. 00;
doubl e t i mer _count er = 0. 00;
/ / unsi gned char f l ag;
/ /     Kal man St at e St r uct ur es    
kal man f i l t er _pi t ch;
/ /     PI D St r uct ur es    
pi d pi d_access;
voi d dr i ve_syst em( doubl e syst em_i nput ) ;
/ *********************************************************************
P, I and D par amet er val ues
The K_P, K_I and K_D val ues ( P, I and D gai ns)
need t o be modi f i ed t o adapt t o t he appl i cat i on at hand
*********************************************************************/
#def i ne T_S 1. 0/ ( SAMPLES_PER_SEC)
/ / PI D Gai ns at 50ms sampl e r at e
doubl e K_P= 1800. 001;
doubl e K_I = 0. 10250;
doubl e K_D= 95. 7501;
i nt mai n( )
{
/ / Conf i gur e t he devi ce f or maxi mumper f or mance but do not change t he PBDI V
/ / Gi ven t he opt i ons, t hi s f unct i on wi l l change t he f l ash wai t st at es, RAM
/ / wai t st at e and enabl e pr ef et ch cache but wi l l not change t he PBDI V.
/ / The PBDI V val ue i s al r eady set vi a t he pr agma FPBDI V opt i on above.
SYSTEMConf i g( SYS_FREQ, SYS_CFG_WAI T_STATES  SYS_CFG_PCACHE) ;
/ / Set PORTA t o al l out put s
TRI SA= 0x0000;
/ / Cl ear PORTA
PORTA= 0x0000;
/ / Used on Mot or Cont r ol pi ns
TRI SG= 0x0000;
PORTG= 0x0000;
/ / Used on PWM pi ns on Mot or Dr i ver
TRI SD= 0x0000;
/ /    Di sabl e J TAG por t   
DDPCONbi t s. J TAGEN = 0;
/ /    Di sabl e Tr ace por t   
DDPCONbi t s. TROEN = 0;
/ /    I ni t i l i ze On boar d Devi ces   
set up_adc( ) ;
set up_pwm( ) ;
set up_t i mer _1( ) ;
set up_t i mer _2( ) ;
set up_usar t ( USART_1, 65) ; / / ( 32) 19200 bps @PBCLK = 10 MHz
set up_usar t ( USART_2, 65) ; / / ( 65) 9600 bps @PBCLK = 10 MHz
77
/ / ( 10) 57600 bps @PBCLK = 10 MHz
/ /    Set up Ti mer 1 I nt er r upt s   
I EC0bi t s. T1I E = 0; / / Di sabl e Ti mer 1 i nt er r upt s
I FS0bi t s. T1I F = 0; / / Cl ear t he Ti mer 1 i nt er r upt f l ag
I PC1bi t s. T1I P = 7; / / Set Ti mer 1 i nt er r upt t o pr i or i t y l evel 7 ( Hi ghest )
I PC1bi t s. T1I S = 0; / / Set Ti mer 1 i nt er r upt t o sub pr i or i t y l evel 0
( Lowest )
PR1 = T1_TI CK_RATE; / / Load Per i od ( Ti mer 1) Regi st er t o pr oduce 0. 050 sec.
/ /    I ni t i l i ze PWM Modul e   
PR2 = 499; / / Load Per i od ( Ti mer 2) Regi st er wi t h Fr eq = 20 kHz f or
PWM.
OC1R = 250; / / I ni t i al i ze Pr i mar y Compar e Resi gt er
OC1RS = 250; / / I ni t i al i ze Secondar y Compar e Resi gt er ( PWM Dut y
Cycl e)
T2CONbi t s. ON = 1; / / Enabl e Ti mer 2
OC1CONbi t s. ON = 1; / / Enabl e Out put Compar e Modul e
/ ****************************************************************************
I ni t i al i ze t he Kal man Fi l t er f or pi t ch st at e. The measur ement noi se
covar i ance ( R_angl e) i s t he noi se i n t he accel er omet er measur ement s.
The pr ocess noi se covar i ance ar e ( Q_angl e, Q_gyr o) . The pr oper set t i ngs f or
t hese val ues needs t o be expl or ed f ur t her , but Q_angl e i s how much we
t r ust t he accel er omet er and Q_gyr o i s how much we t r ust t he gyr o.
( R_angl e)  can be cal cul at ed f r omof f  l i ne sampl es of mesur ement s.
( Q_angl e, Q_gyr o)  Ar e t uned by t r i al and er r or .
****************************************************************************/
/ / Par amet er s ( R_angl e, Q_gyr o, Q_angl e )
kal man_i ni t ( &f i l t er _pi t ch, T_S, R_mat r i x, Q_Gyr o_mat r i x, Q_Accel _mat r i x) ;
/ /         St ar t PI D        
pi d_set up( K_P, K_I , K_D, &pi d_access) ;
/ /     Kni ght Ri der LED Ef f ect    
kni ght _r i der ( ) ;
/ /     Tur n ON t he ADC modul e    
AD1CON1bi t s. ON = 1; / / ADC On bi t ( 1: ADC Enabl e / 0: ADC Di sabl e)
bi t <15>
AD1CON1bi t s. ASAM = 1; / / ADC Sampl e Aut o St ar t bi t
/ / ( 1: ADC aut o sampl i ng / 0: ADC manual sampl i ng)
bi t <2>
/ /     Enabl e Ti mer I nt er r upt s    
T1CONbi t s. ON = 1; / / Enabl e Ti mer 1
I EC0bi t s. T1I E = 1; / / Enabl e Ti mer 1 i nt er r upt s
/ / TX_st r i ng( USART_2, message_t est ) ;
/ /    Enabl e Mul t i  vect or I nt er r upt s   
I NTEnabl eSyst emMul t i Vect or edI nt ( ) ;
/ /    St ar t Kal man Est i mat es   
/ / St ar t t he pi t ch angl e est i mat e
whi l e( ! I FS1bi t s. AD1I F) ; / / Pol l f or ADC i nt er r upt
I FS1bi t s. AD1I F = 0; / / Cl ear ADC i nt er r upt st at us f l ag
/ / ( 1: ADC i nt er r upt occur r ed / 0: NO i nt er r upt )
78
/ / Det er mi ne whi ch buf f er i s i dl e and cr eat e an of f set
of f set = 8 * ( ( ~AD1CON2bi t s. BUFS & 0x01) ) ;
/ / Read t he r esul t of AN0( accel . ) conver si on f r omt he i dl e buf f er
adc_accel = ( ReadADC10( of f set ) ) ;
angl e_est = r ead_accel ( adc_accel , ' r ' ) ;
/ / Loop f or ever
whi l e( 1)
{
whi l e( ! I FS1bi t s. AD1I F) ;
I FS1bi t s. AD1I F = 0; / / Cl ear ADC i nt er r upt st at us f l ag
}/ / End of whi l e( )
r et ur n 0;
}
/ ********************************************************************
* Funct i on Name: dr i ve_syst em *
* Ret ur n Val ue: None *
* Par amet er s: syst emi nput *
* Descr i pt i on: Sends t he PI D cont r ol l er out put t o t he pr ocess *
* i nput . The si gnal i s PWM. *
* *
* ________________ ___________ *
* er r or   PWM   Yout *
*        PI D Cont r ol l er           > DC Mot or s           > *
*  ________________  ___________ *
* *
********************************************************************/
voi d dr i ve_syst em( doubl e syst em_i nput )
{
si gned shor t i nt pr ocess_i n;
pr ocess_i n= ( si gned shor t i nt ) syst em_i nput ;
i f ( pr ocess_i n==0)
{
PORTG= 0x0000;
LED_6= 0;
LED_7= 0;
OC1RS= 0;
}
el se i f ( pr ocess_i n <  0)
{
LED_7= 0;
LED_6= 1;
/ /    Go For war d   
/ / PWM 1 Dut y Cycl e ( RD0)
PORTG=0x0009;
}
el se i f ( pr ocess_i n > 0)
{
LED_6= 0;
LED_7= 1;
79
/ /    Go Backwar d   
/ / PWM 1 Dut y Cycl e ( RD0)
PORTG=0x0006;
}
/ / Send PI D Si gnal t o DC Mot or s
OC1RS= abs( pr ocess_i n) ;
}
/ ******************************************************
Ti mer 1 I nt er r upt Handl er
Not e: Ti mer 1 i nt er r upt ever y 50ms.
*******************************************************/
voi d __I SR( _TI MER_1_VECTOR, i pl 7) Ti mer 1Handl er ( voi d)
{
/ / Ti mer 1 i nt er r upt f l ag set
i f ( I FS0bi t s. T1I F == 1)
{
mPORTAToggl eBi t s( BI T_0) ;
OC1RS=0;
t i mer _count er = t i mer _count er + T_S;
/ /     Get ADC Dat a f r omBuf f er s    
/ / Det er mi ne whi ch buf f er i s i dl e and cr eat e an of f set
of f set = 8 * ( ( ~AD1CON2bi t s. BUFS & 0x01) ) ;
/ / Read t he r esul t of AN0( accel . ) conver si on f r omt he i dl e buf f er
adc_accel = ReadADC10( of f set ) ;
/ / Read t he r esul t of AN1( gyr o. ) conver si on f r omt he i dl e buf f er
adc_gyr o= ReadADC10( of f set + 1) ;
/ /         Read Gyr oscope ADC        
r at e_y= r ead_gyr o( adc_gyr o, ' r ' ) ;
/ /       Read Accel er omet er ADC      
accel _x= r ead_accel ( adc_accel , ' r ' ) ;
/ /             Kal man Fi l t er Pi t ch Est i mat e               
/ **********************************************************
Pass t he measur ed pi t ch val ues ( accel . ) and r at es ( gyr o. )
t hr ough t he Kal man f i l t er t o det er mi ne t he est i mat ed pi t ch
and unbi as gyr o. val ues i n r adi ans.
**********************************************************/
/ /     Execut e Kal man Fi l t er Al gor i t hm   
kal man_pr edi ct ( &f i l t er _pi t ch, r at e_y) ; / / Kal man Pr edi ct St at e
kal man_updat e( &f i l t er _pi t ch, accel _x) ; / / Kal man Updat e St at e
/ /       Get Pr ocess Measur ement      
angl e_est = kal man_get _angl e( &f i l t er _pi t ch) ; / / Kal man Est i mat e Angl e Resul t
/ /       Cal cul at e syst emer r or      
syst em_er r or = r ef er ence  angl e_est ;
/ /         Execut e PI D Cont r ol l er        
dr i ve_pwm= pi d_cont r ol l er ( &pi d_access, syst em_er r or , angl e_est ) ;
/ /              Dr i ve Mot or s             
80
dr i ve_syst em( dr i ve_pwm) ;
/ /     Save Kal man Dat a on Mi r co SD Car d    
f l oat _t o_t ext ( USART_1, t i mer _count er , 3) ;
TX_st r i ng( USART_1, " " ) ;
f l oat _t o_t ext ( USART_1, accel _x, 3) ;
TX_st r i ng( USART_1, " " ) ;
f l oat _t o_t ext ( USART_1, angl e_est , 3) ;
TX_st r i ng( USART_1, " " ) ;
f l oat _t o_t ext ( USART_1, syst em_er r or , 3) ;
TX_st r i ng( USART_1, " " ) ;
f l oat _t o_t ext ( USART_1, dr i ve_pwm, 3) ;
TX_st r i ng( USART_1, car r _r et ur n) ; */
}
/ / Cl ear t he Ti mer 1 i nt er r upt f l ag
I FS0bi t s. T1I F = 0;
}/ / End of I F
}/ / End of Ti mer 1 I nt er r upt Handl er
config_adc.c
/ / =============================================================================
/ *
Pr ogr ammer : J ose L. Cor ona
Pr oj ect Name: ADC Set up Sof t war e f or PI C32
Fi l e: conf i g_adc. c
Dat e: 1/ 19/ 09
MCU: PI C32MX360F512L
Compl i er : MPLAB C32
*/
/ / =============================================================================
voi d set up_adc( voi d) ;
/ ********************************************************************
* Funct i on Name: Set up ADC *
* Ret ur n Val ue: NONE *
* Par amet er s: NONE *
* Descr i pt i on: Thi s r out i ne set up t he ADC modul e f or RB0( AN0) . *
* *
********************************************************************/
voi d set up_adc( )
{
/ *     Set up ADC Modul e      */
/ / Make Al l PORTB di gi t al , but RB0( AN0) / RB1( AN1) = Anal og Mode
AD1PCFGbi t s. PCFG0 = 0;
AD1PCFGbi t s. PCFG1 = 0;
81
/ / Connect RB0( AN0) t o MUX A ( CH0 as posi t i ve i nput i s AN0)
AD1CHSbi t s. CH0SA=0x0;
/ / Connect RB1( AN1) t o MUX B ( CH0 as posi t i ve i nput i s AN1)
AD1CHSbi t s. CH0SB=0x1;
/ / NEG i nput sel ect f or MUX A ( 1: NEG i nput i s AN1/ 0: NEG i nput i s VR )
AD1CHSbi t s. CH0NA=0x0;
/ / NEG i nput sel ect f or MUX B ( 1: NEG i nput i s AN1/ 0: NEG i nput i s VR )
AD1CHSbi t s. CH0NB=0x0;
/ / ADC On bi t ( 1: ADC Enabl e / 0: ADC Di sabl e) bi t <15>
AD1CON1bi t s. ON = 0;
/ / Fr eeze i n Debuge mode bi t ( 1: Fr eeze op / 0: Cont i nue op) bi t <14>
AD1CON1bi t s. FRZ = 0;
/ / St op i n I dl e mode bi t ( 1: St op ADC op / 0: Cont i nue ADC op) bi t <13>
AD1CON1bi t s. SI DL = 0;
/ / ADC Conver i son Tr i gger Sour ce set t o aut o conver t bi t s<10: 8>
AD1CON1bi t s. SSRC = 7;
/ / ADC r esul t f or mat as i nt eger ( 16 bi t ) bi t s<7: 5>
AD1CON1bi t s. FORM = 0;
AD1CON1bi t s. CLRASAM = 0;
/ / Vol t age Ref . Conf i g. bi t s set t o Vr +( Vdd) & Vr  ( Vss) bi t s<15: 13>
AD1CON2bi t s. VCFG = 0;
/ / Scan i nput sel ect i ons f or MUX A ( 1: Scan i nput s / 0: Di sabl e Scan i nput s)
bi t <10>
AD1CON2bi t s. CSCNA = 0;
/ / Sampl e/ Conver t Seq. Per I nt er r upt sel ect i on bi t s<5: 2>
AD1CON2bi t s. SMPI = 1;
/ / ADC r esul t buf f er mode ( 1: t wo 8 wor d buf f er s / 0: one 16 wor d buf f er )
bi t <1>
AD1CON2bi t s. BUFM = 1;
/ / Al t er nat e I nput Sampl e Mode( 1: ALT. bet ween MUX A/ B / 0: Di sabl e ALT. )
bi t <0>
AD1CON2bi t s. ALTS = 1;
AD1CON2bi t s. OFFCAL = 0;
/ / ADC conver si on CLK sour ce ( 1: I nt er nal RC CLK / 0: PBCLK) bi t <15>
AD1CON3bi t s. ADRC = 0;
/ / Aut o Sampl e Ti me bi t s<12: 8> ( 31 Tad)
AD1CON3bi t s. SAMC = 16;
/ / ADC conv. CLK sel ect bi t s<7: 0> Tad=Tpb( ADCS+1) *2 ( Tad=1us)
( Tpb=( 80/ 8) =10MHz)
AD1CON3bi t s. ADCS = 2;
/ / Scan Al l ( AN0 AN15) Anal og i nput s
AD1CSSL= 0xFFFF;
}/ / End of ADC set up.
config_pwm.c
/ / =============================================================================
/ *
Pr ogr ammer : J ose L. Cor ona
Pr oj ect Name: PWM Set up Sof t war e f or PI C32
Dat e: 1/ 14/ 09
MCU: PI C32MX360F512L
Compl i er : MPLAB C32
82
*/
/ / =============================================================================
voi d set up_pwm( voi d) ;
/ ********************************************************************
* Funct i on Name: Set up PWM *
* Ret ur n Val ue: NONE *
* Par amet er s: NONE *
* Descr i pt i on: Thi s r out i ne set up t he PWM modul es 1 & 2 f or *
* DC Mot or s. *
********************************************************************/
voi d set up_pwm( )
{
/ *    Set up out put compar e 1 modul e t o PWM     */
/ / Out put Compar e Per i pher al On bi t ( 1: OC modul e ON/ 0: OC modul e OFF) bi t <15>
OC1CONbi t s. ON = 0;
/ / Fr eeze i n Debuge mode bi t ( 1: Fr eeze op/ 0: Cont i nue op) bi t <14>
OC1CONbi t s. FRZ = 0;
/ / St op i n I dl e mode bi t ( 1: St op USART op/ 0: Cont i nue op) bi t <13>
OC1CONbi t s. SI DL = 0;
/ / 32 bi t Compar e Mode bi t ( 1: 32 bi t t i mer / 0: 16 bi t t i mer ) bi t <5>
OC1CONbi t s. OC32 = 0;
/ / PWM Faul t Condi t i on St at us bi t ( 1: PWM f aul t cond. / 0: NO PWM f aul t cond. )
bi t <4>
OC1CONbi t s. OCFLT = 0;
/ / Out put Compar e Ti mer Sel . bi t ( 1: Ti mer 3 i s CLK sr c. / 0: Ti mer 2 i s CLK sr c. )
bi t <3>
OC1CONbi t s. OCTSEL = 0;
/ / Out put Comapr e Mode Sel ect bi t ( 110: PWM mode on OCx, Faul t pi n di sabl e)
bi t s<2: 0>
OC1CONbi t s. OCM = 6;
}/ / End of PWM set up.
config_timer_1.c
/ / =============================================================================
/ *
Pr ogr ammer : J ose L. Cor ona
Pr oj ect Name: Ti mer 1 Set up Sof t war e f or PI C32
Dat e: 1/ 15/ 09
MCU: PI C32MX360F512L
Compl i er : MPLAB C32
*/
/ / =============================================================================
voi d set up_t i mer _1( voi d) ;
/ ********************************************************************
* Funct i on Name: Set up PWM *
* Ret ur n Val ue: NONE *
* Par amet er s: NONE *
* Descr i pt i on: Thi s r out i ne set ups up t he Ti mer 1. *
* *
********************************************************************/
83
voi d set up_t i mer _1( )
{
/ *     Set up out put Ti mer 1 modul e      */
/ / Ti mer On bi t ( 1: Ti mer 2 Enabl e / 0: Ti mer 2 Di sabl e) bi t <15>
T1CONbi t s. ON = 0;
/ / Fr eeze i n Debuge mode bi t ( 1: Fr eeze op/ 0: Cont i nue op) bi t <14>
T1CONbi t s. FRZ = 0;
/ / St op i n I dl e mode bi t ( 1: St op USART op/ 0: Cont i nue op) bi t <13>
T1CONbi t s. SI DL = 0;
/ / Asynchr onous Ti mer Wr i t e Di sabl e bi t <12>
T1CONbi t s. TWDI S = 0;
/ / Asynchr onous Ti mer Wr i t e i n Pr ogr ess bi t <11>
T1CONbi t s. TWI P = 0;
/ / Gat e Ti me Accul . En. bi t ( 1: Gat e Ti me Acc. En. / 0: Gat e Ti me Acc. Di s. )
bi t <7>
T1CONbi t s. TGATE = 0;
/ / Ti mer I nput Cl ock Pr escal er Sel ect bi t s ( 00: 1: 1 pr escal er val ue)
bi t s<5: 4>
T1CONbi t s. TCKPS = 3;
/ / Ti mer Ext er nal Cl ock I nput Synchr oni zat i on Sel ect i on bi t <2>
T1CONbi t s. TSYNC = 0;
/ / Ti mer Cl ock Sour ce Sel ect bi t ( 1: Ext er nal CLK/ 0: I nt er nal CLK) bi t <1>
T1CONbi t s. TCS = 0;
/ / Cl ear Ti mer 2 Regi st er
TMR1 = 0;
}/ / End of Ti mer 1 set up.
config_timer_2.c
/ / =============================================================================
/ *
Pr ogr ammer : J ose L. Cor ona
Pr oj ect Name: Set up Ti mer 2 Sof t war e f or PI C32
Dat e: 2/ 19/ 09
MCU: PI C32MX360F512L
Compl i er : MPLAB C32
*/
/ / =============================================================================
voi d set up_t i mer _2( voi d) ;
/ ********************************************************************
* Funct i on Name: Set up Ti mer 2 *
* Ret ur n Val ue: NONE *
* Par amet er s: NONE *
* Descr i pt i on: Thi s r out i ne set up Ti mer 2, used f or PWM *
* modul es f or DC Mot or s. *
84
********************************************************************/
voi d set up_t i mer _2( )
{
/ *     Set up out put Ti mer 2 modul e      */
/ / Ti mer On bi t ( 1: Ti mer 2 Enabl e / 0: Ti mer 2 Di sabl e) bi t <15>
T2CONbi t s. ON = 0;
/ / Fr eeze i n Debuge mode bi t ( 1: Fr eeze op/ 0: Cont i nue op) bi t <14>
T2CONbi t s. FRZ = 0;
/ / St op i n I dl e mode bi t ( 1: St op USART op/ 0: Cont i nue op) bi t <13>
T2CONbi t s. SI DL = 0;
/ / Gat e Ti me Accl . En. bi t ( 1: Gat e Ti me Acc. En. / 0: Gat e Ti me Acc. Di s. )
bi t <7>
T2CONbi t s. TGATE = 0;
/ / Ti mer I nput Cl ock Pr escal er Sel ect bi t s ( 000: 1: 1 pr escal er val ue)
bi t s<6: 4>
T2CONbi t s. TCKPS = 0;
/ / 32 bi t Ti mer Mode Se. bi t s ( 1: 32 bi t t i mer / 0: 16 bi t t i mer ) bi t <3>
T2CONbi t s. T32 = 0;
/ / Ti mer Cl ock Sour ce Sel ect bi t ( 1: Ext er nal CLK/ 0: I nt er nal CLK) bi t <1>
T2CONbi t s. TCS = 0;
/ / Cl ear Ti mer 2 Regi st er
TMR2 = 0x0000;
}/ / End of Ti mer 2 set up.
USART_32.h
/ / =============================================================================
/ *
Pr ogr ammer : J ose L. Cor ona
Pr oj ect Name: PI C32 USART set up code
Dat e: 2/ 20/ 09
MCU: PI C32MX360F512L
Compl i er : MPLAB C32
*/
/ / =============================================================================
/ /                                                                             
#i f ndef _USART_32_H
#def i ne _USART_32_H
/ / USART Sel ect i on
#def i ne USART_1 1 / / Used For Logomat i c Dat a Logger
#def i ne USART_2 2 / / Used as a gener al pur pose USART
/ / MAX number of char s f or f l oat mant i ssa
#def i ne MAX_MANTI SA 6
/ / Number of char s
#def i ne MAX_STRI NG 10
/ / PI C32 USART f xn' s
voi d TX_usar t ( char t x_sel , char dat a) ;
voi d TX_st r i ng( char t x_st r i ng_sel , char *dat a) ;
char RX_usar t ( char r x_sel ) ;
doubl e st r i ng_RX( char r x_st r i ng_sel ) ;
85
voi d ser i al _di gi t ( char di gi t _sel , shor t i nt dat a) ;
voi d set up_usar t ( char usar t _sel , i nt baud_r at e) ;
voi d enabl e_PLL( voi d) ;
voi d di sabl e_PLL( voi d) ;
voi d f l oat _t o_t ext ( char t x_st r i ng_sel , doubl e number , char pr eci si on) ;
i nt f t oa( doubl e x, char *st r , char pr ec, char f or mat ) ;
char r x_buf f er [ 15] ;
char x=0;
#endi f
USART_32.c
/ / =============================================================================
/ *
Pr ogr ammer : J ose L. Cor ona
Pr oj ect Name: PI C32 USART set up code
Dat e: 1/ 06/ 09
MCU: PI C32MX360F512L
Compl i er : MPLAB C32
*/
/ / =============================================================================
/ /                                                                             
#i ncl ude " USART_32. h"
/ / =======>> The f ol l owi ng Fxn' s ar e used by t he PI C32 USART <<=======
/ ********************************************************************
* Funct i on Name: set up_usar t *
* Ret ur n Val ue: none *
* Par amet er s: none *
* Descr i pt i on: Thi s r out i ne conf i gur es t he USART *
* 1. USART i s set as 8N1 *
* 2. Asynchr onous Mode *
* 3. Bot h RX ant TX i nt er r upt di sabl e *
* 4. RX set f or Cont i nuous Recei ve *
* 5. Hi gh Baud Rat e set *
********************************************************************/
/ / set up_usar t ( 25) ;
voi d set up_usar t ( char usar t _sel , i nt baud_r at e) / / <<====
{
/ / Conf i gur e USART 2
i f ( usar t _sel == 2)
{
U2BRG = baud_r at e;
/ *      USARTx Mode Resi gt er       */
/ / USARTx Enabl e bi t ( 1: USART Enabl e/ 0: USART Di sabl e) bi t <15>
U2MODEbi t s. ON = 1;
/ / Fr eeze i n Debuge mode bi t ( 1: Fr eeze op/ 0: Cont i nue op) bi t <14>
U2MODEbi t s. FRZ = 0;
/ / St op i n I dl e mode bi t ( 1: St op USART op/ 0: Cont i nue op) bi t <13>
86
U2MODEbi t s. SI DL = 0;
/ / I r DA Encoder and Decoder bi t ( 1: Enabl e/ 0: Di sabl e) bi t <12>
U2MODEbi t s. I REN = 0;
/ / Mode sel ect f or UxRTS pi n bi t ( 1: Si mpl ex mode/ 0: Fl ow cont r ol mode) bi t <11>
U2MODEbi t s. RTSMD = 0;
/ / USARTx Enabl e bi t s<9: 8>
U2MODEbi t s. UEN = 0;
/ / Enabl e Wake up on st ar t bi t det ect ( 1: Wake up enabl e/ 0: Wake up di sabl e)
bi t <7>
U2MODEbi t s. WAKE = 0;
/ / USARTx Loopback mode sel . bi t <6>
U2MODEbi t s. LPBACK = 0;
/ / Aut o Baud enabl e bi t <5>
U2MODEbi t s. ABAUD = 0;
/ / RX pol ar i t y i nver si on bi t ( 1: UxRX i dl e st at e ' 0' / 0: UxRX i dl e st at e ' 1' )
bi t <4>
U2MODEbi t s. RXI NV = 0;
/ / Hi gh Baud r at e enabl e bi t ( 1: Hi gh spd. mode 4x/ 0: St d. spd. mode 16x)
bi t <3>
U2MODEbi t s. BRGH = 0;
/ / Par i t y and Dat a sel ect i on bi t s <2: 1>
U2MODEbi t s. PDSEL = 0;
/ / St op sel ect i on bi t ( 1: 2 st op bi t s / 0: 1 st op bi t s) bi t <0>
U2MODEbi t s. STSEL = 0;
/ *                                  */
/ *     USARTx St at us Resi gt er      */
/ / Aut omat i c addr ess det ect mode enabl e bi t ( 1: Enabl e/ 0: Di sabl e) bi t <24>
U2STAbi t s. ADM_EN = 0;
/ / Aut omat i c addr ess mask bi t s<23: 16>
U2STAbi t s. ADDR = 0;
/ / TX I nt er r upt mode sel ect i on bi t s<15: 14>
U2STAbi t s. UTXI SEL = 2;
/ / TX pol ar i t y i nv. bi t ( 1: UxTX i dl e st at e ' 0' / 0: UxTX i dl e st at e ' 1' )
bi t <13>
U2STAbi t s. UTXI NV = 0;
/ / Recei ver enabl e bi t ( 1: RX enabl e/ 0: RX di sabl e) bi t <12>
U2STAbi t s. URXEN = 1;
/ / Tr ansmi t br eak ( 1: Send Br eak / 0: Don' t send br eak) bi t <11>
U2STAbi t s. UTXBRK = 0;
/ / Tr ansmi t enabl e bi t ( 1: TX enabl e / 0: TX di sabl e) bi t <10>
U2STAbi t s. UTXEN = 1;
/ / RX i nt er r upt mode sel ect i on bi t s<7: 6>
U2STAbi t s. URXI SEL = 0;
/ / Addr ess char act er det ect bi t ( 1: Enabl e / 0: Di sabl e) bi t <5>
U2STAbi t s. ADDEN = 0;
/ *                                  */
}
el se / / Conf i gur e USART 1
{
U1BRG = baud_r at e;
/ *      USARTx Mode Resi gt er       */
/ / USARTx Enabl e bi t ( 1: USART Enabl e/ 0: USART Di sabl e) bi t <15>
U1MODEbi t s. ON = 1;
/ / Fr eeze i n Debuge mode bi t ( 1: Fr eeze op/ 0: Cont i nue op) bi t <14>
U1MODEbi t s. FRZ = 0;
/ / St op i n I dl e mode bi t ( 1: St op USART op/ 0: Cont i nue op) bi t <13>
U1MODEbi t s. SI DL = 0;
87
/ / I r DA Encoder and Decoder bi t ( 1: Enabl e/ 0: Di sabl e) bi t <12>
U1MODEbi t s. I REN = 0;
/ / Mode sel ect f or UxRTS pi n bi t ( 1: Si mpl ex mode/ 0: Fl ow cont r ol mode)
bi t <11>
U1MODEbi t s. RTSMD = 0;
/ / USARTx Enabl e bi t s<9: 8>
U1MODEbi t s. UEN = 0;
/ / Enabl e Wake up on st ar t bi t det ect ( 1: Wake up en. / 0: Wake up di s. )
bi t <7>
U1MODEbi t s. WAKE = 0;
/ / USARTx Loopback mode sel . bi t bi t <6>
U1MODEbi t s. LPBACK = 0;
/ / Aut o Baud enabl e bi t <5>
U1MODEbi t s. ABAUD = 0;
/ / RX pol ar i t y i nver si on bi t <4>
U1MODEbi t s. RXI NV = 0;
/ / Hi gh Baud r at e enabl e bi t ( 1: Hi gh spd. mode 4x/ 0: St d. spd. mode 16x)
bi t <3>
U1MODEbi t s. BRGH = 0;
/ / Par i t y and Dat a sel ect i on bi t s <2: 1>
U1MODEbi t s. PDSEL = 0;
/ / St op sel ect i on bi t ( 1: 2 st op bi t s / 0: 1 st op bi t s) bi t <0>
U1MODEbi t s. STSEL = 0;
/ *                                  */
/ *     USARTx St at us Resi gt er      */
/ / Aut omat i c addr ess det ect mode enabl e bi t ( 1: Enabl e/ 0: Di sabl e) bi t <24>
U1STAbi t s. ADM_EN = 0;
/ / Aut omat i c addr ess mask bi t s<23: 16>
U1STAbi t s. ADDR = 0;
/ / TX I nt er r upt mode sel ect i on bi t s<15: 14>
U1STAbi t s. UTXI SEL = 2;
/ / TX pol ar i t y i nv. bi t ( 1: UxTX i dl e st at e ' 0' / 0: UxTX i dl e st at e ' 1' )
bi t <13>
U1STAbi t s. UTXI NV = 0;
/ / Recei ver enabl e bi t ( 1: RX enabl e/ 0: RX di sabl e) bi t <12>
U1STAbi t s. URXEN = 1;
/ / Tr ansmi t br eak ( 1: Send Br eak / 0: Don' t send br eak) bi t <11>
U1STAbi t s. UTXBRK = 0;
/ / Tr ansmi t enabl e bi t ( 1: TX enabl e / 0: TX di sabl e) bi t <10>
U1STAbi t s. UTXEN = 1;
/ / RX i nt er r upt mode sel ect i on bi t s<7: 6>
U1STAbi t s. URXI SEL = 0;
/ / Addr ess char act er det ect bi t ( 1: Enabl e / 0: Di sabl e) bi t <5>
U1STAbi t s. ADDEN = 0;
}
}
/ ********************************************************************
* Funct i on Name: st r i ng_RX *
* Ret ur n Val ue: NONE *
* Par amet er s: NONE *
88
* Descr i pt i on: Thi s r out i ne Recei ves a st r i ng f r omt he USART *
********************************************************************/
doubl e st r i ng_RX( char r x_st r i ng_sel )
{
unsi gned char t emp_buf f er ;
/ /     Pl ace t he r ecei ved byt e i nt o t he r x_buf f er [ ]    
f or ( t emp_buf f er =0; t emp_buf f er < 16; t emp_buf f er ++)
{
r x_buf f er [ t emp_buf f er ] = RX_usar t ( r x_st r i ng_sel ) ;
i f ( r x_buf f er [ t emp_buf f er ] == ' \ r ' )
{
br eak;
}
}
/ /     Now t r ansmi t ( echo back) t he r x_buf f er [ ] t hat hol ds t he st r i ng    
/ * f or ( x=0; x < 10; x++)
{
i f ( r x_buf f er [ x] == ' \ r ' )
{
br eak;
}
TX_usar t ( r x_st r i ng_sel , r x_buf f er [ x] ) ;
}
*/
/ /     Use t he at of ( )    
r et ur n( at of ( r x_buf f er ) ) ;
}
/ ********************************************************************
* Funct i on Name: RX_usar t *
* Ret ur n Val ue: Val ue t hat i s on t he RCREG *
* Par amet er s: r x_sel : RX f r omei t her USART1 or USART2 *
* Descr i pt i on: Thi s r out i ne Recei ves a byt e f r omt he USART *
* Pol l f or r ecei ved char act er . *
********************************************************************/
char RX_usar t ( char r x_sel ) / / <<====
{
/ / RX f r omUSART 2
i f ( r x_sel ==2)
{
/ / Over r un er r or occur r ed
i f ( U2STAbi t s. OERR==1)
{
/ / ERROR cl ear ed Pr evi ousl y OERR t o ' 0'
U2STAbi t s. OERR=0;
}
/ / whi l e U2RXREG i s empt y, keep pol l i ng
whi l e( ! U2STAbi t s. URXDA) ;
/ / r et ur n what ever i s i n t he U2RXREG r egi st er
r et ur n U2RXREG;
}
el se / / RX f r omUSART 1
{
/ / Over r un er r or occur r ed
i f ( U1STAbi t s. OERR==1)
89
{
/ / ERROR cl ear ed Pr evi ousl y OERR t o ' 0'
U1STAbi t s. OERR=0;
}
/ / whi l e U1RXREG i s empt y, keep pol l i ng
whi l e( ! U1STAbi t s. URXDA) ;
/ / r et ur n what ever i s i n t he U1RXREG r egi st er
r et ur n U1RXREG;
}
}
/ **********************************************************************
* Funct i on Name: TX_st r i ng *
* Ret ur n Val ue: voi d *
* Par amet er s: dat a: poi nt er t o st r i ng of dat a *
* Descr i pt i on: Thi s r out i ne t r ansmi t s a st r i ng of char act er s *
* t o t he USART i ncl udi ng t he nul l . *
**********************************************************************/
voi d TX_st r i ng( char t x_st r i ng_sel , char *dat a) / / <<====
{
/ / Tr ansmi t a Byt e, st op af t er nul l i s t r ansmi t t ed
whi l e( *dat a)
{
/ / Poi nt ( i ncr ement ) t o next char act er t o be t r ansmi t t ed
TX_usar t ( t x_st r i ng_sel , *dat a++) ;
}
}
/ ********************************************************************
* Funct i on Name: TX_usar t *
* Ret ur n Val ue: none *
* Par amet er s: dat a t o t r ansmi t *
* Descr i pt i on: Thi s r out i ne t r ansmi t s a byt e out t he USART *
* af t er t he t r ansmi t r eady bi t goes hi gh *
* Tr ansmi t byt e and wai t f or r eady. *
********************************************************************/
voi d TX_usar t ( char t x_sel , char dat a) / / <<====
{
/ / TX out t he USART 2
i f ( t x_sel ==2)
{
/ / Load dat a t o t he TXREG
U2TXREG = dat a;
/ / whi l e TXREG i s FULL, keep sendi ng dat a
whi l e( ! U2STAbi t s. TRMT) ;
}
el se / / TX out t he USART 1
{
/ / Load dat a t o t he TXREG
U1TXREG = dat a;
/ / whi l e TXREG i s FULL, keep sendi ng dat a
whi l e( ! U1STAbi t s. TRMT) ;
}
}
90
/ **********************************************************************
* Funct i on Name: ser _di gi t *
* Ret ur n Val ue: voi d *
* Par amet er s: dat a *
* Descr i pt i on: Thi s r out i ne t r ansmi t s a char act er out t he *
* ser i al por t as a 4 di gi t number . *
**********************************************************************/
voi d ser i al _di gi t ( char di gi t _sel , shor t i nt dat a) / / <<====
{
TX_usar t ( di gi t _sel , ' 0' + dat a/ 10000 ) ; / / ext r act / send f i r st di gi t
TX_usar t ( di gi t _sel , ' 0' + ( dat a%10000) / 1000 ) ; / / ext r act / send second
di gi t
TX_usar t ( di gi t _sel , ' 0' + ( dat a%1000) / 100 ) ; / / ext r act / send t hi r d di gi t
TX_usar t ( di gi t _sel , ' 0' + ( dat a%100) / 10 ) ; / / ext r act / send f our t h
di gi t
TX_usar t ( di gi t _sel , ' 0' + dat a%10 ) ; / / ext r act / send f i f t h di gi t
}
/ ********************************************************************
* Funct i on Name: put sf ( ) *
* Ret ur n Val ue: st r i ng l egnt h *
* Par amet er s: x, st r i ng, pr eci si on *
* Descr i pt i on: pr i nt s f l oat out t he USART *
********************************************************************/
voi d f l oat _t o_t ext ( char t x_st r i ng_sel , doubl e number , char pr eci si on)
{
char st r i ng[ MAX_STRI NG] ;
f t oa( number , st r i ng, pr eci si on, ' f ' ) ;
/ / Pr i nt ( Send) dat a out of USART
TX_st r i ng( t x_st r i ng_sel , st r i ng) ;
}
/ ********************************************************************
* Funct i on Name: f t oa( ) *
* Ret ur n Val ue: st r i ng l egnt h *
* Par amet er s: x, st r i ng, pr eci si on, f or mat *
* Descr i pt i on: Conver t s f l oat t o asci i *
* *
* Not e: Code was adpat ed f r omTr ampas St er n. *
********************************************************************/
i nt f t oa ( doubl e x, char *st r , char pr ec, char f or mat )
{
i nt i e, i , k, ndi g, f st yl e;
doubl e y;
char *st ar t ;
st ar t =st r ;
/ / Based on pr ecessi on set number di gi t s
ndi g=pr ec+1;
i f ( pr ec<0)
{
ndi g=7;
}
i f ( pr ec>22)
{
ndi g=23;
}
/ / Exponent ' e'
91
f st yl e = 0;
i f ( f or mat == ' f '   f or mat == ' F' )
{
/ / nor mal ' f '
f st yl e = 1;
}
i f ( f or mat ==' g'   f or mat ==' G' )
{
f st yl e=2;
}
i e = 0;
/ / I f x negat i ve, wr i t e mi nus and r ever se
i f ( x < 0)
{
*st r ++ = '  ' ;
x =  x;
}
/ / I f ( x<0. 0) t hen i ncr ement by 10 t i l l bet wen 1. 0 and 10. 0
i f ( x! =0. 0)
{
whi l e ( x < 1. 0)
{
x= x* 10. 0;
i e  ;
}
}
/ / I f x>10 t hen l et ' s shi f t i t down
whi l e( x >= 10. 0)
{
x = x*( 1. 0/ 10. 0) ;
i e++;
}
i f ( abs( i e) >MAX_MANTI SA)
{
i f ( f st yl e==1)
{
f st yl e=0;
f or mat =' e' ;
}
}
/ / I n f f or mat , number of di gi t s i s r el at ed t o si ze
i f ( f st yl e)
{
ndi g= ndi g + i e;
}
i f ( pr ec==0 && i e>ndi g && f st yl e)
{
ndi g=i e;
}
/ / Round x i s bet ween 1 and 10 and ndi g wi l l be pr i nt ed t o
/ / r i ght of deci mal poi nt
y=1;
92
/ / Fi nd l est si gni f i cant di gi t
f or ( i = 1; i < ndi g; i ++)
y = y *( 1. 0/ 10. 0) ; / / Mul t i pl y by 1/ 10 i s f ast er t han
di vi des
x = x+ y *( 1. 0/ 2. 0) ; / / Add r oundi ng
/ / Repai r r oundi ng di sast er s
i f ( x >= 10. 0)
{
x = 1. 0;
i e++;
ndi g++;
}
/ / Check and see i f t he number i s l ess t han 1. 0
i f ( f st yl e && i e<0)
{
*st r ++ = ' 0' ;
i f ( pr ec! =0)
{
*st r ++ = ' . ' ;
}
i f ( ndi g < 0)
{
i e = i e ndi g; / / Li mi t zer os i f under f l ow
}
f or ( i =  1; i > i e; i   )
{
*st r ++ = ' 0' ;
}
}
/ / For each di gi t
f or ( i =0; i < ndi g; i ++)
{
f l oat b;
k = x; / / k = most si gni f i cant di gi t
*st r ++ = k + ' 0' ; / / Out put t he char r epr esent at i on
i f ( ( ( ! f st yl e && i ==0)   ( f st yl e && i ==i e) ) && pr ec! =0)
{
/ / Out put a deci mal poi nt
*st r ++ = ' . ' ;
}
b=( f l oat ) k;
/ / Mul t i pl y by 10 bef or e subt r act i on t o r emove
/ / er r or s f r oml i mi t ed number of bi t s i n f l oat .
b=b*10. 0;
x=x*10. 0;
/ / Subt r act k f r omx
x =x  b;
}
/ / Now, i n est yl e, put out exponent i f not zer o
i f ( ! f st yl e && i e ! = 0)
{
*st r ++ = f or mat ;
i f ( i e < 0) / / I f number has negat i ve exponent
{
93
i e =  i e;
*st r ++ = '  ' ;
}
/ / Now we need t o conver t t he exponent t o st r i ng
f or ( k=1000; k>i e; k=k/ 10) ; / / Fi nd t he decade of exponent
f or ( ; k > 0; k=k/ 10)
{
char t ;
*st r ++ = t + ' 0' ;
i e = i e  ( t *k) ;
}
}
*st r ++ = ' \ 0' ;
/ / r et ur n st r i ng l engt h
r et ur n ( st r  st ar t ) ;
}
accel.c
/ / =============================================================================
/ *
Pr ogr ammer : J ose L. Cor ona
Pr oj ect Name: Read ( get ) t he accel er omet er ADC r eadi ng.
Dat e: 2/ 21/ 09
MCU: PI C32MX360F512L
Compl i er : MPLAB C32
*/
/ / =============================================================================
#def i ne zer o_g_of f set ( 2. 520117*1024) / 3. 3
/ / ( 3. 3V) / 1024= 0. 00322265625 mV/ bi t
#def i ne adc_r es_val ue 0. 00322265625
/ / ( 1g/ 1000mV) *( 0. 00322265625mV/ bi t ) = 0. 00322265625 mg/ bi t
#def i ne accel _mg_r es 0. 00322265625
/ / Const ant s
#def i ne pi 3. 1415926535898
#def i ne degr ees ( 180. 0/ pi )
/ / Fxn decl ar at i on
doubl e r ead_accel ( i nt accel _val ue, char mode) ;
/ ********************************************************************
* Funct i on Name: r ead_accel *
* Ret ur n Val ue: t i l t angl e( degr ees / r adi ans) *
* Par amet er s: accel er omet er val ue, mode *
* Descr i pt i on: accel er omet er ADC r eadi ng *
********************************************************************/
doubl e r ead_accel ( i nt accel _val ue, char mode)
{
doubl e adc_accel _r esul t = 0. 0;
doubl e accel _r eadi ng= 0. 0;
/ / doubl e adc_accel _mV= 0. 0;
94
doubl e accel _deg= 0. 0;
doubl e mg= 0. 0;
/ /     Accel . Dat a Conver si on    
adc_accel _r esul t = ( doubl e) accel _val ue;
/ / Conver t ADC val ue t o mV uni t s
/ / adc_accel _mV= adc_r es_val ue*( adc_accel _r esul t  2. 0) ;
/ / Cal cul at e ADC mV   > r ad ( mG)
mg=( ( adc_accel _r esul t  2. 0)  zer o_g_of f set ) *accel _mg_r es;
/ /     Check f or out +/  1G r ange    
i f ( mg>= 1. 0 )
{
mg= 1. 0;
}
el se i f ( mg<=  1. 0 )
{
mg=  1. 0;
}
/ /     Choose bet ween Degr ees/ Radi ans    
i f ( mode == ' d' )
{
/ / Resul t i n degr ees
accel _deg= asi n( mg) ;
accel _r eadi ng= accel _deg*degr ees;
}
el se i f ( mode == ' r ' )
{
/ / Resul t i n r adi ans
accel _r eadi ng= asi n( mg) ;
}
r et ur n accel _r eadi ng;
}
gyro.c
/ / =============================================================================
/ *
Pr ogr ammer : J ose L. Cor ona
Pr oj ect Name: Read ( get ) t he gyr oscope ADC r eadi ng.
Dat e: 2/ 21/ 09
MCU: PI C32MX360F512L
Compl i er : MPLAB C32
*/
/ / =============================================================================
#def i ne zer o_gyr o_of f set ( 1. 5000*1024) / 3. 3
/ / ( 3. 3V) / 1024= 0. 00322265625 mV/ bi t
#def i ne adc_r es_val ue 0. 00322265625
/ / ( 0. 00322265625 mV) / ( 2mV/ degr ee/ sec) = 1. 6113281 degr ee/ sec ( 0. 0281230 r ad/ sec)
/ / #def i ne gyr o_r es_deg 1. 6113281
#def i ne gyr o_r es_r ad 0. 0281230
95
/ / Const ant s
#def i ne pi 3. 1415926535898
#def i ne degr ees ( 180. 0/ pi )
/ / Fxn decl ar at i on
doubl e r ead_gyr o( i nt gyr o_val ue, char mode) ;
/ ********************************************************************
* Funct i on Name: r ead_accel *
* Ret ur n Val ue: t i l t angl e( degr ees / r adi ans) *
* Par amet er s: accel er omet er val ue, mode *
* Descr i pt i on: accel er omet er ADC r eadi ng *
********************************************************************/
doubl e r ead_gyr o( i nt gyr o_val ue, char mode)
{
doubl e adc_gyr o_r esul t = 0. 0;
doubl e gyr o_r eadi ng= 0. 0;
/ / doubl e adc_gyr o_mV= 0. 0;
doubl e angl e_r at e= 0. 0;
doubl e gyr o_deg= 0. 0;
/ /     Gyr o. Dat a Conver si on    
adc_gyr o_r esul t = ( doubl e) gyr o_val ue;
/ / Conver t ADC val ue t o mV uni t s
/ / adc_gyr o_mV= adc_r es_val ue*( adc_gyr o_r esul t  2. 0) ;
/ / Cal cul at e ADC mV   > r ad/ s
angl e_r at e= ( ( adc_gyr o_r esul t  2. 0)  zer o_gyr o_of f set ) *gyr o_r es_r ad;
/ /     Choose bet ween Degr ees/ Radi ans    
i f ( mode == ' d' )
{
/ / Resul t i n degr ees
gyr o_r eadi ng= angl e_r at e*degr ees;
}
el se i f ( mode == ' r ' )
{
/ / Resul t i n r adi ans
gyr o_r eadi ng= angl e_r at e;
}
r et ur n gyr o_r eadi ng;
}
knight_rider.c
/ / =============================================================================
/ *
Pr ogr ammer : J ose L. Cor ona
Pr oj ect Name: Kni ght Ri der LED Ef f ect
Dat e: 2/ 19/ 09
96
MCU: PI C32MX360F512L
Compl i er : MPLAB C32
*/
/ / =============================================================================
#def i ne LED_DELAY 250000
voi d kni ght _r i der ( voi d) ;
voi d kni ght _r i der ( voi d)
{
/ / Set ar r ay decl ar at i on
const char num[ ] ={1, 2, 4, 8, 16, 32, 64, 128, 64, 32, 16, 8, 4, 2, 1};
char count ;
i nt i , j ;
/ / Scan t he ar r ay st r uct ur e 10 t i mes.
f or ( i =0; i <10; i ++)
{
/ / Cl ear PORTA
PORTA=0x0000;
/ / Smal l del ay
f or ( j =0; j < LED_DELAY; j ++) ;
/ / Scan t he ar r ay st r uct ur e el ement s.
f or ( count = 0; count <= 15; count ++)
{
/ / Smal l del ay.
f or ( j =0; j < LED_DELAY; j ++) ;
/ / Di spl ay ar r ay el ement on LED' s an i ncr ement t o next el ement
ar r ay.
PORTA= num[ count ] ;
}
}/ / End of mai n f or l oop
/ / Cl ear PORTA
PORTA=0x0000;
}
kalman.h
/ / =============================================================================
/ *
Pr ogr ammer : J ose L. Cor ona
Pr oj ect Name: ADC Test Sof t war e usi ng Kal man Fi l t er f or PI C32
Dat e: 2/ 15/ 09
MCU: PI C32MX360F512L
Compl i er : MPLAB C32
*/
/ / =============================================================================
#i f ndef KALMAN_H
#def i ne KALMAN_H
t ypedef st r uct
{
/ / Two st at es, angl e and gyr o bi as. Unbi ased angul ar r at e i s a bypr oduct .
doubl e x_bi as;
97
doubl e x_r at e;
doubl e x_angl e;
/ / Covar i ance of est i mat i on er r or mat r i x.
doubl e P_00;
doubl e P_01;
doubl e P_10;
doubl e P_11;
/ / St at e const ant s.
doubl e dt ;
doubl e R_angl e;
doubl e Q_gyr o;
doubl e Q_angl e;
} kal man;
voi d kal man_i ni t ( kal man *f i l t er , doubl e dt , doubl e R_angl e, doubl e Q_gyr o,
doubl e Q_angl e) ;
voi d kal man_pr edi ct ( kal man *f i l t er , doubl e gyr o_r at e) ;
voi d kal man_updat e( kal man *f i l t er , doubl e angl e_measur ed) ;
/ / Get t he bi as.
doubl e kal man_get _bi as( kal man *f i l t er )
{
r et ur n f i l t er  >x_bi as;
}
/ / Get t he r at e.
doubl e kal man_get _r at e( kal man *f i l t er )
{
r et ur n f i l t er  >x_r at e;
}
/ / Get t he angl e.
doubl e kal man_get _angl e( kal man *f i l t er )
{
r et ur n f i l t er  >x_angl e;
}
#endi f
kalman.c
/ / =============================================================================
/ *
Pr ogr ammer : J ose L. Cor ona
Pr oj ect Name: ADC Test Sof t war e usi ng Kal man Fi l t er f or PI C32
Dat e: 2/ 15/ 09
MCU: PI C32MX360F512L
Compl i er : MPLAB C32
*/
/ / =============================================================================
#i ncl ude " kal man. h"
/ *********************************************************************
The i mpl ememt ed Kal man f i l t er est i mat es t he angl e t hat wi l l be used
on t he PI D cont r ol l er al ogr i t hm. The f i l t er consi st s of t wo st at es
[ angl e, gyr o_bai s] .
98
*********************************************************************/
/ ********************************************************************
* Funct i on Name: kal man_i ni t *
* Ret ur n Val ue: none *
* Par amet er s: st r uct f i l t er , dt , R_angl e, Q_gyr o, Q_angl e *
* Descr i pt i on: I ni t i al i ze t he Kal man Fi l t er par amet er s. *
********************************************************************/
voi d kal man_i ni t ( kal man *f i l t er , doubl e dt , doubl e R_angl e, doubl e Q_gyr o,
doubl e Q_angl e)
{
/ / I ni t i al i ze t he t wo st at es, t he angl e and t he gyr o bi as. As a
/ / bypr oduct of comput i ng t he angl e, we al so have an unbi ased
/ / angul ar r at e avai l abl e.
f i l t er  >x_bi as = 0. 0;
f i l t er  >x_r at e = 0. 0;
f i l t er  >x_angl e = 0. 0;
/ / I ni t i al i ze t he del t a i n seconds bet ween gyr o sampl es.
f i l t er  >dt = dt ;
/ / I ni t i al i ze t he measur ement noi se covar i ance mat r i x val ues.
/ / I n t hi s case, R i s a 1x1 mat r i x t ha r epr esent s expect ed
/ / j i t t er f r omt he accel er omet er .
f i l t er  >R_angl e = R_angl e;
/ / I ni t i al i ze t he pr ocess noi se covar i ance mat r i x val ues.
/ / I n t hi s case, Q i ndi cat es how much we t r ust t he accel er omt er
/ / r el at i ve t o t he gyr os.
/ / Q_gyr o set t o 0. 003 and Q_angl e set t o 0. 001.
f i l t er  >Q_gyr o = Q_gyr o;
f i l t er  >Q_angl e = Q_angl e;
/ / I ni t i al i ze covar i ance of est i mat e st at e. Thi s i s updat ed
/ / at ever y t i me st ep t o det er mi ne how wel l t he sensor s ar e
/ / t r acki ng t he act ual st at e.
f i l t er  >P_00 = 1. 0;
f i l t er  >P_01 = 0. 0;
f i l t er  >P_10 = 0. 0;
f i l t er  >P_11 = 1. 0;
}
/ ********************************************************************
* Funct i on Name: kal man_pr edi ct *
* Ret ur n Val ue: none *
* Par amet er s: st r uct f i l t er , measur ed gyr oscope val ue *
* Descr i pt i on: Cal l ed ever y dt ( Ti mer 1 over f l ow wi t h a bi ased *
* gyr o. Al so updat es t he cur r ent r at e and angl e *
* est i mat e) . *
********************************************************************/
voi d kal man_pr edi ct ( kal man *f i l t er , doubl e dot _angl e)
{
/ / St at i c so t hese ar e kept of f t he st ack.
st at i c doubl e gyr o_r at e_unbi ased;
st at i c doubl e Pdot _00;
st at i c doubl e Pdot _01;
st at i c doubl e Pdot _10;
st at i c doubl e Pdot _11;
99
/ / Unbi as our gyr o.
gyr o_r at e_unbi ased= dot _angl e  f i l t er  >x_bi as;
/ / St or e our unbi ased gyr o est i mat e.
f i l t er  >x_r at e= gyr o_r at e_unbi ased;
/ / Updat e t he angl e est i mat e.
f i l t er  >x_angl e= f i l t er  >x_angl e + ( dot _angl e  f i l t er  >x_bi as) *f i l t er  >dt ;
/ / Comput e t he der i vat i ve of t he covar i ance mat r i x
/ / Pdot = A*P + P*A' + Q
Pdot _00 = f i l t er  >Q_angl e  f i l t er  >P_01  f i l t er  >P_10;
Pdot _01 =  f i l t er  >P_11;
Pdot _10 =  f i l t er  >P_11;
Pdot _11 = f i l t er  >Q_gyr o;
/ / Updat e t he covar i ance mat r i x.
f i l t er  >P_00 += Pdot _00 * f i l t er  >dt ;
f i l t er  >P_01 += Pdot _01 * f i l t er  >dt ;
f i l t er  >P_10 += Pdot _10 * f i l t er  >dt ;
f i l t er  >P_11 += Pdot _11 * f i l t er  >dt ;
}
/ ********************************************************************
* Funct i on Name: kal man_updat e *
* Ret ur n Val ue: none *
* Par amet er s: st r uct f i l t er , measur ed angl e val ue *
* Descr i pt i on: Cal l ed when a new accel er omet er angl e *
* measur ement i s avai l abl e. Updat es t he est i mat ed*
* angl e t hat wi l l be used. *
********************************************************************/
voi d kal man_updat e( kal man *f i l t er , doubl e angl e_measur ed)
{
/ / St at i c so t hese ar e kept of f t he st ack.
st at i c doubl e y;
st at i c doubl e S;
st at i c doubl e K_0;
st at i c doubl e K_1;
/ / Comput e t he er r or i n t he est i mat e.
/ / I nnovat i on or Measur ement Resi dual
/ / y = z  Hx
y= angl e_measur ed  f i l t er  >x_angl e;
/ / Comput e t he er r or est i mat e.
/ / S = C P C' + R
S = f i l t er  >R_angl e + f i l t er  >P_00;
/ / Comput e t he Kal man f i l t er gai ns.
/ / K = P C' i nv( S)
K_0 = f i l t er  >P_00 / S;
K_1 = f i l t er  >P_10 / S;
/ / Updat e covar i ance mat r i x.
/ / P = P  K C P
f i l t er  >P_00  = K_0 * f i l t er  >P_00;
f i l t er  >P_01  = K_0 * f i l t er  >P_01;
f i l t er  >P_10  = K_1 * f i l t er  >P_00;
f i l t er  >P_11  = K_1 * f i l t er  >P_01;
/ / Updat e t he st at e ( new) est i mat es ( Cor r ect t he pr edi ct i on of t he st at e) .
100
/ / Al so adj ust t he bi as on t he gyr o at ever y i t er at i on.
/ / x = x + K * y
f i l t er  >x_angl e= f i l t er  >x_angl e + K_0 * y;
f i l t er  >x_bi as= f i l t er  >x_bi as + K_1 * y;
}
pid.h
/ / =============================================================================
/ *
Pr ogr ammer : J ose L. Cor ona
Pr oj ect Name: PI D al gor i t hmSof t war e f or PI C32
Dat e: 2/ 19/ 09
MCU: PI C32MX360F512L
Compl i er : MPLAB C32
*/
/ / =============================================================================
#i f ndef PI D_H
#def i ne PI D_H
/ / Dat a st r uct ur e f or set poi nt s and dat a used by
/ / t he PI D cont r ol al gor i t hm
/ / Maxi mumval ue of var i abl es ( 32767. 00)
/ / Set l i mi t s t o a over f l ow of si gn pr obl ems
#def i ne MAX_I NT 16000. 00
#def i ne MAX_I _TERM 250. 00
t ypedef st r uct
{
/ / Maxi mumal l owed er r or , avoi d over f l ow
doubl e max_er r or ;
/ / Maxi mumal l owed sumer r or , avoi d over f l ow
doubl e max_sum_er r or ;
/ / Summat i on of er r or s, used f or i nt egr at e cal cul at i ons
doubl e sum_er r or ;
101
/ / Last measur ed val ue, used t o f i nd der i vat i ve of pr ocess val ue.
doubl e pr ev_measur ed_val ue;
/ / The Pr opor t i onal t uni ng const ant , mul t i pl i ed wi t h SCALI NG_FACTOR
doubl e P_Fact or ;
/ / The I nt egr al t uni ng const ant , mul t i pl i ed wi t h SCALI NG_FACTOR
doubl e I _Fact or ;
/ / The Der i vat i ve t uni ng const ant , mul t i pl i ed wi t h SCALI NG_FACTOR
doubl e D_Fact or ;
} pi d;
/ / I ni t i al i ze t he PI D cont r ol l er
voi d pi d_set up( doubl e p, doubl e i , doubl e d, pi d *set _pi d)
/ / PI D cont r ol l er
doubl e pi d_cont r ol l er ( pi d *pi d_get , doubl e sys_er r or , doubl e y) ;
#endi f
pid.c
/ / =============================================================================
/ *
Pr ogr ammer : J ose L. Cor ona
Pr oj ect Name: PI D Al gor i t hmSof t war e f or PI C32
Dat e: 2/ 19/ 09
MCU: PI C32MX360F512L
Compl i er : MPLAB C32
*/
/ / =============================================================================
#i ncl ude " pi d. h"
#def i ne LED_1 PORTAbi t s. RA0
#def i ne LED_2 PORTAbi t s. RA1
#def i ne LED_3 PORTAbi t s. RA2
#def i ne LED_4 PORTAbi t s. RA3
#def i ne LED_5 PORTAbi t s. RA4
#def i ne LED_6 PORTAbi t s. RA5
#def i ne LED_7 PORTAbi t s. RA6
#def i ne LED_8 PORTAbi t s. RA7
#def i ne PI D_Li mi t 375. 0
/ / =======>> The f ol l owi ng Fxn' s ar e used f or PI D Cont r ol l er <<=======
/ ********************************************************************
* Funct i on Name: pi d_set up *
* Ret ur n Val ue: none *
* Par amet er s: P, I , D f act or s, st r uct pi d *
* Descr i pt i on: Set up PI D cont r ol l er par amet er s *
* 1. p_f act or Pr opor t i onal t er m *
102
* 2. i _f act or I nt egr al t er m *
* 3. d_f act or Der i vat e t er m *
* 4. st r uct pi d *
********************************************************************/
voi d pi d_set up( doubl e p, doubl e i , doubl e d, pi d *set _pi d)
{
/ / Cl ear val ues f or PI D cont r ol l er
set _pi d >sum_er r or = 0. 00;
set _pi d >pr ev_measur ed_val ue= 0. 00;
/ / Tuni ng const ant s f or PI D l oop
set _pi d >P_Fact or = p_f act or ;
set _pi d >I _Fact or = i _f act or ;
set _pi d >D_Fact or = d_f act or ;
/ / Li mi t s t o avoi d i nt egr al over f l ow or wi ndup
set _pi d >max_er r or = 8000. 0;
set _pi d >max_sum_er r or = 250. 0;
}
/ ***********************************************************************
* Funct i on Name: pi d_cont r ol l er *
* Ret ur n Val ue: t he ( u) whi ch i s f ed t o t he pr ocess *
* Par amet er s: st r uct pi d, r ef er ence poi nt , pr ocess out put ( y) *
* Descr i pt i on: PI D cont r ol al gor i t hm. Cal cul at es out put f r om *
* set poi nt , pr ocess val ue and PI D st at us. *
* 1. r ef   > Desi r ed val ue *
* 2. y   > Measur ed pr ocess val ue. *
* 3. st r uct pi d *
* *
* ________________ ___________ *
* r ef ____ er r or   u   y *
*      >        > PI D Cont r ol l er         > DC Mot or s         > *
*  ____  ________________  ___________  *
* ^  *
*  
*
*  _____________  *
*     *
*                      I MU Sensor s                    *
*  _____________ *
* *
************************************************************************/
doubl e pi d_cont r ol l er ( pi d *pi d_get , doubl e sys_er r or , doubl e y)
{
LED_2= 0;
LED_3= 0;
LED_4= 0;
LED_5= 0;
doubl e p_t er m;
doubl e i _t er m;
103
doubl e d_t er m;
doubl e u;
/ /       Cal cul at e Pr opor t i onal t er mand l i mi t er r or over f l ow      
i f ( sys_er r or > pi d_get  >max_er r or )
{
LED_2= 1;
p_t er m= MAX_I NT;
}
el se i f ( sys_er r or <  pi d_get  >max_er r or )
{
LED_3= 1;
p_t er m=  MAX_I NT;
}
el se
{
p_t er m= pi d_get  >P_Fact or * sys_er r or ;
}
/ /       Cal cul at e I nt egr al t er mand l i mi t i nt egr al wi ndup/ r unaway      
pi d_get  >sum_er r or += sys_er r or ;
i f ( pi d_get  >sum_er r or > pi d_get  >max_sum_er r or )
{
LED_4= 1;
i _t er m= MAX_I _TERM;
pi d_get  >sum_er r or = pi d_get  >max_sum_er r or ;
}
el se i f ( pi d_get  >sum_er r or <  pi d_get  >max_sum_er r or )
{
LED_5= 1;
i _t er m=  MAX_I _TERM;
pi d_get  >sum_er r or =  pi d_get  >max_sum_er r or ;
}
el se
{
i _t er m= pi d_get  >I _Fact or * pi d_get  >sum_er r or ;
}
/ /       Cal cul at e Der i vat e t er m     
d_t er m= pi d_get  >D_Fact or * ( pi d_get  >pr ev_measur ed_val ue  y) ;
pi d_get  >pr ev_measur ed_val ue= y;
/ /       Sumt he P, I , D f act or s t o get a cont r ol l er out put      
u= p_t er m+ i _t er m+ d_t er m;
/ /       Li mi t t he Cont r ol l er Out put      
i f ( u > PI D_Li mi t )
{
u= PI D_Li mi t ;
}
el se i f ( u <  PI D_Li mi t )
{
u=  PI D_Li mi t ;
}
r et ur n u;
}