Module
NavToReloadSM.c
Description
This module contains the navigation functions from anywhere on the field.
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* include header files for the framework and this service
*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_DeferRecall.h"
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
#include "inc/hw_ssi.h"
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h" // Define PART_TM4C123GH6PM in project
#include "driverlib/gpio.h"
#include "termio.h"
#include "hw_nvic.h"
#include "hw_pwm.h"
#include "hw_timer.h"
#include "hw_ssi.h"
#include "MotorDrive.h"
#include "ADMulti.h"
#include "LineFollowingSM.h"
#include "NavToReloadSM.h"
#include "AligningToBeacon.h"
#include "ReloadIRCaptureModule.h"
#include "NewtonStep.h"
#define NOMINAL_DC 40
#define NOMINAL_RPM 70
#define SLOW_RPM 30 //15
#define TURN_RPM 23
#define ROTATE_ONTO_LINE_ANGLE 5
#define TAPE_TIME_MS (220) // TODO need to tune this! //800
Parameters
ES_Event_t ThisEvent: the event to post to the queue
Returns
boolean: True if no error, False otherwise
Description
Posts an event to the queue of the SM
Notes
Author
S. Park
****************************************************************************/
bool PostNavToReloadSM(ES_Event_t ThisEvent)
{
return ES_PostToService(MyPriority, ThisEvent);
}
/****************************************************************************
Function
RunNavToReloadSM
Parameters
ES_Event_t ThisEvent, event to process
Returns
ES_Event_t ThisEvent, event to return
Description
Posts an event to the queue of the SM
Notes
Author
S. Park
****************************************************************************/
ES_Event_t RunNavToReloadSM(ES_Event_t CurrentEvent)
{
bool MakeTransition = false;/* are we making a state transition?
*/
NavToReloadState_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 BeaconAligning: // If current state is AligningToBeacon
{ // Execute During function for AligningToBeacon. ES_ENTRY & ES_EXIT are
// processed here allow the lower level state machines to re-map
// or consume the event
ReturnEvent = CurrentEvent = DuringBeaconAligning(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 ALIGNED_TO_BEACON
{ if (!DiffNav)
{
NextState = Checking4Line; //Decide what the next
state will be
}
else
{
NextState = DiffNavFindLine;
}
return ReturnEvent;
}
/****************************************************************************
Function
StartNavToReloadSM
Parameters
ES_Event_t CurrentEvent
Returns
nothing
Description
Does any required initialization for this state machine
Notes
Author
S. Park
****************************************************************************/
void StartNavToReloadSM(ES_Event_t CurrentEvent)
{
if (CurrentEvent.EventType != ES_ENTRY)
{
printf("Warning, nav to reload was started with a non-entry event.\r\n");
}
printf("Nav to reload is looking for a period of %d\r\n", CurrentEvent.EventParam);
// Start with motors stopped
StopDrive();
//Initialize to AligningToBeacon
CurrentState = BeaconAligning;
return;
}
/***************************************************************************
private functions
***************************************************************************/
static ES_Event_t DuringBeaconAligning(ES_Event_t Event)
{
//During function for stopped
ES_Event_t ReturnEvent = Event; // assme no re-mapping or comsumption