/****************************************************************************
 Module:
 	Driving_SM.c

 Description:
 	A hierarchical state machine for the ME218B project.

 Module Variables:
	CurrentState
	BumpedFirst
	OurTeam

****************************************************************************/

/****************************************************************************
 Function RunDriving_SM
 	Takes an event to process
	Returns an event to return

****************************************************************************/
Depending on CurrentState:
      If bot is in TeamIDState...
		... and the left IR detector sees a signal,
			call LEDTeamID and pass it the number of the detected beacon.
		... and the right IR detector sees a signal,
			call LEDTeamID and pass it the number of the detected beacon.
               	... and the team is determined,
			switch to the PickUp state.   
       
	If bot is in the PickUp State...
		... and the front right bumper is hit,
			DoPickUpAction #1.
		... and the front left bumper is hit,
			DoPickUpAction #2.
		... and the back right bumper is hit,
			DoPickUpAction #3.
		... and the back left bumper is hit,
			DoPickUpAction #4.
		...and the driving timer expires,
			DoPickUpAction #0.
		...and the PickUp State timer expires (or the event is simulated),
			switch to the Locate Bin state.

	If bot is in the Locate Bin State...
		... and the 'Looking at Bin' event is triggered,
			switch to the Drive to Bin state.
		... and the left IR detector sees a beacon,
			if the beacon is on the same side as the bot
				switch to the Drive to Bin State.
		... and the right IR detector sees a beacon,
			if the beacon is on the same side as the bot
				switch to the Drive to Bin State.

	If bot is in the Drive to Bin State...
		... and either IR detector loses its beacon,
			switch to the Locate Bin State.
		... and the back right bumper is hit,
			set BumpedFirst to Right.
			switch to Align to Bin State.
		... and the back left bumper is hit,
			set BumpedFirst to Left.
			switch to Align to Bin State.

	If bot is in the Align to Bin State...
		... and BumpedFirst is 'Right'...
			and the back left bumper is hit,
				post event 'Time to Dump'
				switch to Chill State.
		... and BumpedFirst is 'Left'...
			and the back right bumper is hit,
				post event 'Time to Dump'
				switch to Chill State.

	If bot is in the Chill State...     
		... and bot posts a 'Team Determined' event,
			Switch to the PickUp State.
		... and bot posts a 'Start Driving again' event,
			Switch to the pickup State. 

If we are making a state transition
    Execute exit function for current state
    Execute entry function for new state
    return ReturnEvent

End of function
         
                    
/****************************************************************************
 Function StartDriving_SM
	Takes an event CurrentEvent
	Returns nothing
	Does any required initialization for this state machine
****************************************************************************/
Set Current State to TeamIDState 
Call RunDriving_SM with CurrentEvent
End of Function

/****************************************************************************
 Function QueryDriving_SM
	Takes nothing
	Returns the current state of the Hopping state machine
****************************************************************************/
Return the value of CurrentState
End of function

/***************************************************************************
 'During' functions:
 ***************************************************************************/

DuringTeamIDState
	Entry functions:
		Start a timer with Time_FindTeamBeacon
			(in case a time limit for this phase is needed)
		Start turning CCW:
			Set Left motor backward 20RPM
			Set Right motor forward 20RPM
	Exit functions: none
	During functions: none
End of function

DuringPickUpState
	Entry functions:
		Start a timer with Time_PickUpBalls
		Do PickUpAction #0
	Exit functions: none
	During functions: none
End of function

DuringLocateBinState
	Entry functions:
		Start turning CCW:
			Set Left motor backward 30RPM
			Set Right motor forward 30RPM
	Exit functions: none
	During functions: none
End of function

DuringDriveToBinState
	Entry functions:
		Start moving backwards:
			Set Left motor backward 30RPM
			Set Right motor backward 30RPM
	Exit functions: none
	During functions: none
End of function


DuringAlignToBinState
	Entry functions:
		If BumpedFirst = Right, start turning CCW:
			Set Left motor backward 15RPM
			Set Right motor forward 30RPM
		If BumpedFirst = left, start turning CW:
			Set Left motor forward 30RPM
			Set Right motor backward 15RPM
	Exit functions:
		Turn motors off
	During functions: none
End of function

DuringChillState
	Entry functions:
		Turn motors off
	Exit functions: none
	During functions: none
End of function

/**********************************************************
 Function LEDTeamID
	Takes an int, the number of the BeaconFound
	Returns nothing
**********************************************************/
Initialize an event
Depending on the number of BeaconFound:
	If BeaconFound is 1:
		Set OurTeam to Red
		Turn the Red Team LEDs on by calling RedLED_On()
	If BeaconFound is 4:
		Set OurTeam to Red
		Turn the Red Team LEDs on by calling RedLED_On()
	If BeaconFound is 2:
		Set OurTeam to Blue
		Turn the Blue Team LEDs on by calling BlueLED_On()
	If BeaconFound is 3:
		Set OurTeam to Blue
		Turn the Blue Team LEDs on by calling BlueLED_On()
Post a 'Team Determined' event to the master SM
End of Function