Anda di halaman 1dari 12

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

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"

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


#define LINE_TIMEOUT_MS 500
#define TURN_TIMEOUT_MS 1200
#define BACKUP_TIMEOUT_MS 1000
#define IR_RELOAD_TIMEOUT_MS 7500

#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

/*---------------------------- Module Functions ---------------------------*/


static ES_Event_t DuringBeaconAligning(ES_Event_t Event);
static ES_Event_t DuringChecking4Line(ES_Event_t Event);
static ES_Event_t DuringTurningLeftTime(ES_Event_t Event);
static ES_Event_t DuringDrivingForwardPID(ES_Event_t Event);
static ES_Event_t DuringWaitingForTape(ES_Event_t Event);
static ES_Event_t DuringTurningRight(ES_Event_t Event);
static ES_Event_t DuringNewtonStepping(ES_Event_t Event);
static ES_Event_t DuringChecking4ReloadIR(ES_Event_t Event);
static ES_Event_t DuringLineFollowing(ES_Event_t Event);
static ES_Event_t DuringDiffNavFindLine(ES_Event_t Event);
/*---------------------------- Module Variables ---------------------------*/
static uint8_t MyPriority;
static NavToReloadState_t CurrentState;
static bool DiffNav = false;

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


/****************************************************************************
Function
PostNavToReloadSM

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;
}

// 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;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
}
break;
case LINE_ON: //If event is ALIGNED_TO_BEACON
{ NextState = LineFollowing; //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;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
}
break;
}
}
}
break;

case Checking4Line: // If current state is Checking4Line

{ ReturnEvent = CurrentEvent = DuringChecking4Line(CurrentEvent);


//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ES_TIMEOUT: //If event is ES_TIMEOUT for LINE_TIMER
{
if (CurrentEvent.EventParam == LINE_TIMER)
{
NextState = TurningLeftTime;//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;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
}
}
break;

case LINE_ON: //If event is LINE_ON


{
NextState = LineFollowing; //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;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
}
break;
}
}
}
break;

case TurningLeftTime: // If current state is TurningLeftTime


{ // Execute During function for TurningLeftTime state.
ReturnEvent = CurrentEvent = DuringTurningLeftTime(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ENCODER_LIMIT_REACHED: //If event is ES_TIMEOUT

{ NextState = DrivingForwardPID; //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;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
}
break;
}
}
}
break;

case DrivingForwardPID: // If current state is DrivingForwardPID


{ // Execute During function for DrivingForwardPID state.
ReturnEvent = CurrentEvent = DuringDrivingForwardPID(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
if (CurrentEvent.EventType == LINE_ON)
{ //If event is LINE_ON
NextState = WaitingForTape; //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;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
}
else if ((CurrentEvent.EventType == LEFT_BUMPER_HIT) ||
(CurrentEvent.EventType == RIGHT_BUMPER_HIT))
{
ReturnEvent.EventType = UNEXPECTED_BUMPER;
}
}
}
break;

case WaitingForTape: // If current state is WaitingForTape


{ // Execute During function for DrivingForwardPID state.
ReturnEvent = CurrentEvent = DuringWaitingForTape(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ENTERED_TAPE: //If event is LINE_ON
{ NextState = TurningRight; //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;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
}
break;
case ES_TIMEOUT:
{
if (CurrentEvent.EventParam == MOVEMENT_TIMER)
{
NextState = TurningRight; //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;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
}
}
break;
}
}
}
break;

case TurningRight: // If current state is TurningRight


{ // Execute During function for TurningRight state.
ReturnEvent = CurrentEvent = DuringTurningRight(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case LINE_ON: //If event is LINE_ALIGNED (TODO: Changed
this so only need one event checker)
{ NextState = LineFollowing; //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;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
}
break;
}
}
}
break;

case NewtonStepping: // If current state is DrivingBackwardTime


{ // Execute During function for DrivingBackwardTime state.
ReturnEvent = CurrentEvent = DuringNewtonStepping(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case NEWTON_DONE: //If event is NEWTON_DONE
{ EntryEventKind.EventType = ES_ENTRY;
// Return READY_FOR_RELOAD and leave SM
ReturnEvent.EventType = READY_FOR_RELOAD;
ReturnEvent.EventParam = CurrentEvent.EventParam;
}
break;
}
}
}
break;

case Checking4ReloadIR: // If current state is Checking4ReloadIR


{ // Execute During function for Checking4ReloadIR state.
ReturnEvent = CurrentEvent = DuringChecking4ReloadIR(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case FOUND_RELOAD_IR: //If event is FOUND_RELOAD_IR
{ // if transitioning to a state with history change kind of entry
EntryEventKind.EventType = ES_ENTRY;
// Return READY_FOR_RELOAD and leave SM
ReturnEvent.EventType = READY_FOR_RELOAD;
ReturnEvent.EventParam = CurrentEvent.EventParam;
}
break;

case ES_TIMEOUT: //If event is ES_TIMEOUT


{ if (CurrentEvent.EventParam == IR_RELOAD_TIMER)
{
ReturnEvent.EventType = FAILED_RELOAD_IR;
}
}
break;
}
}
}
break;

case LineFollowing: // If current state is LineFollowing


{ // Execute During function for LineFollowing state.
ReturnEvent = CurrentEvent = DuringLineFollowing(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
if ((CurrentEvent.EventType == RIGHT_BUMPER_HIT) ||
(CurrentEvent.EventType == LEFT_BUMPER_HIT))
{
NextState = Checking4ReloadIR; //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;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
}
}
}
break;
case DiffNavFindLine: // If current state is TurningLeftTime
// Execute During function for TurningLeftTime state.
ReturnEvent = CurrentEvent = DuringDiffNavFindLine(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case LINE_ON: //If event is ES_TIMEOUT

{ NextState = LineFollowing; //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;
// optionally, consume or re-map this event for the upper
// level state machine
ReturnEvent.EventType = ES_NO_EVENT;
}
break;
}
}
}

// If we are making a state transition


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

CurrentState = NextState; //Modify state variable

// Execute entry function for new state


// this defaults to ES_ENTRY
RunNavToReloadSM(EntryEventKind);
}

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;

// Run the NavToReload SM


RunNavToReloadSM(CurrentEvent);

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

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events


if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
// Start aligning to beacon
SetAligningDirection(ALIGN_CW);
StartAligningToBeacon(Event);
}
else if (Event.EventType == ES_EXIT)
{
// No exit action
RunAligningToBeacon(Event);
}
else
{
// Run AligningToBeaconSM
ReturnEvent = RunAligningToBeacon(Event);
}
return ReturnEvent;
}

static ES_Event_t DuringChecking4Line(ES_Event_t Event)


{
//During function for Checking4Line
ES_Event_t ReturnEvent = Event; // assme no re-mapping or comsumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events


if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
ES_Timer_InitTimer(LINE_TIMER, LINE_TIMEOUT_MS);
EnableLineOn();
}
else if (Event.EventType == ES_EXIT)
{
// No exit action
}
else
{
// No action
}
return ReturnEvent;
}

static ES_Event_t DuringTurningLeftTime(ES_Event_t Event)


{
//During function for TurningLeftTime
ES_Event_t ReturnEvent = Event; // assme no re-mapping or comsumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events


if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
// Turn Left
DriveCCWStraight(SLOW_RPM, true, ROTATE_ONTO_LINE_ANGLE);
}
else if (Event.EventType == ES_EXIT)
{
// Stop drive on exit
StopDrive();
}
else
{
// No other action
}
return ReturnEvent;
}

static ES_Event_t DuringDrivingForwardPID(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))
{
// Drive wheels forward using encoder PID
DriveForwardStraight(NOMINAL_RPM, false, 0);
}
else if (Event.EventType == ES_EXIT)
{
// Stop drive on exit
StopDrive();
}
else
{
// no action
}
return ReturnEvent;
}

static ES_Event_t DuringWaitingForTape(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))
{
// Drive wheels forward using encoder PID
ES_Timer_InitTimer(MOVEMENT_TIMER, TAPE_TIME_MS);
DriveForwardStraight(SLOW_RPM, false, 0);
}
else if (Event.EventType == ES_EXIT)
{
// Stop drive on exit
ES_Timer_StopTimer(MOVEMENT_TIMER);
StopDrive();
}
else
{
// no action
}
return ReturnEvent;
}

static ES_Event_t DuringTurningRight(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))
{
// Turn Right
EnableLineOn();
DriveCWStraight(TURN_RPM, false, 0);
}
else if (Event.EventType == ES_EXIT)
{
// Stop driving motors
StopDrive();
}
else
{
// no action
}
return ReturnEvent;
}

static ES_Event_t DuringNewtonStepping(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))
{
StartNewtonStep(Event);
}
else if (Event.EventType == ES_EXIT)
{
// Stop drive on exit
RunNewtonStep(Event);
}
else
{
ReturnEvent = RunNewtonStep(Event);
}
return ReturnEvent;
}
static ES_Event_t DuringChecking4ReloadIR(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))
{
// Start IR_RELOAD_TIMER
ES_Timer_InitTimer(IR_RELOAD_TIMER, IR_RELOAD_TIMEOUT_MS);
EnableReloadIR();
}
else if (Event.EventType == ES_EXIT)
{
DisableReloadIR();
}
else
{
// No other actions
}
return ReturnEvent;
}

static ES_Event_t DuringLineFollowing(ES_Event_t Event)


{
//During function for LineFollowing
ES_Event_t ReturnEvent = Event; // assme no re-mapping or comsumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events


if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
printf("got during line following\n\r");
StartLineFollowingSM(Event);
}
else if (Event.EventType == ES_EXIT)
{
RunLineFollowingSM(Event);
StopDrive();
}
else
{
//ReturnEvent = RunLineFollowingSM(Event);
}
// return Event without remapping
return ReturnEvent;
}

static ES_Event_t DuringDiffNavFindLine(ES_Event_t Event)


{
//During function for
ES_Event_t ReturnEvent = Event; // assme no re-mapping or comsumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events


if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
DriveCWStraight(SLOW_RPM, false, 0);
EnableLineOn();
}
else if (Event.EventType == ES_EXIT)
{
// No exit action
}
else
{
// No action
}
return ReturnEvent;
}

void SetComingFromShooting(bool Shooting)


{
DiffNav = Shooting;
}

Anda mungkin juga menyukai