/**
* @Function RunExitHSM(ES_Event ThisEvent)
* @param ThisEvent - the event (type and param) to be responded.
* @return Event - return event (type and param), in general should be ES_NO_EVENT
* @brief This function is where you implement the whole of the heirarchical state
* machine, as this is called any time a new event is passed to the event
* queue. This function will be called recursively to implement the correct
* order for a state transition to be: exit current state -> enter next state
* using the ES_EXIT and ES_ENTRY events.
* @note Remember to rename to something appropriate.
* The lower level state machines are run first, to see if the event is dealt
* with there rather than at the current level. ES_EXIT and ES_ENTRY events are
* not consumed as these need to pass pack to the higher level state machine.
* @author J. Edward Carryer, 2011.10.23 19:25
* @author Gabriel H Elkaim, 2011.10.23 19:25 */
ES_Event RunExitHSM(ES_Event ThisEvent)
{
uint8_t makeTransition = FALSE; // use to flag transition
ExitState_t nextState;
static uint8_t trackFlag = FALSE; // used to check if track wire has been found
static uint8_t turnedFlag = FALSE; // used to check if we have flipped 180 already
static uint8_t bounceCount = 0; // make sure we dont get stuck in align states
EventStorage args;
ES_Tattle(); // trace call stack
switch (CurrentState) {
case Exit_Init: // If current state is initial Pseudo State
if (ThisEvent.EventType == ES_INIT)// only respond to ES_Init
{
// Handle initialization of modules
// Initialize sub HSMs
// Move to real state
// These are handled in main() instead
nextState = Exit_Drive;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
;
}
break;
case Exit_Drive:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Drive forward
Drive_Straight(MOTOR_SPEED_CRAWL);
// Start timer for checking when we have driven far enough to have left the castle
ES_Timer_InitTimer(EXIT_HSM_TIMER, TIME_EXIT_OUTSIDE);
break;
case ES_EXIT:
// Stop the timer while we handle turns etc, it will restart on entry
ES_Timer_StopTimer(EXIT_HSM_TIMER);
Drive_Stop();
break;
case TRACK_FOUND:
trackFlag = TRUE;
// Consume
ThisEvent.EventType = ES_NO_EVENT;
break;
case BUMPER:
args.val = ThisEvent.EventParam;
// if center tripped
if (args.bits.event & args.bits.type & BUMP_CENTER) {
nextState = Exit_Straighten;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
// else if left tripped
else if (args.bits.event & args.bits.type & BUMP_LEFT) {
nextState = Exit_Align_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
// else if right tripped
else if (args.bits.event & args.bits.type & BUMP_RIGHT) {
nextState = Exit_Align_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
// Turn 180
if (turnedFlag) {
nextState = Exit_Turn90;
} else {
nextState = Exit_Turn180;
}
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Exit_Straighten:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is active
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Drive straight to recheck bumps
Drive_Straight(MOTOR_SPEED_CRAWL);
++bounceCount;
if (bounceCount == 8) {
nextState = Exit_Backup;
makeTransition = TRUE;
}
// Set the timeout
ES_Timer_InitTimer(EXIT_HSM_TIMER, TIME_EXIT_STRAIGHTEN);
break;
case ES_EXIT:
Drive_Stop();
// Stop timer on exit
ES_Timer_StopTimer(EXIT_HSM_TIMER);
break;
case TRACK_FOUND:
trackFlag = TRUE;
// Consume
ThisEvent.EventType = ES_NO_EVENT;
break;
case BUMPER:
args.val = ThisEvent.EventParam;
// if left bumper and NOT right bumper
if ((args.bits.type & BUMP_LEFT) && (~args.bits.type & BUMP_RIGHT)) {
nextState = Exit_Align_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
// else if right bumper and NOT left bumper
else if ((args.bits.type & BUMP_RIGHT) && (~args.bits.type & BUMP_LEFT)) {
nextState = Exit_Align_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
// else if its right AND left or center it will time out and move to backup
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
if (trackFlag) {
nextState = Exit_Done_Pause;
} else {
nextState = Exit_Backup;
}
makeTransition = TRUE;
// Reset our bounce count, it has resolved a collision
bounceCount = 0;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
// need a timeout for when the center cant be pressed
case Exit_Align_Left:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is active
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Turn into the wall to align
Drive_Right(-MOTOR_SPEED_MEDIUM);
ES_Timer_InitTimer(EXIT_HSM_TIMER, TIME_EXIT_ALIGN);
break;
case ES_EXIT:
// Stop
Drive_Stop();
ES_Timer_StopTimer(EXIT_HSM_TIMER);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
nextState = Exit_Straighten;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
// need a timeout for when the center cant be pressed
case Exit_Align_Right:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is active
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Turn into the wall to align
Drive_Left(-MOTOR_SPEED_MEDIUM);
ES_Timer_InitTimer(EXIT_HSM_TIMER, TIME_EXIT_ALIGN);
break;
case ES_EXIT:
// Stop
Drive_Stop();
ES_Timer_StopTimer(EXIT_HSM_TIMER);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
nextState = Exit_Straighten;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Exit_Backup:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is active
switch (ThisEvent.EventType) {
case ES_ENTRY:
Drive_Straight(-MOTOR_SPEED_CRAWL);
ES_Timer_InitTimer(EXIT_HSM_TIMER, TIME_EXIT_BACKUP);
break;
case ES_EXIT:
Drive_Stop();
ES_Timer_StopTimer(EXIT_HSM_TIMER);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
// Turn 90
nextState = Exit_Turn90;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Exit_Turn90:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is active
switch (ThisEvent.EventType) {
case ES_ENTRY:
Drive_TankRight(MOTOR_SPEED_EXPLORE);
ES_Timer_InitTimer(EXIT_HSM_TIMER, MOTOR_TURN_EX_90);
break;
case ES_EXIT:
Drive_Stop();
ES_Timer_StopTimer(EXIT_HSM_TIMER);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
nextState = Exit_Drive;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Exit_Turn180:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
Drive_TankRight(MOTOR_SPEED_EXPLORE);
// Set 180 turn flag true, don't do this state twice
turnedFlag = TRUE;
ES_Timer_InitTimer(EXIT_HSM_TIMER, MOTOR_TURN_EX_180);
break;
case ES_EXIT:
Drive_Stop();
ES_Timer_StopTimer(EXIT_HSM_TIMER);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
nextState = Exit_Drive;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Exit_Done_Pause:
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Reverse
Drive_Straight(-MOTOR_SPEED_MEDIUM);
ES_Timer_InitTimer(EXIT_HSM_TIMER, TIME_EXIT_BACKUP);
break;
case ES_EXIT:
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
Drive_Stop();
ES_Timer_InitTimer(STALL_TIMER, STALL_TIME_IN_MS);
ThisEvent.EventType = ES_NO_EVENT;
} else if (ThisEvent.EventParam == STALL_TIMER) {
nextState = Exit_Done_State;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default:
break;
}
break;
case Exit_Done_State:
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Turn 180
Drive_TankRight(MOTOR_SPEED_EXPLORE);
ES_Timer_InitTimer(EXIT_HSM_TIMER, MOTOR_TURN_EX_180 - 10);
break;
case ES_EXIT:
// Do nothing
Drive_Stop();
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == EXIT_HSM_TIMER) {
Drive_Stop();
ThisEvent.EventType = CHILD_DONE;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
break;
default: // all unhandled states fall into here
break;
} // end switch on Current State
if (makeTransition == TRUE) { // making a state transition, send EXIT and ENTRY
// recursively call the current state with an exit event
RunExitHSM(EXIT_EVENT);
CurrentState = nextState;
RunExitHSM(ENTRY_EVENT);
}
ES_Tail(); // trace call stack end
return ThisEvent;
}
void Drive_Stop(void) {
Drive_Straight(0);
}
char Drive_Stop(void)
{
return (Drive_Straight(0));
}
