#define FACEOFF_DEBUG

/****************************************************************************
Module
FaceOffSM.c

Revision
2.0.1

Description
This is a template file for implementing state machines.

Notes

History
When Who What/Why
-------------- --- --------
02/27/17 09:48 jec another correction to re-assign both CurrentEvent
and ReturnEvent to the result of the During function
this eliminates the need for the prior fix and allows
the during function to-remap an event that will be
processed at a higher level.
02/20/17 10:14 jec correction to Run function to correctly assign
ReturnEvent in the situation where a lower level
machine consumed an event.
02/03/16 12:38 jec updated comments to reflect changes made in '14 & '15
converted unsigned char to bool where appropriate
spelling changes on true (was True) to match standard
removed local var used for debugger visibility in 'C32
commented out references to Start & RunLowerLevelSM so
that this can compile.
02/07/13 21:00 jec corrections to return variable (should have been
ReturnEvent, not CurrentEvent) and several EV_xxx
event names that were left over from the old version
02/08/12 09:56 jec revisions for the Events and Services Framework Gen2
02/13/10 14:29 jec revised Start and run to add new kind of entry function
to make implementing history entry cleaner
02/13/10 12:29 jec added NewEvent local variable to During function and
comments about using either it or Event as the return
02/11/10 15:54 jec more revised comments, removing last comment in during
function that belongs in the run function
02/09/10 17:21 jec updated comments about internal transitions on During
funtion
02/18/09 10:14 jec removed redundant call to RunLowerlevelSM in EV_Entry
processing in During function
02/20/07 21:37 jec converted to use enumerated type for events & states
02/13/05 19:38 jec added support for self-transitions, reworked
to eliminate repeated transition code
02/11/05 16:54 jec converted to implment hierarchy explicitly
02/25/03 10:32 jec converted to take a passed event parameter
02/18/99 10:19 jec built template from MasterMachine.c
02/14/99 10:34 jec Began Coding
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"

/* include header files for this state machine as well as any machines at the
next lower level in the hierarchy that are sub-machines to this machine
*/
#include "FaceOffSM.h"
#include "Beacon.h"
#include "ReloadSensor.h"
#include "DCMotor.h"
#include "ServoMotor.h"

/*----------------------------- Module Defines ----------------------------*/

// define constants for the states for this machine
// and any other local defines

#define ENTRY_STATE Drive4Offset

#define RED 1
#define BLUE 0

//Assume 1000 ticks per sec

#define ONE_SEC 1000

#define ROTATE_TIME 0.5*ONE_SEC //rotation time per time
#define D4O_TIME 2*ONE_SEC
#define D2R_TIME 3*ONE_SEC //drive to reload station time

//motor defines
#define Forward 0
#define Backward 1
#define CWTurn 2
#define CCWTurn 3
#define LeftMotor 0
#define RightMotor 1

//IRbeacon periods defines

#define TicksPerMS 40000 //use system clock
#define USperMS 1000
#define P_REDRELOAD 600 //red reload station period is 600 us
#define P_BLUERELOAD 500 //blue reload station period is 500 us
#define P_REDGOAL 800 //red attack goal period is 800 us
#define P_BLUEGOAL 700 //blue attack goal period is 700 us
#define P_THRES 25 //set period threshold to 25 us, need to tune

//position defines
#define P1X 930 //intermdeida point on way to reload station
#define P1Y 930
#define P1T 45

#define P1bX 1860 //intermdeida point on way to reload station

#define P1bY 930
#define P1bT 0

#define P2X 2250 //point right in front of reload station

#define P2Y 930
#define P2T 0

#define P2bX 2400

#define P2bY 930
#define P2bT 0

//#define P3X 1235 //dummy firing postion

//#define P3Y 930
//#define P3T 0
/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this machine, things like during
functions, entry & exit functions.They should be functions relevant to the
behavior of this state machine
*/
static ES_Event_t DuringDrive4Offset(ES_Event_t Event);
static ES_Event_t DuringTurn2Reload( ES_Event_t Event);
static ES_Event_t DuringDrive4Offset2(ES_Event_t Event);
static ES_Event_t DuringLook4Reload(ES_Event_t Event);
static ES_Event_t DuringDrive2Reload( ES_Event_t Event);
static ES_Event_t DuringLook4Emitter(ES_Event_t Event);
static ES_Event_t DuringDrive2Emitter(ES_Event_t Event);
static ES_Event_t DuringReload( ES_Event_t Event);

/*---------------------------- Module Variables ---------------------------*/

// everybody needs a state variable, you may need others as well
static FaceOffState_t CurrentState;
static uint16_t TEAM = 1; //set default team color to red
static uint32_t P_RELOAD= 0; //reload station period in ticks
static bool ReachedXGoal = false;
static bool ReachedYGoal = false;
//static bool moveback = false; //check to see if we moved back after we hit the wall
//static uint32_t P_IR_Ticks = 0;

/*------------------------------ Module Code ------------------------------*/

/****************************************************************************
Function
RunFaceOffSM

Parameters
ES_Event: the event to process

Returns
ES_Event: an event to return

Description
add your description here
Notes
uses nested switch/case to implement the machine.
Author
J. Edward Carryer, 2/11/05, 10:45AM
****************************************************************************/
ES_Event_t RunFaceOffSM( ES_Event_t CurrentEvent )
{
//set MakeTransition to false by default
bool MakeTransition = false;
//set NextState to CurrentState
FaceOffState_t NextState = CurrentState;
ES_Event_t EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new
state
ES_Event_t ReturnEvent = CurrentEvent; // assume we are not consuming event

switch ( CurrentState )
{
case Drive4Offset : // 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
ReturnEvent = CurrentEvent = DuringDrive4Offset(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case EV_X_TARGET_REACHED: //if event is EV_X_TARGET_REACHED
ReachedXGoal = true; //set ReachedXGoal to true
//if we have reached YGoal
if (ReachedYGoal == true)
{
#ifdef FACEOFF_DEBUG
printf("we have reached P1 \n\r");
 printRPM();
#endif
//stop robot
StopRobot();
NextState = Turn2Reload;//Decide the next state to be
Turn2Reload
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY_HISTORY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
}
break;

case EV_Y_TARGET_REACHED: //if event is EV_Y_TARGET_REACHED

ReachedYGoal = true; //set ReachedYGoal to true
//if we have reached XGoal
if (ReachedXGoal == true)
{
#ifdef FACEOFF_DEBUG
printf("we have reached P1 \n\r");
printRPM();
#endif
//stop robot
StopRobot();
NextState = Turn2Reload;//Decide the next state to be
Turn2Reload
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY_HISTORY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
}
break;
default:
;
// repeat cases as required for relevant events
}
}
break;
// repeat state pattern as required for other states

case Turn2Reload : // 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
ReturnEvent = CurrentEvent = DuringTurn2Reload(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case EV_ANGLE_TARGET_REACHED: //if event is EV_ANGLE_TARGET_REACHED
#ifdef FACEOFF_DEBUG
printf("we have turned 45 deg \n\r");
printRPM();
#endif

NextState = Drive4Offset2;//Decide next state to be Drive4Offset2

// for internal transitions, skip changing MakeTransition
 MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY_HISTORY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
break;
default:
;
}
}
break;
// repeat state pattern as required for other states
case Drive4Offset2 : // 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
ReturnEvent = CurrentEvent = DuringDrive4Offset2(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case EV_X_TARGET_REACHED: //if event is EV_X_TARGET_REACHED
ReachedXGoal = true; //set ReachedXGoal to be true
#ifdef FACEOFF_DEBUG
printf("we have reached P1b (1860, 930) \n\r");
printRPM();
#endif
NextState = Look4Reload;//Decide next state to be Look4Reload
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY_HISTORY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
break;
#ifdef FACEOFF_DEBUG
case ES_TIMEOUT:
printRPM();
ES_Timer_InitTimer(MOTOR_TIMER, 200);
NextState = Drive4Offset2;//Decide what the next state will be
// for internal transitions, skip changing MakeTransition
MakeTransition = false; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY_HISTORY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
break;
#endif
default:
;
}
}
break;

case Look4Reload : // 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
ReturnEvent = CurrentEvent = DuringLook4Reload(CurrentEvent);
//process any events
 if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case EV_BEACON_FOUND : //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
if((CurrentEvent.EventParam <=
(P_RELOAD+P_THRES))&&(CurrentEvent.EventParam >= (P_RELOAD-P_THRES))){
#ifdef FACEOFF_DEBUG
printf("we found the beacon\n\r");
#endif
//stop beacon capture interrupt and beacon oneshot interrupt
StopBeaconCapture();
StopBeaconOneShot();
NextState = Drive2Reload;//Decide next state to be Drive2Reload
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition

}else{
//set NextState to Look4Reload, keep looking for beacon
NextState = Look4Reload;
//set MakeTransition to false
MakeTransition = false;
}
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY_HISTORY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
break;
// repeat cases as required for relevant events
case EV_BEACON_NOTFOUND: //if event is EV_BEACON_NOTFOUND
//perform dummy searching, rotate a small angle per time
#ifdef FACEOFF_DEBUG
printf("we didn't find the beacon\n\r");
#endif
//set robot to turn CW at some speed
SetRobotDir(CWTurn);
SetRobotSpeed(LeftMotor, 8);
SetRobotSpeed(RightMotor, 10);
//start a timer for rotating
ES_Timer_InitTimer(MOTOR_TIMER, ROTATE_TIME);
NextState = Look4Reload; //set NextState to Look4Reload
MakeTransition = false; //set MakeTransition to false
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY_HISTORY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
break;
// repeat cases as required for relevant events
case ES_TIMEOUT: //if event is ES_TIMEOUT
StartBeaconOneShot(); //restart oneshot after turning a fixed angle
to determine if we find the beacon
NextState = Look4Reload; //set NextState to Look4Reload
MakeTransition = false; //set MakeTransition to false
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY_HISTORY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
break;
 default:
;
}
}
break;
// repeat state pattern as required for other states

case Drive2Reload : // 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
ReturnEvent = CurrentEvent = DuringDrive2Reload(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case EV_X_TARGET_REACHED: //if event is EV_X_TARGET_REACHED
#ifdef FACEOFF_DEBUG
printf("we have reached relaod station \n\r");
printRPM();
#endif
NextState = Look4Emitter;//Decide the next state to be Look4Emitter
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY_HISTORY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
break;
default:
;
}
}
break;
// repeat state pattern as required for other states
case Look4Emitter : // 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
ReturnEvent = CurrentEvent = DuringLook4Emitter(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case EV_EMITTER_DETECTED : //If event is EV_EMITTER_DETECTED
// Execute action function for Look4Emitter, EV_EMITTER_DETECTED

#ifdef FACEOFF_DEBUG
printf("we find the IR emitter \n\r");
printf("the period of emitter is %d\n\r", CurrentEvent.EventParam);
#endif
//stop reload capture and one shot
StopReloadCapture();
StopReloadOneShot();

NextState = Drive2Emitter;//Decide what next state to be

Drive2Emitter
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY_HISTORY;
 // optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
break;
// repeat cases as required for relevant events
case EV_EMITTER_MISSED: //if event is EV_EMITTER_MISSED
//perform dummy searching, rotate a small angle per time
#ifdef FACEOFF_DEBUG
printf("we didn't find the emitter\n\r");
#endif
//set robot to turn CW at some speed
SetRobotDir(CWTurn);
SetRobotSpeed(LeftMotor, 10);
SetRobotSpeed(RightMotor, 10);
//start a timer for turning
ES_Timer_InitTimer(MOTOR_TIMER, ROTATE_TIME);
=======
SetRobotDir(CWTurn);
SetRobotSpeed(LeftMotor, 8);
SetRobotSpeed(RightMotor, 10);
ES_Timer_InitTimer(MOTOR_TIMER, ROTATE_TIME);

//Set NextState to be Look4Emitter

NextState = Look4Emitter;
MakeTransition = false; //mark that we are not making a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY_HISTORY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
break;
// repeat cases as required for relevant events
break;
case ES_TIMEOUT: //if event is ES_TIMEOUT

StartReloadOneShot(); //restart oneshot after turning a fixed angle

to determine if we find the beacon
NextState = Look4Emitter; //set NextState to Look4Emitter
MakeTransition = false; //mark that we are not making a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY_HISTORY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
break;
default:;
}

default:
;
}
break;
// repeat state pattern as required for other states

case Drive2Emitter: //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
ReturnEvent = CurrentEvent = DuringDrive2Emitter(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
 case EV_X_TARGET_REACHED: //if event is EV_X_TARGET_REACHED
#ifdef FACEOFF_DEBUG
printf("we have reached P2b (2350, 930) \n\r");
printRPM();
#endif
StopRobot(); //stop robot
NextState = Reload;//Decide the next state to be Reload
// for internal transitions, skip changing MakeTransition
MakeTransition = true; //mark that we are taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY_HISTORY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
break;
default:;
}
}
break;

case Reload : // 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
ReturnEvent = CurrentEvent = DuringReload(CurrentEvent);
//process any events
if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_ENTRY : //If event is event one
// Execute action function for Reload,ES_ENTRY
#ifdef FACEOFF_DEBUG
printf("we are reloading\n\r");
#endif
NextState = Reload;//Decide next state to be Reload
// for internal transitions, skip changing MakeTransition
MakeTransition = false; //mark that we are not taking a transition
// if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY_HISTORY;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
break;
// repeat cases as required for relevant events
default:
;
}
}
break;
}
// If we are making a state transition
if (MakeTransition == true)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunFaceOffSM(CurrentEvent);

CurrentState = NextState; //Modify state variable

// Execute entry function for new state

// this defaults to ES_ENTRY
RunFaceOffSM(EntryEventKind);
 }
return(ReturnEvent);
}
/****************************************************************************
Function
StartFaceOffSM

Parameters
None

Returns
None

Description
Does any required initialization for this state machine
Notes

Author
J. Edward Carryer, 2/18/99, 10:38AM
****************************************************************************/
void StartFaceOffSM ( ES_Event_t CurrentEvent )
{
// to implement entry to a history state or directly to a substate
// you can modify the initialization of the CurrentState variable
// otherwise just start in the entry state every time the state machine
// is started
//set CurrentState to be ENTRY_STATE
CurrentState = ENTRY_STATE;
//grab team info from passed-in event
TEAM = CurrentEvent.EventParam; //grab TEAM color info
#ifdef FACEOFF_DEBUG
printf("in face off, we are TEAM %d\n\r", TEAM);
#endif
//set reload station period info according to TEAM
if(TEAM == RED) P_RELOAD = P_REDRELOAD;
else P_RELOAD = P_BLUERELOAD;

// call the entry function (if any) for the ENTRY_STATE

RunFaceOffSM(CurrentEvent);
}

/****************************************************************************
Function
QueryFaceOffSM

Parameters
None

Returns
TemplateState_t The current state of the Template state machine

Description
returns the current state of the Template state machine
Notes

Author
J. Edward Carryer, 2/11/05, 10:38AM
****************************************************************************/
FaceOffState_t QueryFaceOffSM ( void )
{
return(CurrentState);
}

/***************************************************************************
 private functions
***************************************************************************/
static ES_Event_t DuringDrive4Offset( ES_Event_t Event)
{
ES_Event_t ReturnEvent = Event; // assume no re-mapping or consumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
{
//implement any entry actions required for this state machine
//upon entering this state, we start to drive the robot forward at some speed
SetRobotDir(Forward);
SetRobotSpeed(LeftMotor, 13);
SetRobotSpeed(RightMotor, 15);
//set target position (P1X, P1Y)
SetTargetPosition(P1X, P1Y);
//reset ReachedXGoal and ReachedYGoal to false
ReachedXGoal = false;
ReachedYGoal = false;
// after that start any lower level machines that run in this state
//StartLowerLevelSM( Event );
// repeat the StartxxxSM() functions for concurrent state machines
// on the lower level
}
else if ( Event.EventType == ES_EXIT )
{
// on exit, give the lower levels a chance to clean up first
//RunLowerLevelSM(Event);
// repeat for any concurrently running state machines
// now do any local exit functionality

}else
// do the 'during' function for this state
{
// run any lower level state machine
// ReturnEvent = RunLowerLevelSM(Event);

// repeat for any concurrent lower level machines

// do any activity that is repeated as long as we are in this state

}
// 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.
return(ReturnEvent);
}

static ES_Event_t DuringTurn2Reload( ES_Event_t Event)

{
ES_Event_t ReturnEvent = Event; // assume no re-mapping or consumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
{
// implement any entry actions required for this state machine
//entry Look4Reload state by starting capture and oneshot

//set robot to turn CW at some speed

SetRobotDir(CWTurn);
SetRobotSpeed(LeftMotor, 8);
SetRobotSpeed(RightMotor, 10);
//Set target heading
SetTargetHeading(P2T);
 // after that start any lower level machines that run in this state
//StartLowerLevelSM( Event );
// repeat the StartxxxSM() functions for concurrent state machines
// on the lower level
}
else if ( Event.EventType == ES_EXIT )
{
// on exit, give the lower levels a chance to clean up first
//RunLowerLevelSM(Event);
// repeat for any concurrently running state machines
// now do any local exit functionality

}else
// do the 'during' function for this state
{
// run any lower level state machine
// ReturnEvent = RunLowerLevelSM(Event);

// repeat for any concurrent lower level machines

// do any activity that is repeated as long as we are in this state

}
// 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.
return(ReturnEvent);
}

static ES_Event_t DuringDrive4Offset2( ES_Event_t Event)

{
ES_Event_t ReturnEvent = Event; // assume no re-mapping or consumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
{
// implement any entry actions required for this state machine
//upon entering this state, we start to drive the robot forward at some speed
SetRobotDir(Forward);
SetRobotSpeed(LeftMotor, 8);
SetRobotSpeed(RightMotor, 10);
//set target position (P1bX, P1bY)
SetTargetPosition(P1bX, P1bY);
//reset ReachedXGoal and ReachedYGoal to be false
ReachedXGoal = false;
ReachedYGoal = false;

#ifdef FACEOFF_DEBUG
printRPM();
ES_Timer_InitTimer(MOTOR_TIMER, 200);
#endif
// after that start any lower level machines that run in this state
//StartLowerLevelSM( Event );
// repeat the StartxxxSM() functions for concurrent state machines
// on the lower level
}
else if ( Event.EventType == ES_EXIT )
{
// on exit, give the lower levels a chance to clean up first
//RunLowerLevelSM(Event);
// repeat for any concurrently running state machines
// now do any local exit functionality

}else
// do the 'during' function for this state
 {
// run any lower level state machine
// ReturnEvent = RunLowerLevelSM(Event);

// repeat for any concurrent lower level machines

// do any activity that is repeated as long as we are in this state

}
// 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.
return(ReturnEvent);
}

static ES_Event_t DuringLook4Reload(ES_Event_t Event){

ES_Event_t ReturnEvent = Event; // assume no re-mapping or consumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
{
// implement any entry actions required for this state machine
//entry Look4Reload state by starting capture and oneshot
printf("we are in Look4 reload station state\n\r");
//init and start beacon capture and beacon oneshot
InitBeaconEdgeCapturePeriod();
InitBeaconOneShot();
StartBeaconCapture();
StartBeaconOneShot();

// after that start any lower level machines that run in this state
//StartLowerLevelSM( Event );
// repeat the StartxxxSM() functions for concurrent state machines
// on the lower level
}
else if ( Event.EventType == ES_EXIT )
{
// on exit, give the lower levels a chance to clean up first
//RunLowerLevelSM(Event);
// repeat for any concurrently running state machines
// now do any local exit functionality

}else
// do the 'during' function for this state
{
// run any lower level state machine
// ReturnEvent = RunLowerLevelSM(Event);

// repeat for any concurrent lower level machines

// do any activity that is repeated as long as we are in this state

}
// 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.
return(ReturnEvent);
}

static ES_Event_t DuringDrive2Reload( ES_Event_t Event)

{
ES_Event_t ReturnEvent = Event; // assume no re-mapping or consumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
 {
// implement any entry actions required for this state machine
//upon leaving this state, we start a timer and start to drive the robot
#ifdef FACEOFF_DEBUG
printf("we are driving to reload station\n\r");
#endif
//set robot to run forward at some speed
SetRobotDir(Forward);
<<<<<<< HEAD
SetRobotSpeed(LeftMotor, 14);
SetRobotSpeed(RightMotor, 14);
//set target position (P2X, P2Y)
=======
SetRobotSpeed(LeftMotor, 13);
SetRobotSpeed(RightMotor, 15);
SetTargetPosition(P2X, P2Y);
//ReachedXGoal = false;
// ReachedYGoal = false;
// ES_Timer_InitTimer(MOTOR_TIMER, 500);
// ES_Timer_InitTimer(MOTOR_TIMER, D2R_TIME);
// after that start any lower level machines that run in this state
//StartLowerLevelSM( Event );
// repeat the StartxxxSM() functions for concurrent state machines
// on the lower level
}
else if ( Event.EventType == ES_EXIT )
{
// on exit, give the lower levels a chance to clean up first
//RunLowerLevelSM(Event);
// repeat for any concurrently running state machines
// now do any local exit functionality

}else
// do the 'during' function for this state
{
// run any lower level state machine
// ReturnEvent = RunLowerLevelSM(Event);

// repeat for any concurrent lower level machines

// do any activity that is repeated as long as we are in this state

#ifdef FACEOFF_DEBUG
printRPM();
#endif
}
// 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.
return(ReturnEvent);
}

static ES_Event_t DuringLook4Emitter(ES_Event_t Event){

ES_Event_t ReturnEvent = Event; // assume no re-mapping or consumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
{
// implement any entry actions required for this state machine
//entry Look4Reload state by starting capture and oneshot
printf("we are in Look4Emitter state\n\r");
//init and start reload capture and reload oneshot
InitReloadCapture();
InitReloadOneShot();
StartReloadCapture();
 StartReloadOneShot();
// after that start any lower level machines that run in this state
//StartLowerLevelSM( Event );
// repeat the StartxxxSM() functions for concurrent state machines
// on the lower level
}
else if ( Event.EventType == ES_EXIT )
{
// on exit, give the lower levels a chance to clean up first
//RunLowerLevelSM(Event);
// repeat for any concurrently running state machines
// now do any local exit functionality
//moveback = false;

}else
// do the 'during' function for this state
{
// run any lower level state machine
// ReturnEvent = RunLowerLevelSM(Event);

// repeat for any concurrent lower level machines

// do any activity that is repeated as long as we are in this state

}
// 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.
return(ReturnEvent);
}

static ES_Event_t DuringDrive2Emitter( ES_Event_t Event)

{
ES_Event_t ReturnEvent = Event; // assume no re-mapping or consumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
{
// implement any entry actions required for this state machine
//upon entering this state, we start to drive the robot forward at some speed
SetRobotDir(Forward);
SetRobotSpeed(LeftMotor, 13);
SetRobotSpeed(RightMotor, 15);
//set target position (P2bX, P2bY)
SetTargetPosition(P2bX, P2bY);
// after that start any lower level machines that run in this state
//StartLowerLevelSM( Event );
// repeat the StartxxxSM() functions for concurrent state machines
// on the lower level
}
else if ( Event.EventType == ES_EXIT )
{
// on exit, give the lower levels a chance to clean up first
//RunLowerLevelSM(Event);
// repeat for any concurrently running state machines
// now do any local exit functionality

}else
// do the 'during' function for this state
{
// run any lower level state machine
// ReturnEvent = RunLowerLevelSM(Event);

// repeat for any concurrent lower level machines

 // do any activity that is repeated as long as we are in this state
}
// 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.
return(ReturnEvent);
}

static ES_Event_t DuringReload( ES_Event_t Event)

{
ES_Event_t ReturnEvent = Event; // assume no re-mapping or consumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ( (Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY) )
{
// implement any entry actions required for this state machine
//init IR PWM
InitIRPWM();
//set servo motors to LoadBall
LoadBall();
// after that start any lower level machines that run in this state
//StartLowerLevelSM( Event );
// repeat the StartxxxSM() functions for concurrent state machines
// on the lower level
}
else if ( Event.EventType == ES_EXIT )
{
// on exit, give the lower levels a chance to clean up first
//RunLowerLevelSM(Event);
// repeat for any concurrently running state machines
// now do any local exit functionality

}else
// do the 'during' function for this state
{
// run any lower level state machine
// ReturnEvent = RunLowerLevelSM(Event);

// repeat for any concurrent lower level machines

// do any activity that is repeated as long as we are in this state

}
// 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.
return(ReturnEvent);
}