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

Module
HSMTemplate.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 "Unloading.h"
#include "MotorService.h"
#include "HSMTemplate.h"
#include "MasterHSM.h"
#include "Loading.h"
#include "Beacon.h"
#include "TopHSMTemplate.h"
#include "PWMService.h"
#include "TemplateFSM.h"
/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines

#define ENTRY_STATE GoToBeacon

#define ActionTriggeringTimer 5
#define RESPONSE_TIME 200
#define FIVE_SECS 5000
#define UTURNTIMER 5000
#define MASTER_Timer 0
#define CRASH_TIMER 3
#define RESPONSE_TIME 200
#define South 700
#define North 800
/*---------------------------- 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 DuringGoToBeacon(ES_Event_t Event);
static ES_Event_t DuringGetTape(ES_Event_t Event);
static ES_Event_t DuringGoToRecycling(ES_Event_t Event);
static ES_Event_t DuringActivateRecycling(ES_Event_t Event);
static ES_Event_t DuringOppositeRecycling(ES_Event_t Event);
static ES_Event_t DuringOpenGate(ES_Event_t Event);
static ES_Event_t DuringCloseGate(ES_Event_t Event);

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

// everybody needs a state variable, you may need others as well
static UnloadState_t CurrentState;
static uint8_t RCYState = 255;
static uint8_t Rcy;//West is zero and east is 1
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
Function
RunUnloadSM

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 RunUnloadingSM(ES_Event_t CurrentEvent)
{
bool MakeTransition = false; /* are we making a state transition? */
UnloadState_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 GoToBeacon: // If current state is state one
{ // 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 = DuringGoToBeacon(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_OPPONENTBEACONDETECTED: //If event is event one
{ // Execute action function for state one
: event one
NextState = GetTape; //Decide what the next state will be
// 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
}
}
}
break;

case GetTape: // If current state is state one

{ // 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 = DuringGetTape(CurrentEvent);
printf("Unloading in GetTape");
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TAPE: //If event is event one
{ // Execute action function for state one :
event one
NextState = GoToRecycling; //Decide what the next state will be
// 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
}
}
}
break;

case GoToRecycling: // If current state is state one

{ // 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 = DuringGoToRecycling(CurrentEvent);
printf("Game in GoToRecycling");
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_BACKCRASH: //If event is event one
{ // Execute action function for state
one : event one
NextState = ActivateRecycling; //Decide what the next state will be
printf("Activating Recycling");
// 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
}
}
}
break;

case ActivateRecycling: // If current state is state one

{ // 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 = DuringActivateRecycling(CurrentEvent);
printf("Game in activate recycling");
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT: //If event is event one
{ // Execute action function for state one : event
one
NextState = OpenGate; //Decide what the next state will be
// 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 ES_GOBACK: //If event is event one

{ // Execute action function for state
one : event one
NextState = OppositeRecycling; //Decide what the next state will be
// 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
}
}
}
break;

/* case OppositeRecycling: // If current state is state one

{ // 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 = DuringOppositeRecycling(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_BACKCRASH: //If event is event one
{ // Execute action function for state
one : event one
NextState = ActivateRecycling; //Decide what the next state will be
// 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
}
}
}break;*/

case OpenGate: // If current state is state one

{ // 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 = DuringOpenGate(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT: //If event is event one
{ // Execute action function for state one : event
one
NextState = CloseGate; //Decide what the next state will be
// 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
}
}
}
break;

case CloseGate: // If current state is state one

{ // 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 = DuringCloseGate(CurrentEvent);
CurrentState = GoToBeacon;
ES_Timer_InitTimer(MASTER_Timer, RESPONSE_TIME);
MakeTransition = false;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
}
break;
// repeat state pattern as required for other states
}
// If we are making a state transition

if (MakeTransition == true)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunUnloadingSM(CurrentEvent);

CurrentState = NextState; //Modify state variable

// Execute entry function for new state

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

/****************************************************************************
Function
StartUnloadSM

Parameters
None

Returns
None

Description
Does any required initialization for this state machine
Notes

Author
J. Edward Carryer, 2/18/99, 10:38AM
****************************************************************************/
void StartUnloadSM(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
//if (ES_ENTRY_HISTORY != CurrentEvent.EventType)
 //Not Using History Function
{
CurrentState = ENTRY_STATE;
}
// call the entry function (if any) for the ENTRY_STATE
RunUnloadingSM(CurrentEvent);
}

/****************************************************************************
Function
QueryUnloadSM

Parameters
None

Returns
UnloadState_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
****************************************************************************/
UnloadState_t QueryUnloadSM(void)
{
return CurrentState;
}

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

static ES_Event_t DuringGoToBeacon(ES_Event_t Event)

{
ES_Event_t ReturnEvent = Event; // assume no re-mapping or consumption
ES_Event_t MotorEvent;
printf("\n\r******************************GetBeacon");

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
//Check beacon value to be detected and start detecting opponent beacon
if (QueryTeam())
{
MotorEvent.EventParam = North;
}
else
{
MotorEvent.EventParam = South;
}
MotorEvent.EventType = ES_GETOPPONENTBEACON;
PostMotor(MotorEvent);
PostBeaconService(MotorEvent);
}
else if (Event.EventType == ES_EXIT)
{}
else
// do the 'during' function for this state
{}

return ReturnEvent;
}

static ES_Event_t DuringGetTape(ES_Event_t Event)

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

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
//Ask the motor to get tape
printf("\n\r******************************GetTape");
MotorEvent.EventType = ES_GETTAPE;
//PostTapeFSM(MotorEvent);
PostMotor(MotorEvent);
}
else if (Event.EventType == ES_EXIT)
{}
else
// do the 'during' function for this state
{}

return ReturnEvent;
}

static ES_Event_t DuringGoToRecycling(ES_Event_t Event)

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

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
MotorEvent.EventType = ES_HOLD;
PostMotor(MotorEvent);
printf( "\n\r******************************GoToRecycling");
printf( "\n\r******************************Query Team is %d and Query East is
%d, Query west is %d", QueryTeam(), QueryEast(), QueryWest());
//based on team and recycling centre to go to, start navigating and set IR emitter
for required frequency
if (QueryTeam() && QueryEast())
{
MotorEvent.EventType = ES_FOLLOWLINE_CCW_R;
SetFrequency(200);
RCYState = 0;
Rcy = 1;
}
if (QueryTeam() && QueryWest())
{
MotorEvent.EventType = ES_FOLLOWLINE_CW_R;
printf("\n\rPosting Followline CW");
SetFrequency(1000);
RCYState = 1;
Rcy = 0;
}
if ((!QueryTeam()) && QueryEast())
{
MotorEvent.EventType = ES_FOLLOWLINE_CW_R;
SetFrequency(200);
printf("\n\rPosting Followline CW");
Rcy = 1;
RCYState = 1;
 }
if ((!QueryTeam()) && QueryWest())
{
MotorEvent.EventType = ES_FOLLOWLINE_CCW_R;
printf("\n\rPosting Followline CCW");
SetFrequency(1000);
Rcy = 0;
RCYState = 0;
}
else
{
MotorEvent.EventType = ES_FOLLOWLINE_CW_R;
if (QueryTeam())
{
SetFrequency(1000);
}
else
{
SetFrequency(200);
}
printf("\n\rPosting Followline CCW - No Recycling Centre Found, Far
Recycling");
Rcy = 0;
RCYState = 1;
}

PostMotor(MotorEvent);
}
else if (Event.EventType == ES_EXIT)
{}
else
// do the 'during' function for this state
{}

return ReturnEvent;
}

static ES_Event_t DuringActivateRecycling(ES_Event_t Event)

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

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
//based on team and recycling, tune IR emitter frequency
printf("\n\r******************************Activate Recycling");
MotorEvent.EventType = ES_FULLFRONT;
PostMotor(MotorEvent);
if (Rcy && QueryEast())
{
SetFrequency(200);
SetDutyPE4(50);
printf("\n\r\n\rRcy is %d", Rcy);
ES_Timer_InitTimer(ActionTriggeringTimer, 200);//process any events
}
else if ((!Rcy) && QueryWest())
{
SetFrequency(1000);
SetDutyPE4(50);
ES_Timer_InitTimer(ActionTriggeringTimer, 200);//process any events
}
else
 {
/* if(QueryTeam())
// SetFrequency(600);
else
// SetFrequency(500);
SetDutyPE4(50);*/
printf("\n\r\n\rRcy is %d", Rcy);

ES_Timer_InitTimer(ActionTriggeringTimer, 200);//process any events

//ReturnEvent.EventType = ES_GOBACK;
}
}
else if (Event.EventType == ES_EXIT)
{}
else
// do the 'during' function for this state
{}

return ReturnEvent;
}

/*static ES_Event_t DuringOppositeRecycling(ES_Event_t Event)

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

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
//if we have to go to the other recycling centre, take Uturn and follow line
printf("\n\r******************************Opposite Recycling");
MotorEvent.EventType = ES_UTURN;
PostMotor(MotorEvent);
}
else if (Event.EventType == ES_EXIT)
{}
else
// do the 'during' function for this state
{
if (Event.EventType == ES_UTURNCOMPLETE)
{
MotorEvent.EventType = ES_FOLLOWLINE;
PostMotor(MotorEvent);
}
}

return ReturnEvent;
}*/

static ES_Event_t DuringOpenGate(ES_Event_t Event)

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

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
//Stop the motor and open the servos
MotorEvent.EventType = ES_HOLD;
PostMotor(MotorEvent);
SetDutyPB6(2);
SetDutyPB7(12);
 ES_Timer_InitTimer(ActionTriggeringTimer, 7000);//process any events
}
else if (Event.EventType == ES_EXIT)
{}
else
// do the 'during' function for this state
{}

return ReturnEvent;
}

static ES_Event_t DuringCloseGate(ES_Event_t Event)

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

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
//close the servos
SetDutyPB6(5);
SetDutyPB7(5);
}
else if (Event.EventType == ES_EXIT)
{}
else
// do the 'during' function for this state
{}

return ReturnEvent;
}

uint8_t QueryNearFar(void)
{
return RCYState;
}