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

Module
SquaringToWall.c

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#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 "driverlib/sysctl.h"
#include "driverlib/pin_map.h"
#include "driverlib/gpio.h"
#include "termio.h"
#include "hw_nvic.h"
#include "hw_pwm.h"
#include "hw_timer.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 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 "SquaringToWall.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 DRIVING_BACK

#define TURN_DRIVE_DUTY (100)
#define COUNTER_TURN_DRIVE (20)
#define SQUARE_TIMEOUT_BACK (1000)
#define SQUARE_TIMEOUT (350)

/*---------------------------- 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 DuringDriveForward(ES_Event_t Event);
static ES_Event_t DuringDriveBack(ES_Event_t Event);

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

// everybody needs a state variable, you may need others as well
static SquaringToWallState_t CurrentState;
static uint32_t LastInput_RightBumper = 0;
static uint32_t LastInput_LeftBumper = 0;
static uint8_t SideHit = 0; //0 means right hit, >0 means left bumper
was hit.
static uint32_t BumperEvent = 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 RunSquaringToWall(ES_Event_t CurrentEvent)
{
bool MakeTransition = false;/* are we making a state transition?
*/
SquaringToWallState_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)
{
//TODO: decide which squaring approach is best.
//while we're driving one side back
case DRIVING_BACK:
{
ReturnEvent = CurrentEvent = DuringDriveBack(CurrentEvent);
if (CurrentEvent.EventType != ES_NO_EVENT)
{
if (((SideHit > 0) && (CurrentEvent.EventType == RIGHT_BUMPER_HIT)) ||
((SideHit == 0) && (CurrentEvent.EventType == LEFT_BUMPER_HIT)))
{
StopDrive();
NextState = DRIVING_FOR;
MakeTransition = true;
ES_Timer_StopTimer(SQUARE_TIMER);
ReturnEvent.EventType = ES_NO_EVENT;
}
else if ((CurrentEvent.EventType == ES_TIMEOUT) && (CurrentEvent.EventParam
== SQUARE_TIMER)) //If timer times out.
{
printf("Timer Timerout!\r\n");
StopDrive();
NextState = DRIVING_FOR;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;
}
}
}
break;

//while we're driving one side forward

case DRIVING_FOR:
{
ReturnEvent = CurrentEvent = DuringDriveForward(CurrentEvent);
if (CurrentEvent.EventType != ES_NO_EVENT)
 {
/*if((SideHit > 0 && CurrentEvent.EventType == RIGHT_BUMPER_HIT) || (SideHit
== 0 && CurrentEvent.EventType == LEFT_BUMPER_HIT)){
StopDrive();
ReturnEvent.EventType = SQUARED;
ES_Timer_StopTimer(SQUARE_TIMER);
}*/
if ((CurrentEvent.EventType == ES_TIMEOUT) && (CurrentEvent.EventParam ==
SQUARE_TIMER))
{
ReturnEvent.EventType = SQUARED;
}
}
}
break;
}
// If we are making a state transition
if (MakeTransition == true)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunSquaringToWall(CurrentEvent);

CurrentState = NextState; //Modify state variable

// Execute entry function for new state

// this defaults to ES_ENTRY
RunSquaringToWall(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 StartSquaringToWall(ES_Event_t CurrentEvent)
{
if (CurrentEvent.EventType != ES_ENTRY)
{
printf("WARNING: Squaring to wall started with a non-ES_ENTRY event.\r\n");
}
if (CurrentEvent.EventParam > 0)
{
printf("Starting squaring to wall because left bumper was hit \r\n");
BumperEvent = RIGHT_BUMPER_HIT;
SideHit = 1;
}
else
{
printf("Starting squaring to wall because right bumper was hit \r\n");
 BumperEvent = LEFT_BUMPER_HIT;
SideHit = 0;
}
CurrentState = ENTRY_STATE;

// call the entry function (if any) for the ENTRY_STATE

RunSquaringToWall(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
****************************************************************************/
SquaringToWallState_t QuerySquaringToWall(void)
{
return CurrentState;
}

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

static ES_Event_t DuringDriveForward(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))
{
// if(SideHit > 0) DriveRightForward(TURN_DRIVE_DUTY);
//else DriveLeftForward(TURN_DRIVE_DUTY);
DriveForward(100);
ES_Timer_InitTimer(SQUARE_TIMER, SQUARE_TIMEOUT);
}
else if (Event.EventType == ES_EXIT)
{
ES_Timer_StopTimer(SQUARE_TIMER);
StopDrive();
}
else
{}
return ReturnEvent;
}

static ES_Event_t DuringDriveBack(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))
{
if (SideHit > 0)
{
SquareMoveRightForward(TURN_DRIVE_DUTY, COUNTER_TURN_DRIVE);
}
else
{
SquareMoveLeftForward(TURN_DRIVE_DUTY, COUNTER_TURN_DRIVE);
}
printf("STARTING TIMER!\r\n");
ES_Timer_InitTimer(SQUARE_TIMER, SQUARE_TIMEOUT_BACK);
}
else if (Event.EventType == ES_EXIT)
{
ES_Timer_StopTimer(SQUARE_TIMER);
}
else
// do the 'during' function for this state
{}
return ReturnEvent;
}

bool Check4LeftBumper(void)
{
bool ReturnVal = false;
uint8_t CurrentInput_LeftBumper;
ES_Event_t BumperEvent;

CurrentInput_LeftBumper = HWREG(GPIO_PORTD_BASE + (GPIO_O_DATA + ALL_BITS)) &

BIT6HI;

if ((CurrentInput_LeftBumper != LastInput_LeftBumper) &&

(CurrentInput_LeftBumper))
{
printf("Left Bumper Triggered \r\n");
BumperEvent.EventType = LEFT_BUMPER_HIT;
PostMasterSM(BumperEvent);

ReturnVal = true;
}

LastInput_LeftBumper = CurrentInput_LeftBumper;
return ReturnVal;
}

bool Check4RightBumper(void)
{
bool ReturnVal = false;
uint8_t CurrentInput_RightBumper;
ES_Event_t BumperEvent;

CurrentInput_RightBumper = HWREG(GPIO_PORTD_BASE + (GPIO_O_DATA + ALL_BITS)) &

BIT7HI;
if ((CurrentInput_RightBumper != LastInput_RightBumper) &&
(CurrentInput_RightBumper))
{
printf("Right Bumper Triggered \r\n");
BumperEvent.EventType = RIGHT_BUMPER_HIT;
PostMasterSM(BumperEvent);

ReturnVal = true;
}
 LastInput_RightBumper = CurrentInput_RightBumper;
return ReturnVal;
}

void InitBumpers(void)
{
if ((HWREG(SYSCTL_PRGPIO) & BIT3HI) != BIT3HI) // if haven't enabled the port
yet
{
// Set up port D by enabling the peripheral clock
HWREG(SYSCTL_RCGCGPIO) |= BIT3HI;

// Waiting for the peripheral to be ready

while ((HWREG(SYSCTL_PRGPIO) & BIT3HI) != BIT3HI)
{
;
}
}

// Setting PD6 to digital input

HWREG(GPIO_PORTD_BASE + GPIO_O_DEN) |= (BIT6HI);
HWREG(GPIO_PORTD_BASE + GPIO_O_DIR) &= (BIT6LO);

// Setting PD7 to digital input

HWREG(GPIO_PORTD_BASE + GPIO_O_DEN) |= (BIT7HI);
HWREG(GPIO_PORTD_BASE + GPIO_O_DIR) &= (BIT7LO);

// Setting PD6 and PD7 to pull down

HWREG(GPIO_PORTD_BASE + GPIO_O_PDR) |= BIT6HI;
HWREG(GPIO_PORTD_BASE + GPIO_O_PDR) |= BIT7HI;
}