Team: Colon
Blow
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.
}
