Initialize
Timer module
Wait 500
[ms]
Set output
pins
Set analog
input pins
Initialize
Arm module
Initialize
Motors module
Initialize
SPI module, this will block until first target is assigned by overlord
Determine curTarget from SPI module
Set startColor based on first target assignment
Turn on
team color LED
Initialize
Position module
Pos_GoToStart()
Set arm to
attack position
Set state
to TOTARGET
Set resetPosFlag to false
Repeat
Forever:
Set SPIStatus to SPI_getStatus()
Set ArmStopped to isArmStopped()
Set Pos_state to Pos_getStatus(SPIStatus)
If state changed
Print state
information for debugging
Switch state:
TOTARGET:
If Pos_state is STOPPED & SPIStatus
not DELAY
If
SPIStatus is GAME_OVER
Pos_GoToHome()
state = TOHOME
Else
swingArm(curTarget)
state = HITTING
HITTING:
If ArmStopped is True
SPI_requestTargetStatus()
state = CONFIRMING_HIT
CONFIRMING_HIT:
If SPIStatus is GAME_OVER
Pos_GoToHome()
state = TOHOME
If SPIStatus is TARGET_STATUS_READY
If
SPI_isCurTargetHit()
Set
resetPosFlag to false
Set
lastTarget to curTarget
state = GETTING_NEW_TARGET
Else
If
resetPosFlag
Set
resetPosFlag to false
SPI_skipCurTarget()
state = GETTING_NEW_TARGET
Set
lastTarget to curTarget
Else
Pos_ResetCurrentPos(curTarget)
state = TOTARGET
Set
resetPosFlag to true
GETTING_NEW_TARGET:
If SPIStatus is GAME_OVER
Pos_GoToHome()
state = TOHOME
Set curTarget to SPI_getCurTarget()
If curTarget not equal to -1
Pos_GoToTarget(curTarget)
state = TOTARGET
TOHOME:
If Pos_state is STOPPED
waveArm()
state = WAVING
Set
waveCount to zero
WAVING:
If ArmStopped
Incrememnt waveCount
If
waveCount less than 3
waveArm()
Else
Send
arm to attack position
state = DONE
DONE:
If ArmStopped
Send
arm to home position