You are on page 1of 6

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

Module
AligningToBeacon.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 "AligningToBeacon.h"
#include "BeaconIRInputCaptureModule.h"
#include "MotorDrive.h"
#include "MasterSM.h"
/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines

#define ENTRY_STATE STOPPING


#define COMPENSATE_TIME_MS (200)
#define COMPENSATE_SPEED (30)
#define COMPENSATE_ANGLE (2)
#define ALIGNING_SPEED (23)
#define STOPPING_TIME (300)

/*---------------------------- 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 DuringStopping(ES_Event_t Event);
static ES_Event_t DuringRotating(ES_Event_t Event);
static ES_Event_t DuringCompensating(ES_Event_t Event);

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


// everybody needs a state variable, you may need others as well
static AligningToBeaconState_t CurrentState;

static uint32_t PeriodArray[2][4] = { { 600, 700, 500, 800 }, { 500,


800, 600, 700 } };

static uint8_t Direction = 0;

/*------------------------------ 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 RunAligningToBeacon(ES_Event_t CurrentEvent)
{
bool MakeTransition = false;/* are we making a state
transition? */
AligningToBeaconState_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 STOPPING:
{
ReturnEvent = CurrentEvent = DuringStopping(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
if ((CurrentEvent.EventType == ES_TIMEOUT) && (CurrentEvent.EventParam ==
MOVEMENT_TIMER))
{
NextState = ROTATING;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;
}
}
}
break;
case ROTATING: // 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 = DuringRotating(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case FOUND_IR: //If we found IR!
{ //ES_Timer_InitTimer(MOVEMENT_TIMER,STOPPING_TIME);
//StopDrive();
//diable interrupts for IR
DisableBeaconIR();
//break;
//case ES_TIMEOUT : //
//if(CurrentEvent.EventParam == MOVEMENT_TIMER)
//{
NextState = COMPENSATING;
MakeTransition = true;

//tell the state machine above that we're aligned


ReturnEvent.EventType = ES_NO_EVENT;
//don't worry about making a transition, as the state machine above
should stop aligning.
// }
}
break;
}
}
}
break;

case COMPENSATING: // 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 = DuringCompensating(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
if (CurrentEvent.EventType == ENCODER_LIMIT_REACHED)
{
//stop drive
StopDrive();
//tell the state machine above that we're aligned
ReturnEvent.EventType = ALIGNED_TO_BEACON;
//don't worry about making a transition, as the state machine above should
stop aligning.
}
}
}
break;
}
//If we are making a state transition
//We shouldn't enter this ever for this state machine. I'm keeping it for code
consistency.
if (MakeTransition == true)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunAligningToBeacon(CurrentEvent);

CurrentState = NextState; //Modify state variable

// Execute entry function for new state


// this defaults to ES_ENTRY
RunAligningToBeacon(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 StartAligningToBeacon(ES_Event_t CurrentEvent)
{
if (CurrentEvent.EventType != ES_ENTRY)
{
printf("WARNING: the alignign to beacon state machine was started by a
non-ES_ENTRY event.\r\n");
}
printf("In \"StartAligningTobeacon\" we are looking for a beacon with period of
%d\r\n", CurrentEvent.EventParam);
// 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)
{
CurrentState = ENTRY_STATE;
}
// call the entry function (if any) for the ENTRY_STATE
RunAligningToBeacon(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
****************************************************************************/
AligningToBeaconState_t QueryAligningToBeacon(void)
{
return CurrentState;
}

/***************************************************************************
private functions
***************************************************************************/
static ES_Event_t DuringStopping(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))
{
StopDrive();
ES_Timer_InitTimer(MOVEMENT_TIMER, STOPPING_TIME);
EnableBeaconIR(PeriodArray[QueryTeamBit()][Event.EventParam]);
}
else if (Event.EventType == ES_EXIT)
{
ES_Timer_StopTimer(MOVEMENT_TIMER);
}
else
// do the 'during' function for this state
{
//we don't need to do anything here.
}
return ReturnEvent;
}

static ES_Event_t DuringRotating(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))
{
//TODO, make sure drive library uses PID to make sure it turns wells
if (Direction)
{
DriveCWStraight(ALIGNING_SPEED, false, 0);
}
else
{
DriveCCWStraight(ALIGNING_SPEED, false, 0);
}
//Enable IR Beacon Interrupt with period coming from ES_ENTRY event
//EnableBeaconIR(PeriodArray[QueryTeamBit()][Event.EventParam]);
printf("In \"DuringRotating\" we initialized the beacon ir interrupt to look for
a period of %d\r\n", Event.EventParam);
}
else if (Event.EventType == ES_EXIT)
{
printf("EXIT EVENT INTO DuringRotating\r\n");
//stop drive
StopDrive();
//diable interrupts for IR
DisableBeaconIR();
//tell the state machine above that we're aligned
}
else
// do the 'during' function for this state
{
//we don't need to do anything here.
}
return ReturnEvent;
}

static ES_Event_t DuringCompensating(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))
{
//TODO, make sure drive library uses PID to make sure it turns wells
if (Direction)
{
DriveCCWStraight(COMPENSATE_SPEED, true, COMPENSATE_ANGLE);
}
else
{
DriveCWStraight(COMPENSATE_SPEED, true, COMPENSATE_ANGLE);
}
}
else if (Event.EventType == ES_EXIT)
{
StopDrive();
}
else
// do the 'during' function for this state
{
//we don't need to do anything here.
}
return ReturnEvent;
}
// CCW = 0, CW = 1
void SetAligningDirection(uint8_t Dir)
{
Direction = Dir;
}