ME 218B
Dan Aukes
Kristina Babiarz
Josh Oechslin
March 11, 2008
“Speed is the essence of war…travel
by unexpected routes and strike him where he has taken no precautions.”[1]
Our strategy was based on our ability to traverse complex, “tape ridden” terrain in the most efficient way possible: with superior speed. Even though we only went for goal #1, which was the lowest scoring goal, by acting quickly and finishing the game early, we hoped to limit the other team’s points. Our mechanical and electrical designs were centered around our need for speed.
We designed an autonomous robot that implemented a simple strategy to collect balls, hold them, and release them into goal #1. To collect balls, we placed a piece of foam core in the front of our robot to depress the ball request button. As each ball dropped from the dispenser onto our robot, a funnel-like device channeled the balls into a hole, which led to a PVC pipe with a door motor acting as an electro-mechanical switch to prevent balls from leaving our robot.
Our robot’s chassis consisted of a ¼”-thick piece of laser-cut acrylic (10” x 3”) and a 1/8”-thick piece of laser-cut masonite (10” x 10” with ¼” fillets). We placed the front edge of the acrylic piece two inches from the front edge of the masonite, which was also the front edge of our robot. This decision was based on our desire to place the drive wheels closer to the front of our robot rather than in the middle.
The ¼” piece of acrylic was attached to our drive transmission system consisting of the following parts/mechanisms (see Figure 1 below):
Figure 1: Drive transmission
(middle), line sensors (top), PVC pipe and casters (bottom)
To provide our robot excellent stability while driving, we placed two spherical caster wheels at the back side of our robot. We placed these two casters 4.67” apart center-to-center, which was long enough for stability, yet not significantly long to prevent our robot from moving too slowly at a given duty cycle.
To track the various pieces of tape on the playing fields, we implemented a set of three QRB1134 reflective object sensors. Each adjacent pair of tape sensors was placed 0.5625” apart center-to-center based on this measurement of the tape’s width. We desired to have only the middle tape sensor activated while traveling in a perfectly-straight line. To provide our robot a precisely-manufactured part on which to mount our tape sensors, we used a 1/8”-thick laser-cut piece of masonite.
To provide our robot greater robustness in dealing with the possibility of encountering the playing fields’ walls, we implemented four QRB1134 reflective object sensors, one at each corner of our robot. To allow for tweaking the direction and location of these proximity sensors, we designed our masonite chassis to have L-shaped holes at each of the four corners, with the width and/or diameter of these holes slightly larger than the width/diameter of the tape sensor slots (about 0.15”).
As each ball dropped from the ball dispenser onto our robot, it fell onto a funnel made from foam-core. The funnel was sloped in two different directions to prevent balls from getting stuck en route to our PVC pipe and ball release mechanism. We cut a hole at the nadir of our funnel with a diameter slightly larger than the ball’s diameter of 1.75”. To attach the funnel to our robot, we used some electrical tape instead of hot glue to allow us the ability to tweak various components inside our robot.
Figure 2: Funnel mechanism
We collected five balls at each trip to the ball dispenser because our PVC pipe could hold five to six balls without the possibility of balls getting stuck in the hole of our funnel. To provide an electro-mechanical ball release mechanism, we drilled a hole on the top surface of the PVC pipe near the end of the pipe. A door motor was placed directly above this hole, which opened for releasing balls and closed for preventing balls from leaving our robot at inappropriate times. To elevate the door motor above the PVC pipe, we designed two identical 1/8”-thick pieces of masonite with slots for the door motor’s mounting holes and placed this on both sides of the PVC pipe.
Figure 3: Door motor (orange object
near top) on top of PVC pipe
To adequately incorporate our electrical circuits and E128 microcontroller board, we laser-cut several sets of four holes through which we mounted various circuits. In addition, all of the circuits except for the battery lead board (to prevent accidental contact of this board with the environment) were visible to us as we tested our robot, which aided our diagnosis of various problems.
Figure 4: Power Distribution
For all logic level signals, an LM7805 voltage regulator was used to convert the 14 V battery potential to 5 V. All motors ran off of 14 V.
Figure 5: Line sensing circuit (x3).
Figure
6: Wall sensing circuit (x4).
Looking at the absolute maximum ratings and the electrical/optical characteristics from Figure 8 below, the minimum resistor value for the diode is 66 Ohms, which provides a maximum current of 50 mA.
(5 V – 1.7 V)/50 mA = 66 Ohms
In Figure 6 above, a 75 Ohm 5% resistor was used giving a possible current range of 41.9- 46.3 mA, which is below the maximum allowable current. A large forward current was needed in order to induce a large collector current (Figure 8, Typical performance curves) so that distances farther from the wall could be detected (i.e. @ 0.05” Ic = ~4 mA, @ 1.5” Ic = 0.6 mA). The 10K Ohm resistor was chosen empirically; it gave the best performance for distance sensing.
In Figure 5 above, a 200 Ohm 5% resistor was used providing a possible current range of 15.7-17.4 mA. Since this circuit was used for tape sensing, the distance from the sensor to the ground was fixed. An appropriate resistor value (1K Ohms) was selected empirically, and the distance was then optimized to allow for the greatest change in voltage between the white playing surface and the red tape.
The analog output from both circuits provided a 0V-5V signal; this signal was fed into the analog pins of the E128. In software, the line sensing signals were input into a proportional-derivative controller. The feedback signal was the difference between the two outer line sensors (Figure 7), and the command signal was set to zero.
Figure
7: Line sensing PD control.
Figure
8: QRB1134 Absolute maximum
ratings, Electrical/Optical characteristics, and Typical performance curves[2].
Figure 9: Motor driver with TLE5206
(2 wheel motors, 1 door lock motor).
The TLE5206 was used because it allows for 5 A continuous current and 6 A peak current. The motors used to drive the robot had a stall current of 7 A. This is slightly above the peak rated current for this driver, but sufficient for the task.
R = 2.0 Ohms
14 V/ 2.0 Ohms = 7 A = Istall
The door lock motor supplied by the SPDL lab, which was used to dispense the nerf balls, had a stall current of ~4 A.
R = 3.8 Ohms
14 V/ 3.8 Ohms = 3.68 A = Istall
Figure 10: Beacon sensing circuit
(x4).
To convert the irradiance of the IR signal into a voltage, we used a trans-resistive circuit. The non-inverting input of this op-amp was given a virtual ground (2 V). We tested a range of resistor values and found that 3.3 kΩ gave the strongest signal without saturating the LM6144.
As a general note, we decided to use the LM6144 because it is a rail to rail op amp and would maximize our output range.
The virtual ground was created using a LM6144 as a buffer with a non-inverting input from a 10KΩ potentiometer in parallel with a 0.1-μF capacitor (for signal stabilization).
After the trans-resistive circuit, the signal entered an inverting amplification stage using an LM6144 with a gain of 22. By sufficiently amplifying the IR signal, beacon signals emanating from distances greater than the length of the course were detectible.
Following this amplification, the signal entered a high-pass filter with a cutoff frequency of about 700 Hz. The beacons broadcasted at 1.25 kHz. To filter out ambient light, a high pass filter was used with a cutoff frequency of ~700 Hz. The calculated values are below.
Table 1:
Corner
freq: |
723.4315595 |
Hz |
R |
10 |
K Ohms |
C |
22 |
nF |
Finally, the signal entered an LM339N comparator. We used a tight hysteresis band centered around 2 V to filter the signal and produce clean logic levels.
Table 2: E128 Connections
E128 Pin Connections |
||
E128 Pin |
Purpose |
Connection |
U0 |
PWM |
Left Forward |
U1 |
PWM |
Left Reverse |
U2 |
PWM |
Right Forward |
U3 |
PWM |
Right Reverse |
U4 |
PWM |
Door Motor Up |
U5 |
PWM |
Door Motor Down |
AD0 |
Wall Sensors |
Back Left Wall Sensor |
AD1 |
Wall Sensors |
Back Right Wall Sensor |
AD2 |
Wall Sensors |
Front Left Wall Sensor |
AD3 |
Wall Sensors |
Front Right Wall Sensor |
AD4 |
--- |
--- |
AD5 |
Line Sensors |
Left Line Sensor |
AD6 |
Line Sensors |
Middle Line Sensor |
AD7 |
Line Sensors |
Right Line Sensor |
T0 |
Beacon Sensors |
Beacon 4 |
T1 |
Beacon Sensors |
Beacon 5 |
T2 |
Beacon Sensors |
Beacon 6 |
T3 |
Beacon Sensors |
Beacon 7 |
Table 3: Project purchases.
Part |
Supplier |
Unit type |
Quantity |
Cost per unit |
Total unit cost |
Sockets |
SPDL shop |
Part |
4 |
0.80 |
3.20 |
20K
potentiometers |
SPDL shop |
Part |
4 |
0.75 |
3.00 |
Resistors |
SPDL shop |
Part |
20 |
0.01 |
0.20 |
0.1 µF
tantalum capacitors |
SPDL shop |
Part |
2 |
0.50 |
1.00 |
TLE
5206-2 motor driver board set |
SPDL shop |
Board set |
2 |
9.50 |
19.00 |
TLE
5206-2 motor driver board set (donation) |
SPDL shop |
Board set |
1 |
9.50 |
9.50 |
TLE
5206-2 motor driver chip |
SPDL shop |
Part |
1 |
7.50 |
7.50 |
2"
inner diameter black PVC pipe |
Home Depot |
1’ pipe |
2 |
1.50 |
3.00 |
2"
inner diameter acrylic mail tube |
TAP Plastics |
2’ pipe |
2 |
2.99 |
5.98 |
Protoboards
with rails (1.5" x 2.5") |
SPDL shop |
Protoboard |
6 |
1.40 |
8.40 |
Door
motor |
SPDL shop |
Part |
1 |
5.50 |
5.50 |
7.2 V
battery with clip |
SPDL shop |
Part |
2 |
13.00 |
26.00 |
Maxon
drive motors (donation) |
Maxon |
Part |
2 |
0.00 |
0.00 |
Solder |
SPDL shop |
1 foot |
10 |
0.10 |
1.00 |
6-32 nuts |
Home Depot |
Bag |
1 |
0.99 |
0.99 |
Twisted
wire pair |
SPDL shop |
1 foot |
20 |
0.10 |
2.00 |
QRB1134
tape sensor |
SPDL shop |
Part |
7 |
1.50 |
10.50 |
2-pin
female molex connector |
SPDL shop |
Part |
3 |
0.10 |
0.30 |
3-pin
male molex connector |
SPDL shop |
Part |
3 |
0.10 |
0.30 |
3-pin
female molex connector |
SPDL shop |
Part |
3 |
0.10 |
0.30 |
4-pin
male molex connector |
SPDL shop |
Part |
7 |
0.10 |
0.70 |
4-pin
female molex connector |
SPDL shop |
Part |
7 |
0.10 |
0.70 |
Small
molex connector terminal |
SPDL shop |
Part |
50 |
0.05 |
2.50 |
Large
molex connector terminal |
SPDL shop |
Part |
20 |
0.10 |
2.00 |
0.25"
spider coupler |
SPDL shop |
Part |
4 |
1.25 |
5.00 |
0.125"
spider coupler |
SPDL shop |
Part |
4 |
1.25 |
5.00 |
Power
lead connector |
SPDL shop |
Part |
2 |
0.35 |
0.70 |
7805CT
voltage regulator |
SPDL shop |
Part |
4 |
1.50 |
6.00 |
Acrylic |
Donated |
1’x1’ |
2 |
0.00 |
0.00 |
Masonite |
Donated |
1’x1’ |
6 |
0.00 |
0.00 |
TOTAL
PROJECT COST |
- |
- |
- |
- |
130.27 |
· Prototype early and often (with foam-core!) so that you can test the mobility of your robot without relying on the LaserCAMM
· Use cheap materials, because then you have no regret redoing mockups many times.
· Make sure you find a balance between planning and doing.
· Do not rely on the LaserCAMM for all of your part-cutting! If you do, you will either have to wait in a long line or perhaps witness the machine not functioning.
· Get PRL safety training early in the quarter
·
Minton’s Lumber and TAP Plastics are close to
each other in
· McMaster has cheaper parts, they're just hard to find on their website
· Fix noise in your circuits with grounded shielding, twisted pair wire, and capacitors across your input wires
· Choose what type of team you want to be early: a team that splits responsibilities and delegates tasks, or one that works together to maximize understanding and involvement.
· Have fun with your team and others, otherwise you might be miserable at times
· Take lots of breaks – try new burrito places (while you’re making trips to parts store), get fresh air often
Team:
Dan Aukes
Kristina Babiarz
Josh Oechslin
Course: ME 218B
Final Project
include files
define some constants
define the door lock motor opening speed
define the door lock motor closing speed
define the cutoff value for determining whether
you're on the line.
declare function prototypes
declare module variables
declare unsigned long OverflowCount, for
retaining the number of times the E128 timer has overflowed.
declare unsigned long TimerOverFlow, for holding
the overflowcount in a form that can be easily added to the current timer
register.
declare BeaconT Beacon4, Beacon6, and
Beacon7. These structures hold all the
information for calculating and retaining information about our three beacon
detectors.
declare InputT Beacon4In, Beacon6In, and
Beacon7In. These structures hold input
data for each of our Beacon sensors, allowing us to easily read the input
values.
declare ints LineL, LineM, and LineR. These variables hold analog values for each
of our three analog line sensors
declare unsigned char bLineL, bLineM, and
bLineR. These variables hold boolean
data regarding whether our analog Line Sensor values are above a pre-set analog
value.
declare int state for state machine. Initialize to stFlashDetect.
declare structure LineControlInfo, of type
LineControlInfoT, for holding all of the line control variables
declare int DriveSpeed, for storing the current
open-loop DriveSpeed
declare unsigned long Time, for storing the
current time in milliseconds
declare unsigned char StartFlash, for storing
whether the flash has been seen
declare CalibrateLineL, CalibrateLineM,
CalibrateLineR as ints for storing analog line sensor calibration data
declare int calibrated, for storing whether
calibration has occurred
declare structures for holding closed-loop gain
values.
main() function
void main(void)
{
declare InitReturn of type ADS12ReturnTyp, for storing
ADS12_Init() return variable.
Initialize Analog subystem
Enable Interrupts
Initialize Timer 0 subsystem
Initialize Timer 1 subsystem
Initialize PWM subsystem
Initialize Beacon4In structure
Initialize Beacon6In structure
Initialize Beacon7In structure
Turn off motors
Turn off door lock motor
initialize SlowLineGains for slowly following the line
initialize MediumLineGains for following the line at medium
speed
initialize FastLineGains for following the line at top
speed
initialize EnterLineGains for entering the line(slow
average speed, high compensation)
initialize LineGains to one of the defined gains
structures.
while forever
{
run the state machine.
}
}
Function StateMachine()
Purpose: This function controls the transition
between different states, as well as controlling all of the non-timer-based
actuation and sensing.
void
StateMachine(void)
{
declare CourseSide, of type DirectionT, for storing which
side of the course this is.
declare InitialDirection, for storing which side of the
line sensors hits the tape first
declare TimeOld, for storing the time of the last state.
declare BeaconCount and CornerCount, for storing whether
the current state has seen a corner or beacon
declare int LastState, for comparing the current state with
the state of the last cycle.
save laststate
if the time is greater than the time limit, change states
to stDone
main state machine switch statement
switch(state)
{
case stFlashDetect:
{
if StartFlash then the flash has been detected. change
state to stBeaconOrient
}
case stBeaconOrient :
{
set the DriveSpeed to 15
run BeaconOrient function, using beacon4, turning left,
looking for 53% duty cycle with a tolerance of 2. if BeaconOrient returns a non-zero state,
{
run BeaconOrient to reset it
change state to stInitialLineFind
Reset InitialDirection
}
}
case stInitialLineFind :
{
Set DriveSpeed to 15
Drive forward at DriveSpeed
if the elapsed time is greater than 900 milliseconds and
Calibrated is not set
{
retain the current left, middle, and right line sensor
readingss (presumably on white).
set Calibrated.
}
the elapsed time is greater than 1000ms and the left line
sensor is on tape
{
set initial direction to left
move state to stDispenseLineTransition
}
the elapsed time is greater than 1000ms and the right line
sensor is on tape
{
set initial direction to right
move state to stDispenseLineTransition
}
}
case stDispenseLineTransition:
{
set DriveSpeed to 15
if the initial direction was left and the left line sensor
is no longer on the tape, move state to stDispenseEnterLine
if the initial direction was right and the right line
sensor is no longer on the tape, move state to stDispenseEnterLine
}
case stDispenseEnterLine:
{
set DriveSpeed to 15
update closed-loop line-following gains to EnterLineGains.
if left and right line sensors are off the line and the
middle line sensor is on black, move state to stDispenseLineControl1.
}
case stDispenseLineControl1 :
{
set DriveSpeed to 15
update closed-loop line-following gains to MediumLineGains.
if beacon4 sees an average beacon duty cycle between 45 and
55, set BeaconCount
if no line sensor sees the line
{
Stop motors
move state to stDispenseLineLost
reset BeaconCount
}
if left and right sensors are on the line
{
if beaconcount is set
{
stop motors
change state to stDispenseButtonWait
reset beaconcount
}
}
}
case stDispenseButtonWait :
{
declare numballs, for storing how many balls have been
picked up
set DriveSpeed to 15
close doorlock motor
if elapsed time is greater than 1100 milliseconds
{
increment numballs
turn off door lock motor
change state to stDispenseDriveOut
if numballs is greater than 5
{
change state to stGoalTurn1
reset numballs
}
}
}
case stDispenseDriveOut :
{
set DriveSpeed to 15
drive reverse at DriveSpeed
if elapsed time is greater than 300
{
change state to stDispenseDriveIn
}
}
case stDispenseDriveIn :
{
set DriveSpeed to 15
drive forward at DriveSpeed
if the left line sensor is on the tape and right is not,
turn left at DriveSpeed
if the right line sensor is on the tape and left is not,
turn right at DriveSpeed
if both tape sensors are on the tape
{
stop drive motors
change state to stDispenseButtonWait
}
}
case stGoalTurn1 :
{
set DriveSpeed to 30
drive reverse at DriveSpeed
if elapsed time is greater than 800
{
change state to stGoalTurn2
}
}
case stGoalTurn2 :
{
set DriveSpeed to 30
Rotate left at DriveSpeed
if no line sensor detects the line, change state to
stGoalTurn3
}
case stGoalTurn3 :
{
set DriveSpeed to 30
if the left line sensor turns on, change state to
stGoalTurn4
}
case stGoalTurn4 :
{
set DriveSpeed to 30
if the left line sensor turns off, change state to
stGoalLineControl
}
case stGoalLineTransition:
{
set DriveSpeed to 30
if the initial direction was left and the left line sensor
is now off, change state to stGoalLineEnter
if the initial direction was right and the right line
sensor is now off, change state to stGoalLineEnter
}
case stGoalLineEnter:
{
set DriveSpeed to 30
update closed-loop line-following gains to EnterLineGains.
if left and right line sensors are off the line, and middle
is on black
{
change state to stGoalLineControl
}
}
case stGoalLineControl :
{
set DriveSpeed to 30
update closed-loop line-following gains to FastLineGains.
if beacon4 sees
an average beacon duty cycle between 25 and 35, set BeaconCount
if no line sensor detects the line
{
stop the drive motors
change state to stGoalLineLost
reset beaconcount
reset cornercount
}
if elapsed time is greater than 1650 and cornercount is set
{
change state to stPoopTurn2
reset beaconcount
reset cornercount
}
if both the left and right tape sensors are on
{
if beaconcount is true
{
change state to stPoopTurn1
reset beaconcount
reset cornercount
}
}
}
case stPoopTurn1 :
{
set DriveSpeed to 30
Drive reverse at DriveSpeed
if elapsed time is greater than 1200 ms
{
change state to stPoopTurn2
}
}
case stPoopTurn2 :
{
set DriveSpeed to 30
rotate left at DriveSpeed
if no line is seen, change state to stPoopTurn3
}
case stPoopTurn3 :
{
set DriveSpeed to 30
if left line sensor sees the line, change state to
stPoopTurn4
}
case stPoopTurn4 :
{
set DriveSpeed to 30
if the left line sensor doesn't see the line
{
change state to stPoopTurn5
}
}
case stPoopTurn5:
{
set DriveSpeed to 30
update closed-loop line-following gains to MediumLineGains.
if elapsed time is greater than 700 ms
{
change state to stPoopChuteOpen
}
}
case stPoopChuteOpen :
{
set DriveSpeed to 30
stop motors
Open doorlock motor
if elapsed time is greater than 100 ms
{
change state to stPoopChuteClose
}
}
case stPoopChuteClose :
{
declare poopcount, for storing how many balls have been
released
set DriveSpeed to 30
close Doorlock motor
if elapsed time is greater than 500 ms
{
increment poopcount
change state to stPoopChuteOpen
if poopcount is greater than 6
{
change state to stDispenseLineControl2
reset poopcount
}
}
}
case stDispenseLineControl2 :
{
set DriveSpeed to 30
update closed-loop line-following gains to FastLineGains.
if beacon4 sees an average beacon duty cycle between 45 and
55, set BeaconCount
if no line sensor sees the line
{
stop the motors
change state to stDispenseLineLost
reset beaconcount
}
if both the left and right beacon sensors are on
{
if beaconcount is set
{
stop the motors
change state to stDispenseButtonWait
reset beaconcount
}
}
}
case stDispenseLineLost :
{
set drivespeed to 30
if the course side is not set
{
if the LineControlInfo.SavedError is less than zero
{
rotate left at drivespeed
set CourseSide to right
}
{
otherwise rotate right at drivespeed
set CourseSide to left
}
}
otherwise
{
if courseside is left
{
rotate right at drivespeed
}
otherwise
{
rotate right at drivespeed
}
}
if the left line is seen first
{
reset LineControlInfo.SavedError
update initialdirection to left
change state to stDispenseLineTransition
}
if the right line is seen first
{
reset LineControlInfo.SavedError
update initialdirection to right
change state to stDispenseLineTransition
}
}
case stGoalLineLost :
{
set drivespeed to 30
set cornercount
if we are on the left side of the course, rotate left at
drivespeed
otherwise, rotate right at drivespeed
if the left line sensor is on the tape
{
reset LineControlInfo.SavedError
set InitialDirection to left
change state to stGoalLineTransition
}
if the right line sensor is on the tape
{
reset LineControlInfo.SavedError
set InitialDirection to right
change state to stGoalLineTransition
}
}
case stDone:
{
stop drive motors
turn off door lock motor
}
default:
{
change state to stDone
}
}
if state is not equal
to the last state, update time_old
}
Function DoorLockMotor()
Purpose: This function operates the door lock
motor with PWM control. There are three
different states it can be in, open closed, or off.
Inputs:
DoorMotorT
position: enumerated datatype of the three states
DoorOff:
both H-bridge directions are off
DoorClosed:
motor is forward-biased
DoorOpen:
motor is reverse-biased
int
Speed: integer value between 0 and 100 which determines the PWM duty cycle
void
DoorLockMotor(DoorMotorT position, int Speed)
{
if speed is greater than 100, set speed equal to 100
if speed is less than 0, set speed equal to 0
switch based on the input position
{
case DoorOff:
set both directions to 0
case DoorClose:
set PWMDTY4 to 0
set PWMDTY5 to a scaled value between 0 and 100% of full
duty cycle as given by PWM_PERIOD
case DoorOpen:
set PWMDTY4 to a scaled value between 0 and 100% of full
duty cycle as given by PWM_PERIOD
set PWMDTY5 to 0
}
}
LINE CONTROL VARIABLE FUNCTION
Purpose: Update line sensors' analog variables
Inputs:
LineGainsT
Gains: Structure containing speed, KP, and KD values.
void
LineControl(LineGainsT Gains)
{
read line sensor value, LineL, scale by 1000 and the
calibration value, and save as LineL
read line sensor value, LineL, scale by 1000 and the
calibration value, and save as LineM
read line sensor value, LineL, scale by 1000 and the
calibration value, and save as LineR
if LineL is less than LINE_SENSOR_CUTOFF, set bLineL
if LineM is less than LINE_SENSOR_CUTOFF, set bLineM
if LineR is less than LINE_SENSOR_CUTOFF, set bLineR
save errorlast from error
update new error value
update command 1 to average speed
find the proportional command and scale by KP
find the derivative command and scale by KD
add proportional and derivative command signals and save as
command2
if savederror is greater than zero
{
if error is greater than the absolute value of saved error,
update error with the new max
}
if saved error is less than zero
{
if error is greater than the absolute value of saved error,
update error with the new max
}
if the line sensors are off, on, and off the
line(respectively), reset savederror
if the current state is one of several control-driven
states
{
drive according to the linecontrol command signals
}
}
TIMER OVERFLOW INTERRUPT FUNCTION
Purpose: Increment timer 0/channel 2's overflow
number and set overflow time
void
interrupt _Vec_tim0ovf Timer0OverflowHandler(void)
{
Reset the overflow flag
increment overflow count
convert overflow count to a usable form.
}
TIMER 0/CHANNEL 4 INTERRUPT HANDLER FUNCTION
Purpose: Handle timer 0's channel 4 interrupt
features:
clear timer flag 4, handle overflow with timer
flag 2, update
beacon 4's period and duty cycle
void
interrupt _Vec_tim0ch4 Timer0Ch4InterruptHandler(void)
{
declare unsigned long dt
reset Timer 4 flag
if overflow flag is set
{
reset overflow flag
increment overflow coutn
convert overflow count to a usable form.
}
if Beacon4In is nonzero
{
save periodlast
update period
}
otherwise
{
update onperiod
determine the the period and save as dt
if dt is within 25% of the actual duty cycle of the beacons
{
update duty, scaled by 100
}
otherwise
{
reset duty to 0
reset DutyGoodCount
}
}
}
TIMER 0/CHANNEL 5 INTERRUPT HANDLER FUNCTION
Purpose: Handle timer 0's channel 5 interrupt
features:
void
interrupt _Vec_tim0ch5 Timer0Ch5InterruptHandler(void)
{
clear the timer 5 flag
set StartFlash
disable Timer5 interrupts
}
TIMER 0/CHANNEL 6 INTERRUPT HANDLER FUNCTION
Purpose: Handle timer 0's channel 6 interrupt
features:
clear timer flag 6, handle overflow with timer
flag 2, update
beacon 6's period and duty cycle
void
interrupt _Vec_tim0ch6 Timer0Ch6InterruptHandler(void)
{
declare unsigned long dt
reset Timer 6 flag
if overflow flag is set
{
reset overflow flag
increment overflow coutn
convert overflow count to a usable form.
}
if Beacon6In is zero
{
save periodlast
update period
}
otherwise
{
update onperiod
calculate period
if dt is within 25% of the specified beacon frequency
{
update duty as a percentage of ontime/period
}
otherwise
{
reset duty
reset dutygoodcount
}
}
}
TIMER 0/CHANNEL 7 INTERRUPT HANDLER FUNCTION
Purpose: Handle timer 0's channel 7 interrupt
features:
clear timer flag 7, handle overflow with timer
flag 2, update
beacon 7's period and duty cycle
void
interrupt _Vec_tim0ch7 Timer0Ch7InterruptHandler(void)
{
declare unsigned long dt
clear timer 7 flag
if overflow flag is set
{
reset overflow flag
increment overflow coutn
convert overflow count to a usable form.
}
if beacon7in is zero
{
save periodlast
update period
}
otherwise
{
update onperiod
calculate dt as the total period
if dt is within 25% of specified period
{
update duty as
percentage of onperiod/period
}
otherwise
{
clear duty
clear dutygoodcount
}
}
}
TIMER 1/CHANNEL 7 INTERRUPT HANDLER FUNCTION
Purpose: Handle timer 1's channel 7 interrupt features:
clear timer flag 7, update current time by 2.4 ms
clock pulse, update
timer 1/channel 7's current time, update beacon
4's period and duty cycle
void
interrupt _Vec_tim1ch7 Timer1Ch7InterruptHandler(void)
{
declare static unsigned long Millisecondsx10.
increment Millisecondsx10 by 24
calculate actual time
clear timer7 flag
calculate new output compare time
enable interrupts
update beacon information for beacon 4
update beacon information for beacon 6
update beacon information for beacon 7
run LineControl()
}
Drive() function
Purpose: Move the drive motors in particular
configurations
Inputs:
DriveType:
enumerated type for selecting the drive configuration
Direction:
enumerated type for selecting motor directions
SpeedAverage:
average speed added to both motors
SpeedDifference:
differential speed added to one motor and subtracted from the other.
void
Drive(DriveTypeT DriveType, DirectionT Direction, int
SpeedAverage, int SpeedDifference)
{
if the drive type is anything but DriveLineFollow
{
if SpeedAverage is greater than 100, peg it at 100.
if SpeedAverage is less than zero, peg it at 0.
}
switch depending on DriveType
{
case DriveLineFollow:
{
declare ints A and B
Calculate A by adding SpeedAverage and SpeedDifference
Calculate B by subtracting SpeedDifference from
SpeedAverage
if A is less than -100, peg it at 100.
if A is greater than 100, peg it at 100.
if B is less than -100, peg it at 100.
if B is greater than 100, peg it at 100.
if A is greater than or equal to 0
{
set duty cycle for PWMDTY0 and PWMDTY1
}
otherwise
{
negate A
set duty cycle for PWMDTY0 and PWMDTY1
}
if B is greater than or equal to 0
{
set duty cycle for PWMDTY2 and PWMDTY3
}
otherwise
{
negate B
set duty cycle for PWMDTY2 and PWMDTY3
}
}
case DriveRotate:
{
if direction is left
{
Set duty cycles
}
otherwise if direction is right
{
Set duty cycles
}
break;
}
case DriveTurn:
{
if direction is left
{
Set duty cycles
}
otherwise if direction is right
{
Set duty cycles
}
}
case DriveTurnReverse:
{
if direction is left
{
Set duty cycles
}
otherwise if direction is right
{
Set duty cycles
}
}
case DriveStraight:
{
if direction is forward
{
Set duty cycles
}
otherwise if direction is reverse
{
Set duty cycles
}
}
case DriveStop:
{
Set all duty cycles to 0
}
default:
{
Set all duty cycles to 0
}
}
}
BEACON ORIENT FUNCTION
Purpose: Perform rotational search to find a
beacon given duty cycle parameters
Input: Rotational direction, beacon of interest,
duty cycle of interest, duty cycle tolerance, beacon state
Output: Beacon state (1 for succesful beacon
detection or no output)
int BeaconOrient(DirectionT
Direction,BeaconT Beacon, unsigned int GoalDutyCycle, unsigned
int Tolerance,unsigned
char Reset)
{
if reset
{
reset beaconstate
}
{
switch on BeaconState
{
case 0:
{
rotate left
if the beacon's average duty is within tolerance
{
change to BeaconState 1
stop motors
}
break
}
case 1:
{
break(do nothing)
}
default
{
break(do nothing)
}
}
}
return BeaconState
}
include files
********************************************
**********
BEACON UPDATE FUNCTION **********
********************************************
Purpose:
Motor operation at set T0-T3 duty cycles
Input:
Beacon struct duty cycle-relevant variables
Output:
Beacon struct duty cycle-relevant variables
********************************************
BeaconT UpdateBeacon(BeaconT Beacon, unsigned
long TimerOverFlow)
{
declare int j for incrementing through array
if the duty is greater than zero
{
if the beacon period is greater than 2times the beacon
frequency,
{
reset the duty to zero
reset the duty good counter to zero
}
otherwise, if the Duty is not between 0 and 100
{
reset the duty to zero
reset the duty good counter to zero
}
otherwise
{
increment the good count
}
}
otherwise
{
set the good count to 0
}
if the good count is
> 0
{
save the current duty into element i of thearray
increment i
save only the modulus of i
}
otherwise
{
for all the elements in the array
{
clear the element.
}
reset i to 0
}
if the goodcount is greater than the array size
by 5(eliminating any bad startup periods)
{
reset the average to 0
for all the elements in the array
{
add the element to the average
}
scale the average by the number of elements
}
otherwise
{
set the average to 0
}
return the beacon data structure
}
include files
InputT CreatePortEInput(E128Register BitMask)
{
create variable Input of type InputT
update the internal memory of Input with pointers for port
E.
set the bitmask for Input equal to BitMask
Return Input
}
InputT CreatePortTInput(E128Register BitMask)
{
return the result of InputConfig with pointers for port T
and BitMask
}
InputT CreatePortMInput(E128Register BitMask)
{
Return the result of InputConfig with pointers for port M
and BitMask
}
InputT CreatePortADInput(E128Register BitMask)
{
create variable Input of type InputT
update the internal memory of Input with pointers for port
AD.
set the bitmask for Input equal to BitMask
Return Input
}
OutputT CreatePortTOutput(E128Register BitMask)
{
return the result of OutputConfig with pointers for port T
and BitMask
}
OutputT CreatePortMOutput(E128Register BitMask)
{
return the result of OutputConfig with pointers for port M
and BitMask
}
OutputT CreatePortADOutput(E128Register BitMask)
{
create variable Output of type OutputT
update the internal memory of Output with pointers for port
AD
set output.bitmask equal to BitMask.
Return Output
}
InputT InputConfig(E128Register *DirPort, E128Register *Port,
E128Register BitMask)
{
create variable Input of type InputT
set internal variables equal to input variables
modify the port's direction register with 0's in the
bitmasked locations
return Input
}
OutputT OutputConfig(E128Register *DirPort, E128Register
*Port, E128Register BitMask)
{
create variable Output of type OutputT
set internal variables equal to input variables
modify the port's direction register with 1's in the
bitmasked locations
return Output
}
E128Register InputRead(InputT Input)
{
return the data register bitmasked with Input.BitMask
}
E128Register OutputRead(OutputT Output)
{
return the data register bitmasked with Output.BitMask
}
void
OutputWrite(OutputT Output, E128Register Value)
{
create temporary variables a and b of type E128Register
set a equal to the bits we want to change
set b equal to the bits we don't want to change
bitwise-or a and b together and set the data register equal
to that value.
}
include files
void
InitTimer0(void)
{
Input Capture/Output
Compare (1=output compare)
Set all Timers for
Input Capture
Enable the timer
subsystem
Output Compare Action (00=nothing 0L=toggle M0=clear ML=set)
Input Capture Action
(00=disabled 0A=rising B0=falling BA=any edge)
Configure all inputs
for any edge
enable interrupts for
timer 4, 5, 6, and 7.
enable the overflow
interrupt
}
void
InitTimer1(void)
{
Input Capture/Output
Compare (1=output compare)
set timer 7 for output
compare
Enable the timer
subsystem
Output Compare Action
(00=nothing 0L=toggle M0=clear ML=set)
set timer 7 to toggle
Input Capture Action
(00=disabled 0A=rising B0=falling BA=any edge)
disable all input
capture actions
enable interrupts for
timer 4, 5, and 6.
initialize first timer
value for TIM1_TC7
}
void InitPWM(void)
{
Enable PWM channel 0,
1, 2, 3, 4, 5
Set PWM Polarity
Register
Set PWM Clock register
Set PWM Prescaler
register to (bus clock/2)
disable center
align
set bits in the control
register
Scale clock SA
Scale clock SB
Set the PWM period
registers.
initialize the duty
cycle registers to zero
Link PWM channel 0 to
the T0 pin.
}