InitMotor

Initialize AD pins and read initial values

Initialize limit switch pins and direction pins
Set Duty on Motor PWM ports to zero
Post ES_INIT to self

RunMotor
Current State is InitPState
If ES_INIT received, change state to Waiting

Current State is Waiting

If ES_START received, change state to InGame
If ES_GAMEOVER received, change state to GameOver

Current State is InGame

If ES_BACKCRASH is received, Hold the motors at zero speed
If ES_FULLFRONT is received, Set both motors to forward
If ES_FULLBACK is received, Set both motors to backward
If ES_HOLD is received, Hold the motors at zero speed
If ES_GAMEOVER is received, Hold the motors at zero speed, switch state to GameOver
If ES_CW is received, Rotate Left Motor and hold right motor
If ES_CCW is received, Rotate Right motor and hold the left motor
If ES_GETOPPONENTBEACON is received, Rotate Left motor and hold the right motor
If ES_GETTAPE is received, move both motors forward, set master tape flag and switch to GetTapeMonster State

Current State is GetTapeMonster

If ES_BACKCRASH is received, Hold the motors at zero speed
If ES_FULLFRONT is received, Set both motors to forward
If ES_FULLBACK is received, Set both motors to backward
If ES_HOLD is received, Hold the motors at zero speed
If ES_GAMEOVER is received, Hold the motors at zero speed, switch state to GameOver
If ES_FOLLOWLINECW_R is received, Rotate Right Motor in reverse and hold other motor, set all three tape flags
If ES_FOLLOWLINECCW_R is received, Rotate Left motor in reverse and hold other motor, set all three tape flags
If ES_GETOPPONENTBEACON is received, Rotate Left motor and hold the right motor, move to InGame state
If ES_TAPEMIDDLE is received, move both motors backwards, move to FollowLine state

Current State is FollowLine

If ES_BACKCRASH is received, Hold the motors at zero speed, reset TapeFlags, move to InGame state
If ES_FULLFRONT is received, Set both motors to forward
If ES_FULLBACK is received, Set both motors to backward
If ES_HOLD is received, Hold the motors at zero speed
If ES_GAMEOVER is received, Hold the motors at zero speed, switch state to GameOver
If ES_TAPERIGHT is received, Rotate Left motor in reverse and hold other motor, set all three tape flags
If ES_TAPELEFT is received, Rotate Left motor and hold the right motor, move to InGame state
If ES_TAPEMIDDLE is received, move both motors backwards, move to FollowLine state

CurrentState is GameOver
------

Check4FrontWall
If front limit switch position has changed, and is pressed, post ES_FRONTCRASH to MainMasterSM
Update Lastlimitswitch position as currentposition
Check4RearWall
If rear limit switch position has changed, and is pressed, post ES_BACKCRASH to MainMasterSM and MotorService
Update Lastlimitswitch position as currentposition

Check4Tape
If MasterTapeFlag is set, check if any tape sensor senses tape and post ES_TAPEDETECTED to masterservice if Tape is
detected
If TapeLeftFlag is set, check if left sensor detects tape and post ES_TAPE_LEFT to motor service if Tape is detected
If TapeMiddle is set, check if Middle sensor detects tape and post ES_TAPE_MIDDLE to motor service if Tape is detected
If TapeRight is set, check if Right sensor detects tape and post ES_TAPE_RIGHT to motor service if Tape is detected