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

Module
NavToGoalShot.c

****************************************************************************/
/*----------------------------- 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 "NavToGoalShot.h"
#include "AligningToBeacon.h"
#include "MotorDrive.h"
#include "SquaringToWall.h"
#include "MasterSM.h"
/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines
//TODO: SWITCH BACK!!!!!!!
#define ENTRY_STATE BACKING_UP_SLIGHTLY_GOAL_SHOT

#define BACK_UP_RPM (30)

#define BACK_UP_DIST1 (6)

#define BACK_UP_DIST2 (8)
#define STOP_TIME (200)

#define TURN_RIGHT_RPM (23)

#define TURN_RIGHT_COMPENSATE_ANGLE (2)
#define DRIVE_FORWARD_RPM_FAST (70)
#define DRIVE_FORWARD_RPM_SLOW (53)
/*---------------------------- 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 DuringBackingUpSlightlyGoal(ES_Event_t Event);
static ES_Event_t DuringAligningGoal(ES_Event_t Event);
static ES_Event_t DuringTurningRightGoal(ES_Event_t Event);
static ES_Event_t DuringDriveForwardGoal(ES_Event_t Event);
static ES_Event_t DuringSquaringToWallGoal(ES_Event_t Event);
static ES_Event_t DuringBackingOffGoal(ES_Event_t Event);
static ES_Event_t DuringAligningToOppGoal(ES_Event_t Event);
static ES_Event_t DuringTurningRightCompensate(ES_Event_t Event);

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

// everybody needs a state variable, you may need others as well
static NavToGoalShotState_t CurrentState;
static bool DefenseFlag = false;
static bool FirstOffenseFlag = false;
static uint16_t TurnRightAngles[2] = { 16, 16 };
static uint16_t DriveForwardDist[2] = { 50, 44 };

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

/****************************************************************************
Function
RunTemplateSM

Parameters
ES_Event_t: the event to process
 Returns
ES_Event_t: 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 RunNavToGoalShot(ES_Event_t CurrentEvent)
{
bool MakeTransition = false;/* are we making a state transition?
*/
NavToGoalShotState_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 BACKING_UP_SLIGHTLY_GOAL_SHOT: //use encoders to drive forward.
{ ReturnEvent = CurrentEvent = DuringBackingUpSlightlyGoal(CurrentEvent);

if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active

{
//when we reach encoder limit, build in a delay before we start peeling.
if (CurrentEvent.EventType == ENCODER_LIMIT_REACHED)
{
printf("we got a movement timer while in backing off.!\r\n");
StopDrive();
ES_Timer_InitTimer(MOVEMENT_TIMER, STOP_TIME);
ReturnEvent.EventType = ES_NO_EVENT;
}
//when the delay is over, we want to transition to peeling off.
else if ((CurrentEvent.EventType == ES_TIMEOUT) && (CurrentEvent.EventParam
== MOVEMENT_TIMER))
{
ReturnEvent.EventType = ES_NO_EVENT;
MakeTransition = true;
NextState = ALIGNING_GOAL_SHOT;
}
}
}
break;

case ALIGNING_GOAL_SHOT: //first look for our home beacon

{ ReturnEvent = CurrentEvent = DuringAligningGoal(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ALIGNED_TO_BEACON: //If event is event one
{ //now we will drive forward.
NextState = TURNING_RIGHT_GOAL_SHOT;
MakeTransition = true; //mark that we are taking a transition
ReturnEvent.EventType = ES_NO_EVENT;
}
break;
}
}
}
 break;

case TURNING_RIGHT_GOAL_SHOT: //first look for our home beacon

{ ReturnEvent = CurrentEvent = DuringTurningRightGoal(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
if (CurrentEvent.EventType == ENCODER_LIMIT_REACHED)
{
StopDrive();
ES_Timer_InitTimer(MOVEMENT_TIMER, STOP_TIME);
ReturnEvent.EventType = ES_NO_EVENT;
}
//when the delay is over, we want to transition to peeling off.
else if ((CurrentEvent.EventType == ES_TIMEOUT) && (CurrentEvent.EventParam
== MOVEMENT_TIMER))
{
ReturnEvent.EventType = ES_NO_EVENT;
MakeTransition = true;
NextState = DRIVING_FORWARD_GOAL_SHOT;
}
}
}
break;

case DRIVING_FORWARD_GOAL_SHOT: //use encoders to drive forward.

{ ReturnEvent = CurrentEvent = DuringDriveForwardGoal(CurrentEvent);

if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active

{
if ((CurrentEvent.EventType == ENCODER_LIMIT_REACHED) ||
(CurrentEvent.EventType == LEFT_BUMPER_HIT) || (CurrentEvent.EventType ==
RIGHT_BUMPER_HIT))
{
//DriveForwardStraight(DRIVE_FORWARD_RPM_SLOW, false, 0);
StopDrive();
ES_Timer_InitTimer(MOVEMENT_TIMER, STOP_TIME);
ReturnEvent.EventType = ES_NO_EVENT;
}
else if ((CurrentEvent.EventType == ES_TIMEOUT) && (CurrentEvent.EventParam
== MOVEMENT_TIMER))
{
if (DefenseFlag)
{
ReturnEvent.EventType = DONE_WITH_NAV_TO_DEFENSE;
MakeTransition = false;
}
else
{
ReturnEvent.EventType = ES_NO_EVENT;
MakeTransition = true;
NextState = ALIGNING_TO_OPP_GOAL_SHOT;
}
}
}
}
break;
case ALIGNING_TO_OPP_GOAL_SHOT: //use encoders to drive forward.
{ ReturnEvent = CurrentEvent = DuringAligningToOppGoal(CurrentEvent);

if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active

{
if (CurrentEvent.EventType == ALIGNED_TO_BEACON)
{
 //ReturnEvent.EventType = READY_TO_SHOOT;
NextState = TURNING_RIGHT_COMPENSATE;
MakeTransition = true;
}
}
}
break;

case TURNING_RIGHT_COMPENSATE: //Turning Right to compensate after aligning

to opponent goal
{ ReturnEvent = CurrentEvent = DuringTurningRightCompensate(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
if (CurrentEvent.EventType == ENCODER_LIMIT_REACHED)
{
ReturnEvent.EventType = READY_TO_SHOOT;
}
}
}
break;
}
// If we are making a state transition
if (MakeTransition == true)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunNavToGoalShot(CurrentEvent);

CurrentState = NextState; //Modify state variable

// Execute entry function for new state

// this defaults to ES_ENTRY
RunNavToGoalShot(EntryEventKind);
}
return ReturnEvent;
}

/****************************************************************************
Function
StartTemplateSM

Parameters
None

Returns
None

Description
Does any required initialization for this state machine
Notes

Author
J. Edward Carryer, 2/18/99, 10:38AM
****************************************************************************/
void StartNavToGoalShot(ES_Event_t CurrentEvent)
{
if (CurrentEvent.EventType != ES_ENTRY)
{
printf("Warning: Defense state machine was started with a non-entry
event.\r\n");
}

if (ES_ENTRY_HISTORY != CurrentEvent.EventType)
 {
CurrentState = ENTRY_STATE;
}
// call the entry function (if any) for the ENTRY_STATE
RunNavToGoalShot(CurrentEvent);
}

/****************************************************************************
Function
QueryTemplateSM

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
****************************************************************************/
NavToGoalShotState_t QueryNavToGoalShot(void)
{
return CurrentState;
}

/***************************************************************************
private functions
***************************************************************************/

static ES_Event_t DuringBackingUpSlightlyGoal(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))
{
DriveBackwardStraight(BACK_UP_RPM, true, BACK_UP_DIST1);
}
else if (Event.EventType == ES_EXIT)
{
StopDrive();
}
else
{}

return ReturnEvent;
}

static ES_Event_t DuringAligningGoal(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))
{
SetAligningDirection(ALIGN_CW);
Event.EventParam = OUR_GOAL;
StartAligningToBeacon(Event);
 }
else if (Event.EventType == ES_EXIT)
{
RunAligningToBeacon(Event);
}
else
{
// run any lower level state machine
ReturnEvent = RunAligningToBeacon(Event);
}
return ReturnEvent;
}

static ES_Event_t DuringTurningRightGoal(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))
{
DriveCWStraight(TURN_RIGHT_RPM, true, TurnRightAngles[QueryDefenseStrat()]);
}
else if (Event.EventType == ES_EXIT)
{
StopDrive();
}
else
{}
return ReturnEvent;
}

static ES_Event_t DuringDriveForwardGoal(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))
{
DriveForwardStraight(DRIVE_FORWARD_RPM_FAST, true,
DriveForwardDist[QueryDefenseStrat()]);
}
else if (Event.EventType == ES_EXIT)
{
StopDrive();
}
else
{}
return ReturnEvent;
}

static ES_Event_t DuringSquaringToWallGoal(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))
{
StartSquaringToWall(Event);
}
else if (Event.EventType == ES_EXIT)
{
 RunSquaringToWall(Event);
}
else
{
ReturnEvent = RunSquaringToWall(Event);
}
return ReturnEvent;
}

static ES_Event_t DuringBackingOffGoal(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))
{
DriveBackwardStraight(BACK_UP_RPM, true, BACK_UP_DIST2);
}
else if (Event.EventType == ES_EXIT)
{
StopDrive();
}
else
{}

return ReturnEvent;
}

static ES_Event_t DuringAligningToOppGoal(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))
{
SetAligningDirection(ALIGN_CCW);
Event.EventParam = THEIR_GOAL;
StartAligningToBeacon(Event);
}
else if (Event.EventType == ES_EXIT)
{
RunAligningToBeacon(Event);
}
else
{
ReturnEvent = RunAligningToBeacon(Event);
}

return ReturnEvent;
}

static ES_Event_t DuringTurningRightCompensate(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))
{
DriveCWStraight(TURN_RIGHT_RPM, true, TURN_RIGHT_COMPENSATE_ANGLE);
}
else if (Event.EventType == ES_EXIT)
 {
StopDrive();
}
else
{}

return ReturnEvent;
}

void SetDefenseFlag(bool status)

{
DefenseFlag = status;
}

void SetFirstOffenseFlag(bool status)

{
FirstOffenseFlag = status;
}