Pseudocode

*****************************************************
MODULE: MasterSM
*****************************************************
Module Variables: MyPriority, Team, CurrentState, gameTime
-----------Function: InitMasterSM
Params: priority
save priority
initialize all ports
initialize inputs & outputs
init PWM
startMasterSM
-----------Function: RunMasterSM
Params: ES_Event CurrentEvent
if current state is INIT
set CurrentEvent to return val of DuringInit
if CurrentEvent is START_GAME
transition to RUN state
else if current state is RUN
set CurrentEvent to return val of DuringRun
if CurrentEvent is END_GMAE
transition to INIT state
-----------Function: StartMasterSM
Params: ES_Event CurrentEvent
set current state to Init
initialize capture routines
RunMasterSM
-----------Function: GetTeam
return Team
-----------Function: GameTimeRemaining
if current state is RUN
return gameTime
else
return (-1)
-----------Private Function: DuringInit
Params: ES_Event Event
if Event is entry event
start team timer
else if Event is exit event
else
if Event is time timer timeout
check team line to see if changed
if it has changed switch the current team

restart timer
-----------Private Function: DuringRun
Params: ES_Event Event
if Event is entry event
start game timer
initialize gameTime to 138
start GameSM
else if Event is exit event
RunGameSM with exit event
else
RunGameSM with event
if Event is game timer timeout
decrement gameTime
restart timer
if Event is UPDATED_POSITION
save updated position
if Event is OUR_ATTACK_START
tell strategy we madeAdAttack

*****************************************************
MODULE: GameSM
*****************************************************
Module Variables: CurrentState
-----------Function: RunGameSM
Params: ES_Event CurrentEvent
if current state is WAIT
set CurrentEvent to return val of DuringWait
if CurrentEvent is CAPTURE_START
transition to CAPTURE state
if CurrentEvent is DRIVE_START
transition to DRIVE state
if CurrentEvent is SHOOT_START
transition to SHOOT state
if CurrentEvent is START_ROTATE_SEARCH
transition to DRIVE state
set entry param to ROTATE_S
if CurrentEvent is START_REVERSE
transition to DRIVE state
set entry param to REVERSE_S
else if current state is CAPTURE
set CurrentEvent to return val of DuringCapture
if CurrentEvent is COMMAND_FINISHED
transition to WAIT state
else if current state is DRIVE
set CurrentEvent to return val of DuringDrive
if CurrentEvent is COMMAND_FINISHED

transition to WAIT state

else if current state is SHOOT
set CurrentEvent to return val of DuringShoot
if CurrentEvent is COMMAND_FINISHED
transition to WAIT state
-----------Function: StartGameSM
Params: ES_Event CurrentEvent
Set CurrentState to WAIT
RunGameSM
-----------Private Function: DuringWait
Params: ES_Event Event
if Event is entry event
get next command
-----------Private Function: DuringCapture
Params: ES_Event Event
if Event is entry event
start CaptureSM
else if Event is exit event
RunCaptureSM with exit event
else
RunCaptureSM with event
-----------Private Function: DuringDrive
Params: ES_Event Event
if Event is entry event
start DriveSM
else if Event is exit event
RunDriveSM with exit event
else
RunDriveSM with event
-----------Private Function: DuringShoot
Params: ES_Event Event
if Event is entry event
start ShootSM
else if Event is exit event
RunShootSM with exit event
else
RunShootSM with event
*****************************************************
MODULE: CaptureSM
*****************************************************
Module Variables: CurrentState, Period, LastCapture,
LookForInterrupt
-----------Function: RunCaptureSM
Params: ES_Event CurrentEvent

if current state is MEAS_F

set CurrentEvent to return val of DuringMeasFreq
if CurrentEvent is MEASURED_FREQ
transition to SEND_F1
else if current state is SEND_F
set CurrentEvent to return val of DuringSendFreq
if CurrentEvent is PAC_RESPONSE
if we don't own the location or the send failed
transition back to MEAS_F
else
we own the location
return COMMAND_FINISHED event
if CurrentEvent is capture timeout
failed to capture in set time
return COMMAND_FINISHED event
-----------Function: StartCaptureSM
Params: ES_Event CurrentEvent
Set CurrentState to MEAS_F
Start capture timeout timer
RunCaptureSM
-----------Function: StartLookForHall
Params: bool flag
Set LookForInterrupt to flag
-----------Function: HallInterruptResponse
clear interrupt
grab captured value and calculate period
if the last capture is valid
calculate period
if period is valid
save measured period
if we are looking for interrupt
if current state is MEAS_F
post MEASURED_FREQ event to MasterSM
if we are in DriveSM and LookingToStop on station
post MEASURED_FREQ event to MasterSM
update LastCapture to prepare for next edge
restart the interrupt
-----------Private Function: DuringMeasFreq
Params: ES_Event Event
if Event is entry event
set LastCapture and period to invalid
set LookForInterrupt to true
-----------Private Function: DuringSendFreq
Params: ES_Event Event
if Event is entry event
post PAC_REQUEST event to PacSM w/ period

*****************************************************
MODULE: DriveSM
*****************************************************
Module Variables: CurrentState, PositionDesired, PositionActual,
ResultingPosition, InitialMoveTics, DriveTics, DriveDiffIntegral,
LeftMotorDuty, RightMotorDuty, LeftMotorTics, DuringMovement,
StopAtStation, IgnoreStations
-----------Function: RunDriveSM
Params: ES_Event CurrentEvent
if current state is WAIT_FOR_POSITION
if CurrentEvent is ES_ENTRY && param is ROTATE_S
transition to ROTATE_SEARCH
if CurrentEvent is ES_ENTRY && param is REVERSE_S
transition to REVERSE_STATE
if CurrentEvent is UPDATED_POSITION
get position from strategy
set ResultingPosition to DesiredPosition
transition to ORIENT
else if current state is MOVE
set CurrentEvent to return val of DuringMove
if CurrentEvent is FINISHED_MOVEMENT
transition to WAIT_FOR_POSITION
return COMMAND_FINISHED event
else if current state is ORIENT
set CurrentEvent to return val of DuringOrient
if CurrentEvent is FINISHED_MOVEMENT
transition to WAIT_FOR_POSITION
else if current state is ROTATE_SEARCH
if CurrentEvent is entry event
start search timer
set flag to stop on a detected station
rotate bot
if CurrentEvent is search timeout
stop bot
transition to WAIT_FOR_POSITION
else if current state is REVERSE_STATE
if CurrentEvent is entry event
start reverse timer
set flag to ignore stations
reverse bot
if CurrentEvent is reverse timeout
stop bot
transition to WAIT_FOR_POSITION
if CurrentEvent is MEASURED_FREQ and we want to stop at a station
stop the bot
transition to WAIT_FOR_POSITION
return COMMAND_FINISHED event
-----------Function: StartDriveSM

Params: ES_Event CurrentEvent

Set CurrentState to WAIT_FOR_POSITION
Set all motor tic constants to zero
get desired position
set StopAtStation to false
RunDriveSM
-----------Function: LookingToStop
Params: bool flag
Set StopAtStation to flag
-----------Function: IgnoreStationsOnMove
Params: bool flag
Set IgnoreStations to flag
-----------Function: GetResultingPosition
returns ResultingPosition struct
-----------Function: LeftMotorEncoderCapture
clear interrupt
if DuringMovement
increment tics for drive timer
if DriveTics > 0
decrement DriveTics
increment LeftMotorTics for control law
otherwise we have reached the destination
stop bot
post FINISHED_MOVEMENT event to MasterSM
set DuringMovement to false
-----------Function: RightMotorEncoderCapture
clear interrupt
if DuringMovement
increment tics for drive timer
if DriveTics > 0
increment RightMotorTics for control law
-----------Private Function: DuringMove
Params: ES_Event Event
if Event is entry event
set StopAtStation to false
determine distance to travel
determine number of tics to travel (DriveTics)
if DriveTics is not zero
move bot forward at DesiredDutyMove
set LeftMotorDuty & RightMotorDuty to DesiredDutyMove
start control law timer
else
return FINISHED_MOVEMENT event
else if Event is exit event
stop bot
set move default to stop on stations

else
if control law timer exp
call control law
if IgnoreStations && StopAtStation are false
if DriveTics is less than ProximityDistance
set StopAtStation to true
set hall to tell us if freq. detected
-----------Private Function: DuringOrient
Params: ES_Event Event
if Event is entry event
determine rot_angle to travel & direction
update resulting position orientation
determine number of tics to travel (DriveTics)
if DriveTics is not zero
rotate bot forward at DesiredDutyRot
set LeftMotorDuty & RightMotorDuty to DesiredDutyRot
start control law timer
else
return FINISHED_MOVEMENT event
else if Event is exit event
stop bot
else
if control law timer exp
call control law
-----------Private Function: DifferentialControl
generate proportional error as difference in L & R tics
add proportional error to the integral error
generate duty difference using KI control
generate new values for left and right duty
-- centered around desired duty
perform anti-windup
update motor duty for each motors

*****************************************************
MODULE: ShootSM
*****************************************************
Module Variables: CurrentState, CurrentWidth, servoDir
-----------Function: RunShootSM
Params: ES_Event CurrentEvent
if current state is FIRE
set CurrentEvent to return val of DuringFire
if CurrentEvent is COMMAND_FINISHED
return COMMAND_FINISHED event
-----------Function: StartShootSM
Params: ES_Event CurrentEvent
Set CurrentState to FIRE

RunShootSM
-----------Private Function: DuringFire
Params: ES_Event Event
if Event is entry event
start flywheel
start flywheel shoot timer w/ warmup time
turn on servo
set servo initial position
else if Event is exit event
turn off flywheel
set servo to initial position
turn off servo
else
if shoot timeout event
increment servo motor position
update servo motor position
if at max/min servo position
change direction
restart time with pause time
else if at initial servo position
done shooting, start shoot_exp timer
else
restart timer with update servo length
if shoot_exp timeout event
set return event to COMMAND_FINISHED

*****************************************************
MODULE: PacSM
*****************************************************
Module Variables: MyPriority, CurrentState, ResponseBytes,
AfterWaitingState, RequestPeriod, GameStatus, AdAttacks,
StationStatus
-----------Function: InitPacSM
Params: priority
save priority
startPacSM
-----------Function: RunPacSM
Params: ES_Event CurrentEvent
if current state is INIT_PAC
if CurrentEvent is timeout
transition to WAITING state
else if current state is WAITING
if CurrentEvent is timeout
transition to AfterWaitingState
else if current state is CHECK_STATUS
set CurrentEvent to return val of DuringChecking

if CurrentEvent is PAC_DATA_RECEIVED
transition to WAITING state
else if current state is SEND_REQUEST
set CurrentEvent to return val of DuringSending
if CurrentEvent is PAC_REQUEST_END
transition to WAITING state
-----------Function: StartPacSM
Params: ES_Event CurrentEvent
set current state to INIT_PAC
init constants
initialize Pac capture routine
RunPacSM
-----------Function: PacInterruptResponse
disable interrupt
read value from Data Register
post PAC_DATA_RECEIVED event
-----------Function: GetStationStatus
Params: station
return the owner of the station
-----------Function: StationsOwned
Local Var: stations_owned
for each station
if the station status equals our team
increment stations_owned
return stations_owned
-----------Function: GetResponseAcknowledge
Params: Response_Byte
isolate and return acknowledge bits in Response_Byte
-----------Function: GetResponseOwner
Params: Response_Byte
isolate and return owner bits in Response_Byte
-----------Function: GetResponseLocation
Params: Response_Byte
isolate and return location bits in Response_Byte
-----------Private Function: DuringWaiting
Params: ES_Event Event
if Event is entry event
start pac timer with query rate length
if afterWaitingState is SEND_REQUEST
restart timer with Pac requeste rate length
else if Event is exit event

else
if Event is PAC_REQUEST
set AfterWaitingState to SEND_REQUEST
restart pac timer with Pac request length
save request data
-----------Private Function: DuringChecking
Params: ES_Event Event
if Event is entry event
enable interrupt
send status request to the register
else if Event is exit event
else
if interrupt occured
check game status
if game is on
check stations
check ad attacks
if Event is PAC_REQUEST
set AfterWaitingState to SEND_REQUEST
save request data
-----------Private Function: DuringSending
Params: ES_Event Event
if Event is entry event
enable interrupt
form request byte
send status request to the register
start query timer
else if Event is exit event
set AfterWaitingState to CHECK_STATUS
else
if query timer expeires
send query
restart query timer
if interrupt occured
if second byte is valid
post PAC_RESPONSE to MasterSM
set ReturnEvent to PAC_REQUEST_END
-----------Private Function: CheckGameStatus
if gameStatus is off and repsonse byte status is on
post start game event
update game status
if the game is on and response byte status is off
update game status to off
post end game event

-----------Private Function: CheckStations

Local Var: updated
set updated to false
update Station_Status using ResponseBytes
if any station updated
set updated to true
if updated is true
post PAC_STATIONS_UPDATED to MasterSM

*****************************************************
MODULE: PositionSM
*****************************************************
Module Variables: MyPriority, CurrentState, tic,
last_beacon, beacon_tics[4], first_tic, last_tic,
avg_array_x[][], avg_counter, last_position, averaged_position,
Period, LastCapture, first_capture, last_tics_per_rev,
first_rev
-----------Function: InitPositionSM
Params: priority
save priority
start beacon and tape sensor interrupts
startPositionSM
-----------Function: RunPositionSM
Params: ES_Event CurrentEvent
if current state is INIT_P
if CurrentEvent is START_TRIANGULATE
transition to TRIANGULATE state
else if current state is TRIANGULATE
set CurrentEvent to return val of DuringTriangulate
if CurrentEvent is END_TRIANGULATE
transition to INIT_P state
-----------Function: StartPositionSM
Params: ES_Event CurrentEvent
set current state to INIT_P
init constants
RunPositionSM
-----------Function: GetPosition
return the averaged position struct
-----------Function: PTInterruptResponse
clear interrupt
grab captured value and calculate period
if the last capture is valid
calculate frequency
if frequency is valid

determine the beacon seen

if the current beacon is different from the last beacon
if the current beacon is not the next beacon in the sequence
clear all beacon values
save the start tic of the current beacon
set the last beacon equal to current beacon
start the PT timer
otherwise
save the last tic beacon was seen at
else
set first capture to false
update LastCapture to prepare for next edge
restart the interrupt
-----------Function: PTEncoderInterruptResponse
clear source of interrupt
increment tic number
kick off interrupt
-----------Function: TapeSensorInterruptResponse
clear source of interrupt
set last_tics_per_rev equal to tic number
set tic number to zero
kick off interrupt
-----------Private Function: DuringTriangulate
Params: ES_Event Event
if Event is entry event
set avg_counter to 0
set beacons to unknown / invalid
set first_capture to true
start phototransistor motor
else if Event is exit event
stop phototransistor motor
else
if Event PT timeout occurs
implement midpoint scheme
calculate and store midpoint tic
update location
if location is updated
add position to the averaging scheme
increment average counter
if we have enough positions
calculate the average position
post UPDATED_POSITION event to MasterSM
set return event to END_TRIANGULATE
-----------Private Function: CalculateAveragePosition
determine raw mean of positions
determine variance and standard deviation
remove points greater than a deviation from mean
calculate average position of remaining points
-----------Private Function: UpdatePosition

determine if beacon tics can be used to calculate position

if not return false
calculate theta and phi
determine theta offset and phi offset
generate x,y coordinate from offsets
if point is not within the field dimensions
throw out outlier / return false
determine orientation using position and last beacon / tic

*****************************************************
MODULE: Strategy
*****************************************************
Module Variables: MyPriority, commandIndex, routineIndex,
active_routine, tries, cityIndex, NextCity, shoot_position,
balls_left
-----------Function: GetNextCommand
if no current active routine
get next routine
increment routineIndex
set commandIndex to zero
set active_routine to true
call appropriate routine function
increment commandIndex
-----------Function: GetNextRoutine
if cityIndex is less than 6
if city was acquired or already tried twice
increment cityIndex
set tries to 0
set first_move_to_city to true
if cityIndex < 5
if (first_move_to_city)
set routine to DRIVE_CAPTURE_ROUTINE
set NextCity using cityIndex
set first_move_to_city to false
increment tries
else
increment tries
set routine to REVERSE_DC_ROUTINE
return
if not in shoot position
set routine to START_MOVE_ROUTINE to move to shoot position
set shoot_position to true
if we still have balls left
routine is SHOOTING_ROUTINE
decrement balls_left

return
-----------Function: StartMoveToShootRoutine
ignore stations on move
if commandIndex is zero
move to middle of field
if commandIndex is 1
triangulate
move to MidShootingPosition
if commandIndex is 2
set currentPosition to resulting position from move
move to final shooting position
set active_routine to false
-----------Function: DriveCaptureRoutine
don't ignore stations on move
if commandIndex is zero
triangulate
move to next city
if commandIndex is 1
start capture
set active_routine to false
-----------Function: ReverseDCRoutine
don't ignore stations on move
if commandIndex is 0
reverse
if commandIndex is 1
triangulate
move to next city
if commandIndex is 2
start capture
set active_routine to false