Module level functions:

DuringDrive4Offset
DuringTurn2Reload
DuringDrive4Offset2
DuringLook4Reload
DuringDrive2Reload
DuringLook4Emitter
DuringDrive2Emitter
DuringReload

Module level variables:

CurrentState, TEAM, P_RELOAD, ReachedXGoal, ReachedYGoal

RunFaceOffSM
set MakeTransition to false by default
set NextState to CurrentState
default to normal entry to new state
assume we are not consuming event

if current state is Drive4Offset

Execute During function for Drive4Offset. ES_ENTRY & ES_EXIT are
processed here allow the lower level state machines to re-map
or consume the event
process any events

if an event is active
if event is EV_X_TARGET_REACHED
set ReachedXGoal to true
if we have reached YGoal
stop robot
Decide the next state to be Turn2Reload
for internal transitions, skip changing MakeTransition
mark that we are taking a transition
if transitioning to a state with history change kind of
entry
optionally, consume or re-map this event for the upper
level state machine
end if
end if
end of this block

if event is EV_Y_TARGET_REACHED
set ReachedYGoal to true
if we have reached XGoal
stop robot
Decide the next state to be Turn2Reload
for internal transitions, skip changing MakeTransition
mark that we are taking a transition
if transitioning to a state with history change kind of
entry
optionally, consume or re-map this event for the upper
level state machine
end if
end of this block

end of this block

 if current state is Turn2Reload
Execute During function for Turn2Reload. ES_ENTRY & ES_EXIT are
processed here allow the lower level state machines to re-map
or consume the event
process any events
if an event is active
if event is EV_ANGLE_TARGET_REACHED
Decide next state to be Drive4Offset2
for internal transitions, skip changing MakeTransition
mark that we are taking a transition
if transitioning to a state with history change kind of entry
optionally, consume or re-map this event for the upper
level state machine
end of this block

end of this block

if current state is Drive4Offset2

Execute During function for state Drive4Offset2. ES_ENTRY & ES_EXIT
are
processed here allow the lower level state machines to re-map
or consume the event
process any events

if an event is active
if event is EV_X_TARGET_REACHED
set ReachedXGoal to be true
Decide next state to be Look4Reload
for internal transitions, skip changing MakeTransition
mark that we are taking a transition
if transitioning to a state with history change kind of entry
optionally, consume or re-map this event for the upper
level state machine
end of this block

end of this block

if current state is Look4Reload

Execute During function for state Look4Reload. ES_ENTRY & ES_EXIT are
processed here allow the lower level state machines to re-map
or consume the event
process any events

if an event is active
if event is EV_BEACON_FOUND
Execute action function for Look4Reload, EV_BEACON_FOUND
if CurrentEvent EventParam is close to the reload period we
are looking for
stop beacon capture interrupt and beacon oneshot
interrupt
Decide next state to be Drive2Reload
for internal transitions, skip changing MakeTransition
mark that we are taking a transition
else
set NextState to Look4Reload, keep looking for beacon
set MakeTransition to false
 end if
if transitioning to a state with history change kind of entry
optionally, consume or re-map this event for the upper
level state machine
end of this block

if event is EV_BEACON_NOTFOUND
perform dummy searching, rotate a small angle per time
set robot to turn CW at some speed
start a timer for rotating
set NextState to Look4Reload
set MakeTransition to false
if transitioning to a state with history change kind of entry
optionally, consume or re-map this event for the upper
level state machine
ReturnEvent.EventType = ES_NO_EVENT;
end of this block

if event is ES_TIMEOUT
restart oneshot after turning a fixed angle to determine if
we find the beacon
set NextState to Look4Reload
set MakeTransition to false
if transitioning to a state with history change kind of entry
optionally, consume or re-map this event for the upper
level state machine
end of this block

end of this block

if current state is Drive2Reload

Execute During function for state Drive2Reload. ES_ENTRY & ES_EXIT
are
processed here allow the lower level state machines to re-map
or consume the event
process any events

if an event is active
if event is EV_X_TARGET_REACHED
decide the next state to be Look4Emitter
for internal transitions, skip changing MakeTransition
mark that we are taking a transition
if transitioning to a state with history change kind of entry
optionally, consume or re-map this event for the upper
level state machine
end of this block

end of this block

if current state is Look4Emitter

Execute During function for state Look4Emitter. ES_ENTRY & ES_EXIT
are
processed here allow the lower level state machines to re-map
or consume the event

process any events

if an event is active
 if event is EV_EMITTER_DETECTED
Execute action function for Look4Emitter, EV_EMITTER_DETECTED
stop reload capture and one shot
Decide what next state to be Drive2Emitter
for internal transitions, skip changing MakeTransition
mark that we are taking a transition
if transitioning to a state with history change kind of entry
optionally, consume or re-map this event for the upper
level state machine
end of this block

if event is EV_EMITTER_MISSED
perform dummy searching, rotate a small angle per time
set robot to turn CW at some speed
start a timer for turning
Set NextState to be Look4Emitter
mark that we are not making a transition
if transitioning to a state with history change kind of entry
optionally, consume or re-map this event for the upper
level state machine
end of this block

if event is ES_TIMEOUT
restart oneshot after turning a fixed angle to determine if
we find the beacon
set NextState to Look4Emitter
mark that we are not making a transition
if transitioning to a state with history change kind of entry
optionally, consume or re-map this event for the upper
level state machine
end of this block

end of this block

if current state is Drive2Emitter

Execute During function for state one. ES_ENTRY & ES_EXIT are
processed here allow the lower level state machines to re-map
or consume the event
process any events
if an event is active
if event is EV_X_TARGET_REACHED
stop robot
Decide the next state to be Reload
for internal transitions, skip changing MakeTransition
mark that we are taking a transition
if transitioning to a state with history change kind of entry
optionally, consume or re-map this event for the upper
level state machine
end of this block

end of this block

if current state is Reload

Execute During function for state one. ES_ENTRY & ES_EXIT are
processed here allow the lowe
level state machines to re-map
or consume the event
 process any events

if an event is active
If event is event one
Execute action function for Reload,ES_ENTRY
Decide next state to be Reload
for internal transitions, skip changing MakeTransition
mark that we are not taking a transition
if transitioning to a state with history change kind of entry
optionally, consume or re-map this event for the upper
level state machine
end of this block

end of this block

if we are making a state transition

Execute exit function for current state
Modify state variable
Execute entry function for new state
this defaults to ES_ENTRY
end if

return ReturnEvent

End of RunFaceOffSM

StartFaceOffSM
set CurrentState to be ENTRY_STATE
grab team info from passed-in event
set reload station period info according to TEAM
call the entry function (if any) for the ENTRY_STATE
End of StartFaceOffSM

QueryFaceOffSM
return CurrentState
End

/***************************************************************************
private functions
***************************************************************************/
DuringDrive4Offset
assume no re-mapping or consumption

process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ES_ENTRY
implement any entry actions required for this state machine
upon entering this state, we start to drive the robot forward at
some speed
set target position (P1X, P1Y)
reset ReachedXGoal and ReachedYGoal to false

after that start any lower level machines that run in this state
else if ES_EXIT
on exit, give the lower levels a chance to clean up first
now do any local exit functionality
else
 do the 'during' function for this state
run any lower level state machine
end if

return either Event, if you don't want to allow the lower level machine
to remap the current event, or ReturnEvent if you do want to allow it.
End of DuringDrive4Offset

DuringTurn2Reload
assume no re-mapping or consumption

process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ES_ENTRY
implement any entry actions required for this state machine
set robot to turn CW at some speed
Set target heading
after that start any lower level machines that run in this state
else if ES_EXIT
on exit, give the lower levels a chance to clean up first
now do any local exit functionality
else
do the 'during' function for this state
run any lower level state machine
end if

return either Event, if you don't want to allow the lower level machine
to remap the current event, or ReturnEvent if you do want to allow it.
End of DuringTurn2Reload

DuringDrive4Offset2
assume no re-mapping or consumption

process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ES_ENTRY
implement any entry actions required for this state machine
upon entering this state, we start to drive the robot forward at some
speed
set target position (P1bX, P1bY)
reset ReachedXGoal and ReachedYGoal to be false
after that start any lower level machines that run in this state
else if ES_EXIT
on exit, give the lower levels a chance to clean up first
else
do the 'during' function for this state
run any lower level state machine

return either Event, if you don't want to allow the lower level machine
to remap the current event, or ReturnEvent if you do want to allow it.
End of DuringDrive4Offset2

DuringLook4Reload
assume no re-mapping or consumption

process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

 if ES_ENTRY
implement any entry actions required for this state machine
entry Look4Reload state by starting capture and oneshot
init and start beacon capture and beacon oneshot
else if ES_EXIT
on exit, give the lower levels a chance to clean up first
now do any local exit functionality
else
do the 'during' function for this state
run any lower level state machine

return either Event, if you don't want to allow the lower level machine
to remap the current event, or ReturnEvent if you do want to allow it.
End of DuringLook4Emitter

DuringDrive2Reload
assume no re-mapping or consumption

process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ES_ENTRY
implement any entry actions required for this state machine
upon entering this state, we start a timer and start to drive the
robot
set robot to run forward at some speed
set target position (P2X, P2Y)
after that start any lower level machines that run in this state
else if ES_EXIT
on exit, give the lower levels a chance to clean up first
now do any local exit functionality
else
do the 'during' function for this state
run any lower level state machine

return either Event, if you don't want to allow the lower level machine
to remap the current event, or ReturnEvent if you do want to allow it.
End of DuringDrive2Reload

DuringLook4Emitter
assume no re-mapping or consumption

process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ES_ENTRY
implement any entry actions required for this state machine
entry Look4Reload state by starting capture and oneshot
init and start reload capture and reload oneshot
after that start any lower level machines that run in this state

else if ES_EXIT
on exit, give the lower levels a chance to clean up first
else
do the 'during' function for this state
run any lower level state machine

return either Event, if you don't want to allow the lower level machine
to remap the current event, or ReturnEvent if you do want to allow it.
End of DuringLook4Emitter

DuringDrive2Emitter
assume no re-mapping or consumption

process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ES_ENTRY
implement any entry actions required for this state machine
upon entering this state, we start to drive the robot forward at some
speed
set target position (P2bX, P2bY)

after that start any lower level machines that run in this state
else if ES_EXIT
on exit, give the lower levels a chance to clean up first
else
do the 'during' function for this state
run any lower level state machine

return either Event, if you don't want to allow the lower level machine
to remap the current event, or ReturnEvent if you do want to allow it.
End of DuringDrive2Emitter

DuringReload
assume no re-mapping or consumption

process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ES_ENTRY
implement any entry actions required for this state machine
init IR PWM
set servo motors to LoadBall
after that start any lower level machines that run in this state
else if ES_EXIT
on exit, give the lower levels a chance to clean up first
else
do the 'during' function for this state
run any lower level state machine

return either Event, if you don't want to allow the lower level machine
to remap the current event, or ReturnEvent if you do want to allow it.
End of DuringReload