static ES_Event DuringAttack_t( ES_Event Event)
{
ES_Event 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) )
{
// implement any entry actions required for this state machine
// Stop positioning when in the attack SM
ES_Timer_StopTimer(POSITION_CHECK);
// after that start any lower level machines that run in this state
//StartLowerLevelSM( Event );
StartAttackSM(Event);
// repeat the StartxxxSM() functions for concurrent state machines
// on the lower level
}
else if ( Event.EventType == ES_EXIT )
{
// on exit, give the lower levels a chance to clean up first
//RunLowerLevelSM(Event);
ReturnEvent = RunAttackSM(Event);
// repeat for any concurrently running state machines
// now do any local exit functionality
// Count the number of shots, and don't start a next shot timer
// if we are out of ammo
shotCounter++;
if (shotCounter < 5)
{
ES_Timer_InitTimer(ATTACK_PHASE_TIMER, NEXT_SHOT_T);
}
// Reset positioning
ES_Event ResetEvent;
ResetEvent.EventType = ES_RESET_DESTINATION;
PostMasterSM(ResetEvent);
}else
// do the 'during' function for this state
{
// run any lower level state machine
// ReturnEvent = RunLowerLevelSM(Event);
ReturnEvent = RunAttackSM(Event);
// repeat for any concurrent lower level machines
// do any activity that is repeated as long as we are in this state
}
// return either Event, if you don't want to allow the lower level machine
// to remap the current event, or ReturnEvent if you do want to allow it.
return(ReturnEvent);
}
/****************************************************************************
Function
RunPhotoTransistorService
Parameters
ES_Event : the event to process
Returns
ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise
****************************************************************************/
ES_Event RunPhotoTransistorService( ES_Event ThisEvent )
{
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
// If we have stopped seeing pulses, it's time to evaluate whether we saw a beacon
if ((ThisEvent.EventType == ES_TIMEOUT) && (ThisEvent.EventParam == AVERAGE_BEACONS_TIMER))
{
// If the beacon we were interested in had enough pulses
if (numSamples[LastBeacon] >= NUMBER_PULSES_TO_BE_ALIGNED)
{
// set the last update time for the beacon
beacons[LastBeacon].lastUpdateTime = captureInterrupt(PHOTOTRANSISTOR_INTERRUPT_PARAMATERS);
// set the angle to the beacon based on the average of all pulses measured
beacons[LastBeacon].lastEncoderAngle = CalculateAverage(LastBeacon);
// Store this beacon as the last updated beacon
LastUpdatedBeacon = LastBeacon;
// Determine if we should recalculate our position and angle based on whether or not we have received 3 consecutive pulses
if (TimeForUpdate())
{
ES_Event NewEvent;
NewEvent.EventType = ES_CALCULATE_POSITION;
PostPositionLogicService(NewEvent);
}
}
// Reset stored average information
ResetAverage();
// Reallow new beacons to be recorded
Bucketing = true;
LastBeacon = NULL_BEACON;
ES_Timer_StopTimer(AVERAGE_BEACONS_TIMER);
}
else if (ThisEvent.EventType == ES_ALIGN_TO_BUCKET)
{
// If we are trying to align to the bucket for a shot,
// shift the way we handle interrupts
AligningToBucket = true;
enableCaptureInterrupt(PHOTOTRANSISTOR_INTERRUPT_PARAMATERS);
}
return ReturnEvent;
}
//The interrupt response for our phototransistor
void PhotoTransistor_InterruptResponse(void)
{
// Clear Interrupt
clearCaptureInterrupt(PHOTOTRANSISTOR_INTERRUPT_PARAMATERS);
// Determine Capture time
uint32_t ThisCapture = captureInterrupt(PHOTOTRANSISTOR_INTERRUPT_PARAMATERS);
// Calculate period
uint32_t Period = ((ThisCapture - LastCapture) * MICROSECONDS_DIVISOR ) / TICKS_PER_MS;
//Store the Last Cpature
LastCapture = ThisCapture;
//Iterate Through the different frequency options
for (int i = 0; i < NUMBER_BEACON_FREQUENCIES; i++)
{
// If the period matches a beacon period
if (ToleranceCheck(Period, beacons[i].period, PERIOD_MEASURING_ERROR_TOLERANCE))
{
// If we're searching for a beacon
if (Bucketing)
{
buckets[i]++;
}
// If we're supposed to align to the bucket, and the number of pulses we've seen for
// this beacon is greater than our threshold
if ((AligningToBucket) && (buckets[i] >= NUMBER_PULSES_FOR_BUCKET))
{
// If this is the beacon corresponding to our target bucket
if (((MyColor() == COLOR_BLUE) && (i == BEACON_INDEX_NW)) || ((MyColor() == COLOR_RED) && (i == BEACON_INDEX_SE)))
{
// stop our drive
clearDriveAligningToBucket();
// Post an aligned event
ES_Event AlignedEvent;
AlignedEvent.EventType = ES_ALIGNED_TO_BUCKET;
PostMasterSM(AlignedEvent);
// stop aligning
AligningToBucket = false;
// reset all buckets
for (int j = 0; j < NUMBER_BEACON_FREQUENCIES; j++)
{
buckets[j] = 0;
}
// reset average information
ResetAverage();
// return to searching for beacons
Bucketing = true;
LastBeacon = NULL_BEACON;
ES_Timer_StopTimer(AVERAGE_BEACONS_TIMER);
return;
}
}
// If we're not aligning to a bucket, and the number of pulses we've seen for this
// beacon is greater than our threshold
else if (buckets[i] >= NUMBER_PULSES_TO_BE_ALIGNED && LastBeacon == NULL_BEACON && !AligningToBucket)
{
// store the beacon to be recorded
LastBeacon = i;
// reset all buckets
for (int j = 0; j < NUMBER_BEACON_FREQUENCIES; j++)
{
buckets[j] = 0;
}
// restart the timer to evaluate the beacon
ES_Timer_InitTimer(AVERAGE_BEACONS_TIMER, AVERAGE_BEACONS_T);
Bucketing = false;
}
// increment the sum of all encoder angles for this beacon
sums[i] += GetPeriscopeAngle();
// increment the number of pulses seen for this beacon
numSamples[i]++;
// If the evaluation timer is already running, restart it
ES_Timer_SetTimer(AVERAGE_BEACONS_TIMER, AVERAGE_BEACONS_T);
}
}
}
/**
* @Function RunRoachFSM(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 is the function that implements the actual state machine; in the
* roach case, it is a simple ping pong state machine that will form the
* basis of your more complicated exploration. 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.
* @author Gabriel H Elkaim, 2013.09.26 15:33 */
ES_Event RunMotorYFSM(ES_Event ThisEvent) {
uint8_t makeTransition = FALSE;
Motor_Y_State_t nextState;
ES_Tattle(); // trace call stack
static unsigned int eventParam = 0;
static unsigned int i = 0;
static unsigned int j = 0;
switch (CurrentState) {
case InitQState:
if (ThisEvent.EventType == ES_INIT) {
nextState = selfCalibrateY;
makeTransition = TRUE;
}
break;
case selfCalibrateY:
switch (ThisEvent.EventType) {
case ES_ENTRY:
break;
case CALIBRATED:
if (ThisEvent.EventParam == MOTOR_X) {
Stepper_Init(MOTOR_Y, 77);
reverseY(4000);
ES_Timer_InitTimer(MOTOR_Y_TIMER, 5000);
}
break;
case Y_LIMIT:
if (ThisEvent.EventParam == Y_MIN_MASK) {
initialY = getFilteredAD(Y_SLIDER);
newPosition = AD_SCALE_Y * initialY;
currentPosition = 0;
setStepsTaken(MOTOR_Y, 0);
positionDifference = currentPosition - newPosition;
if (positionDifference < 0) {
//forwardX(abs(positionDifference));
} else if (positionDifference > 0) {
1; //do nothing, already at zero
}
Stepper_Halt(MOTOR_Y);
}
break;
//case Y_LIMIT:
case ES_TIMEOUT:
if (ThisEvent.EventParam == MOTOR_Y_TIMER) {
initialY = getFilteredAD(Y_SLIDER);
//Stepper_Resume(MOTOR_Y);
newPosition = AD_SCALE_Y * initialY;
currentPosition = 0;
setStepsTaken(MOTOR_Y, 0);
positionDifference = currentPosition - newPosition;
if (positionDifference < 0) {
//forwardY(abs(positionDifference));
} else if (positionDifference > 0) {
1; //do nothing, already at zero
}
nextState = waitingY;
makeTransition = TRUE;
}
break;
case ES_EXIT:
Stepper_Halt(MOTOR_Y);
ES_Timer_StopTimer(MOTOR_Y_TIMER);
break;
default:
break;
}
break;
case waitingY:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
break;
case XY_CHANGE:
nextState = movingBufferY;
makeTransition = TRUE;
memEventY = ThisEvent;
break;
case PRESETCHANGE:
//Coming from presetFSM
//nextState = movingY_Preset;
//makeTransition = TRUE;
break;
case ES_EXIT:
Stepper_Resume(MOTOR_Y);
setStepsRate(MOTOR_Y, 77);
//ES_Timer_StopTimer(MOTOR_Y_TIMER);
break;
default:
break;
}
}
break;
case movingBufferY:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
ES_Timer_InitTimer(MOTOR_Y_TIMER, 200);
break;
case XY_CHANGE:
nextState = movingBufferY;
makeTransition = TRUE;
memEventY = ThisEvent;
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam = MOTOR_Y_TIMER) {
nextState = movingY;
makeTransition = TRUE;
}
break;
case ES_EXIT:
setStepsRate(MOTOR_Y, 77);
break;
default:
break;
}
}
break;
case movingY:
switch (ThisEvent.EventType) {
case ES_ENTRY:
Stepper_Init(MOTOR_Y, 77);
newPosition = AD_SCALE_Y * memEventY.EventParam;
currentPosition = getStepsTaken(MOTOR_Y);
positionDifference = currentPosition - newPosition;
if (positionDifference < 0) {
yDirection = FORWARD;
forwardY(abs(positionDifference));
} else {
yDirection = REVERSE;
reverseY(abs(positionDifference));
}
break;
case ALMOSTOUTOFSTEPS:
setStepsRate(MOTOR_Y, 15);
break;
case OUTOFSTEPS:
Stepper_Halt(MOTOR_Y);
ES_Timer_InitTimer(MOTOR_Y_TIMER, 50);
setStepsRate(MOTOR_Y, 77);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == MOTOR_Y_TIMER) {
Stepper_Halt(MOTOR_Y);
nextState = waitingY;
makeTransition = TRUE;
}
break;
case XY_CHANGE:
nextState = movingBufferY;
makeTransition = TRUE;
memEventY = ThisEvent;
break;
case ES_EXIT:
lastYDirection = yDirection;
setStepsRate(MOTOR_Y, 77);
ES_Timer_StopTimer(MOTOR_Y);
break;
default:
nextState = waitingY;
makeTransition = TRUE;
break;
}
break;
case movingBufferY2:
//ThisEvent = RunmovingYSubFSM(ThisEvent);
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
//printf("movingBuffer Enter:%d\n", memEventY.EventParam);
ES_Timer_InitTimer(MOTOR_Y_TIMER, 200);
break;
case XY_CHANGE:
//printf("Y_CHANGE\n");
//determine if a change of direction is about to occur
newPosition = AD_SCALE_Y * ThisEvent.EventParam;
currentPosition = getStepsTaken(MOTOR_Y);
positionDifference = currentPosition - newPosition;
if (positionDifference < 0) {
yDirection = FORWARD;
} else {
yDirection = REVERSE;
}
//if direction change, start slowing down
memEventY = ThisEvent;
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam = MOTOR_Y_TIMER) {
printf("yDirection:%d, lastYDirection:%d\n", yDirection, lastYDirection);
if (yDirection != lastYDirection) {
printf("directionChange!\n");
//setStepsRate(MOTOR_Y, 15);
//make an event instead!
//ES_Timer_InitTimer(MOTOR_Y, 150);
//while (!(getStepsRate(MOTOR_Y) < 20));
nextState = changingDirection;
makeTransition = TRUE;
} else {
nextState = movingY;
makeTransition = TRUE;
}
}
break;
case ES_EXIT:
//Stepper_Init(MOTOR_Y,500);
break;
default:
break;
}
}
break;
case changingDirection:
switch (ThisEvent.EventType) {
case ES_ENTRY:
//printf("movingBuffer Enter:%d\n", memEventY.EventParam);
//ES_Timer_InitTimer(MOTOR_Y_TIMER, 50);
Stepper_ChangeStepRate(MOTOR_Y, 15);
break;
case SPEEDMATCH:
printf("speedmatch!\n");
nextState = movingY;
makeTransition = TRUE;
break;
case XY_CHANGE:
//printf("Y_CHANGE\n");
nextState = movingBufferY2;
makeTransition = TRUE;
memEventY = ThisEvent;
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam = MOTOR_Y_TIMER) {
nextState = movingY;
makeTransition = TRUE;
}
break;
case ES_EXIT:
//Stepper_Init(MOTOR_Y,500);
break;
default:
break;
}
break;
default:
break;
}
if (makeTransition == TRUE) {
RunMotorYFSM(EXIT_EVENT);
lastState = CurrentState;
CurrentState = nextState;
RunMotorYFSM(ENTRY_EVENT);
}
//printf("Leaving RunMotorYFSM with CurrentState = %d\n", CurrentState);
ES_Tail(); // trace call stack end
return ThisEvent;
}
/**
* @Function RunTimerService(ES_Event ThisEvent)
* @param ES_Event - the event to process
* @return ES_NO_EVENT or ES_ERROR
* @brief accepts the timer events and updates the state arrays
* @author Max Dunne 2013.01.04 */
ES_Event RunTimerService(ES_Event ThisEvent) {
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
switch (ThisEvent.EventType) {
case ES_INIT:
break;
case ES_TIMEOUT:
case ES_TIMERACTIVE:
case ES_TIMERSTOPPED:
UserTimerStates[ThisEvent.EventParam] = ThisEvent.EventType;
break;
default:
//ReturnEvent.EventType = ES_ERROR;
break;
}
#ifdef TIMER_SERVICE_TEST
{
uint8_t i;
switch (timerServiceTestingState) {
case init:
if (ThisEvent.EventType == ES_INIT) {
printf("Timer Module INITED succesfully\r\n");
break;
}
printf("Timer %d had event %d at time %d\r\n", ThisEvent.EventParam, ThisEvent.EventType, ES_Timer_GetTime());
if ((ThisEvent.EventParam == (TIMERS_USED - 1)) && (ThisEvent.EventType == ES_TIMERACTIVE)) {
timerServiceTestingState = expired;
printf("Testing timer user functions [expired][stopped][active]{state}\r\n");
}
break;
case expired:
for (i = 0; i < TIMERS_USED; i++) {
printf("(%d):[%d,%d,%d]{%d} ", i, IsTimerExpired(i), IsTimerStopped(i), IsTimerActive(i), GetUserTimerState(i));
}
printf("\r\n");
if ((ThisEvent.EventParam == (TIMERS_USED - 1)) && (ThisEvent.EventType == ES_TIMEOUT)) {
timerServiceTestingState = runstop;
ES_Timer_InitTimer(0, 500);
for (i = 1; i < TIMERS_USED; i++) {
ES_Timer_SetTimer(i, 1000);
ES_Timer_StartTimer(i);
}
}
break;
case runstop:
printf("Timer %d had event %d at time %d\r\n", ThisEvent.EventParam, ThisEvent.EventType, ES_Timer_GetTime());
if ((ThisEvent.EventParam == (0)) && (ThisEvent.EventType == ES_TIMEOUT)) {
for (i = 1; i < TIMERS_USED; i++) {
ES_Timer_StopTimer(i);
}
}
if ((ThisEvent.EventParam == (TIMERS_USED - 1)) && (ThisEvent.EventType == ES_TIMERSTOPPED)) {
printf("Testing of User Timer Functions is complete.\r\n");
}
break;
default:
ReturnEvent.EventType = ES_ERROR;
break;
}
}
#endif
//ES_Tail();
return ReturnEvent;
}
ES_Event RunLabFSM( ES_Event ThisEvent )
{
static char speedByte;
static char dirByte;
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
switch ( CurrentState )
{
case InitPState:
{
if ( ThisEvent.EventType == ES_INIT )// only respond to EF_Init
{
initAll();
ES_Timer_InitTimer(LED_TIMER,LED_TIME);
CurrentState = NoController;
}
break;
}
case NoController:
{
if(ThisEvent.EventType == ES_ControlReq)
{
sendControlReq((uchar)ThisEvent.EventParam);
ES_Timer_InitTimer(TIMEOUT_TIMER,3000); //start the timeout timer
}
if(ThisEvent.EventType==ES_ControlBypass)
{
sendControlReq((uchar)ThisEvent.EventParam);
ES_Timer_InitTimer(TIMEOUT_TIMER,3000); //start the timeout timer
CurrentState=Controlling;
ES_Timer_StopTimer(LED_TIMER);
offAllLEDs();
printf("bypass- move to Control State\r\n");
debugMode=1;
}
if(ThisEvent.EventType == ES_ControlAck)
{
if(currHzvNum< NUM_HZVS)
{
CurrentState=Controlling;
offAllLEDs();
ES_Timer_StopTimer(LED_TIMER);
sendMessage(sendData,2,0x21, 0x80 + currHzvNum);
ES_Timer_InitTimer(ACK_TIMER,1000);
ES_Timer_InitTimer(COMMAND_TIMER,200);
printf("Moving to Controlling State\r\n");
}
else
{
printf("hzv num bad, got cont ack before sent req %i\r\n", (int) currHzvNum);
}
}
if(ThisEvent.EventType==ES_TIMEOUT) //retry sending controlREq command
{
if(ThisEvent.EventParam==COMMAND_TIMER)
{
sendControlReq(currHzvNum);
printf("retrying\r\n");
}
else if(ThisEvent.EventParam==TIMEOUT_TIMER)
{
currHzvNum=NUM_HZVS;
ES_Timer_StopTimer(COMMAND_TIMER);
printf("time out!\r\n");
}
else if(ThisEvent.EventParam==LED_TIMER)
{
if(bgIsDone())
advanceLEDs();
else
advanceCalibLEDs();
ES_Timer_InitTimer(LED_TIMER,LED_TIME);
}
}
break;
}
case Controlling:
{
if(ThisEvent.EventType==ES_Ack)
{
ES_Timer_InitTimer(ACK_TIMER,1000);
printf("SM GOT ACK\r\n");
}
if(ThisEvent.EventType==ES_TIMEOUT)
{
if(ThisEvent.EventParam==ACK_TIMER && !debugMode) //dont time out on debug mode
{
printf("TIMED OUT!--------------------------------\r\n");
CurrentState=NoController;
currHzvNum=NUM_HZVS;
ES_Timer_InitTimer(LED_TIMER,LED_TIME);
}
else if(ThisEvent.EventParam==COMMAND_TIMER)
{
sendData[0]=0x02;
// getCommands(sendData+1,sendData+2); //sets commands 1 and 2
lightDrumLEDs(getBucket(LEFT),getBucket(RIGHT));
getCommands(&speedByte,&dirByte) ;
sendData[1]=speedByte;
sendData[2]=dirByte;
sendData[3]=shouldAttack(); //HI when atk pin is hi
sendData[4]=0; //custom data is all 0s
sendData[5]=0;
sendData[6]=0;
sendMessage(sendData,7,0x21,0x80+currHzvNum);
ES_Timer_InitTimer(COMMAND_TIMER,200);
printf("Send: s:%i t:%i a:%i\r\n",(char) sendData[1],(char) sendData[2], (char) sendData[3]);
}
}
if(ThisEvent.EventType==ES_ControlBypass)
{
printBgSaves();
}
break;
}
}
return ReturnEvent;
}
/**
* @Function RunRamSubHSM(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 RunTapeSubHSM(ES_Event ThisEvent)
{
uint8_t makeTransition = FALSE; // use to flag transition
TapeState_t nextState;
EventStorage args;
ES_Tattle(); // trace call stack
switch (CurrentState) {
case Tape_Init:
if (ThisEvent.EventType == ES_INIT)
{
// Make sure timer isnt going to be running
ES_Timer_StopTimer(TAPE_SUB_HSM_TIMER);
// now put the machine into the actual initial state
CurrentState = Ram_Begin;
makeTransition = FALSE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case Tape_Searching:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Drive forward
Drive_Forward(MOTOR_SPEED_CRAWL);
// Set up frustration timer
ES_Timer_InitTimer(TAPE_SUB_HSM_TIMER, TAPE_FRUSTRATION_TIMER);
break;
case ES_EXIT:
// Stop
Drive_Stop();
// Stop timer
ES_Timer_StopTimer(TAPE_SUB_HSM_TIMER);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == TAPE_SUB_HSM_TIMER) {
nextState = Tape_Reversing;
makeTransition = TRUE;
// Consume
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case TAPE:
args.val = ThisEvent.EventParam;
// Cancel timer for now, tape has been seen
ES_Timer_StopTimer(TAPE_SUB_HSM_TIMER);
if (args.bits.event & args.bits.type & TAPE_CENTER) {
// If center connects, begin to follow the tape
nextState = Tape_Follow_Front;
makeTransition = TRUE;
} else if ((args.bits.event & args.bits.type & TAPE_FRONT_LEFT) &&
(~args.bits.type & TAPE_FRONT_RIGHT)) {
// If front left entered tape and right is off tape
Drive_Left(MOTOR_SPEED_CRAWL);
} else if ((args.bits.event & args.bits.type & TAPE_FRONT_RIGHT) &&
(~args.bits.type & TAPE_FRONT_LEFT)) {
// If front right entered tape and left is off tape
Drive_Right(MOTOR_SPEED_CRAWL);
} else if (~args.bits.type & (TAPE_FRONT_RIGHT | TAPE_FRONT_LEFT)) {
// If front right and left are both off tape
Drive_Forward(MOTOR_SPEED_CRAWL);
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Tape_Reversing:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Start reversing
Drive_Forward(-MOTOR_SPEED_CRAWL);
// Set timer
ES_Timer_InitTimer(TAPE_SUB_HSM_TIMER, 1000);
break;
case ES_EXIT:
// Stop
Drive_Stop();
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == TAPE_SUB_HSM_TIMER) {
nextState = Tape_Turning;
makeTransition = TRUE;
// Consume
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Tape_Turning:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Start turning CW 45
Drive_TankRight(MOTOR_SPEED_EXPLORE);
// Set timer
ES_Timer_InitTimer(TAPE_SUB_HSM_TIMER, MOTOR_TURN_EX_45);
break;
case ES_EXIT:
// Stop
Drive_Stop();
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == TAPE_SUB_HSM_TIMER) {
nextState = Tape_Searching;
makeTransition = TRUE;
// Consume
ThisEvent.EventType = ES_NO_EVENT;
}
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Tape_Follow_Front:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
// Start driving straight
Drive_Forward(MOTOR_SPEED_CRAWL);
break;
case ES_EXIT:
// Do nothing
break;
case TAPE:
args.val = ThisEvent.EventParam;
if (args.bits.event & args.bits.type & TAPE_FRONT_LEFT) {
// If front left on tape
Drive_Left(MOTOR_SPEED_CRAWL);
} else if (args.bits.event & args.bits.type & TAPE_FRONT_RIGHT) {
// If front right on tape
Drive_Right(MOTOR_SPEED_CRAWL);
} else if (args.bits.event & ~args.bits.type &
(TAPE_FRONT_LEFT | TAPE_FRONT_RIGHT)) {
// If front left off tape
Drive_Forward(MOTOR_SPEED_CRAWL);
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Tape_Follow_Back:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
break;
case ES_EXIT:
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Tape_Done_State:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
break;
case ES_EXIT:
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
RunTapeSubHSM(EXIT_EVENT);
CurrentState = nextState;
RunTapeSubHSM(ENTRY_EVENT);
}
ES_Tail(); // trace call stack end
return ThisEvent;
}
/**
* @Function RunTemplateSubHSM(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 RunAlignCollect_SubHSM(ES_Event ThisEvent) {
uint8_t makeTransition = FALSE; // use to flag transition
SubAlignCollect_SubHSMState_t nextState; // <- change type to correct enum
ES_Tattle(); // trace call stack
ES_Event Tcontrol;
static int aligning_start_time, aligning_end_time;
static int aligning_difference_time = 0;
static int back_tape_flag = 0;
switch (CurrentState) {
case InitPSubState: // If current state is initial Psedudo State
if (ThisEvent.EventType == ES_INIT)// only respond to ES_Init
{
nextState = Buffer_State;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case Buffer_State:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
//PivotRight(newspeed);
break;
case ES_EXIT:
break;
case FORCED:
nextState = Checking;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
// ThisEvent.EventParam = ThisEvent.EventParam;
break;
case ES_TIMEOUT:
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
break;
default:
break;
}
}
break;
case Checking:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
PivotRight(speed*1.2);//(speed*.5);
// TurnRight(speed);
// StraightForward(speed);
// ES_Timer_InitTimer(replungetimer, 300);
break;
case ES_EXIT:
break;
case TAPE:
if ((ThisEvent.EventParam & 0x01) == 0x01) { // B
printf("\nback on\n");
aligning_start_time = ES_Timer_GetTime();
back_tape_flag = 1;
int i;
for(i = 0; i < 100000; i++)
{
asm("nop");
}
// nextState = Pivoting_Right_AC;
// makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if (((ThisEvent.EventParam & 0b000001) == 0b000000) && back_tape_flag) { // Back tape off T
aligning_end_time = ES_Timer_GetTime();
FullStop();
printf("\nback tape off\n");
int i;
for(i = 0; i < 100000; i++)
{
asm("nop");
}
aligning_difference_time = aligning_end_time - aligning_start_time;
// printf("\n\naligning_diff_time %d\n", aligning_difference_time);
nextState = Pivoting_Left_AC;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}// if ((ThisEvent.EventParam & 0b110000) == 0b110000) { // LL, L: if miss most of the stem/land on T top
// nextState = Pivoting_Right_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
// else if ((ThisEvent.EventParam & 0b111100) == 0b111100) { // LL, L,F, R: if hit T stem head-on
// nextState = Turning_Right_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// } else if ((ThisEvent.EventParam & 0b100000) == 0b100000) { // LL entry
// nextState = Turning_Left_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
else {
ThisEvent.EventType = ES_NO_EVENT;
}
// else if ((ThisEvent.EventParam & T_Left) == T_Left) {
// nextState = Pivoting_Right;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// } else if ((ThisEvent.EventParam & T_Right) == T_Right) {
// nextState = Pivoting_Left;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
break;
case ES_TIMEOUT:
// nextState = Pivoting_Right_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
break;
default:
break;
}
}
break;
case Pivoting_Right_AC:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
PivotRight(tspeed * 1);
ES_Timer_InitTimer(replungetimer, 900);
break;
// case TAPE:
// if ((ThisEvent.EventParam & 0x01) == 0x01) { // B
// nextState = Reverse_Adjusting_Right;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
// else {
// ThisEvent.EventType = ES_NO_EVENT;
// }
// break;
case ES_EXIT:
FullStop();
break;
case ES_TIMEOUT:
// nextState = Lost;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
if (ThisEvent.EventParam == replungetimer) {
nextState = Reverse_Adjusting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default:
break;
}
}
break;
case Pivoting_Left_AC:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
PivotLeft_WH(speed * .7);
int spintime = aligning_difference_time / 2;
// printf("\n\n%dcheck spin time\n", spintime);
ES_Timer_InitTimer(replungetimer, spintime);
break;
case ES_EXIT:
FullStop();
break;
case ES_TIMEOUT:
// nextState = Lost;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
if (ThisEvent.EventParam == replungetimer) {
FullStop();
// printf("\n\ncheck\n");
nextState = Reverse_Adjusting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default:
break;
}
}
break;
case Turning_Left_AC:
//ThisEvent = Run_AA_LineFollowing_SubHSM(ThisEvent); // run sub-state machine for this state
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
TurnLeft(speed);
break;
case TAPE:
if ((ThisEvent.EventParam & 0b000100) == 0b000100) { // R
nextState = Turning_Right_AC; //TODO: straight on
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_EXIT:
FullStop();
break;
case ES_TIMEOUT:
// ThisEvent.EventType = ES_NO_EVENT;
default: break;
}
}
break;
case Turning_Right_AC:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
TurnRight(speed);
break;
// case BUMPED:
// nextState = Wall_Hugging;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// break;
case TAPE:
if ((ThisEvent.EventParam & 0x01) == 0x01) { // B
nextState = Reverse_Adjusting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_EXIT:
FullStop();
break;
case ES_TIMEOUT:
// nextState = Lost;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
break;
default: break;
}
}
break;
case Stopping_AC:
FullStop();
break;
case Reversing_AC:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
StraightReverse(speed * 1.3);
ES_Timer_InitTimer(ammofindtimer, ammofindtimervalue);
break;
case ES_EXIT:
ES_Timer_StopTimer(ammofindtimer);
ES_Timer_StopTimer(portalsearchingtimer);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == ammofindtimer) {
ES_Timer_InitTimer(portalsearchingtimer, 600);
StraightForward(speed);
ThisEvent.EventType = ES_NO_EVENT;
} else if (ThisEvent.EventParam == portalsearchingtimer) {
FullStop();
Tcontrol.EventType = AMMO;
Tcontrol.EventParam = 0;
ThisEvent.EventType = ES_NO_EVENT;
PostDagobotHSM(Tcontrol);
}
break;
default:
break;
}
}
break;
case Forwarding_AC:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
StraightForward(speed);
break;
case TAPE:
if ((ThisEvent.EventParam & 0b001000) == 0b000000) { // Front off the T
nextState = Reversing_AC;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_EXIT:
break;
case ES_TIMEOUT:
break;
default:
break;
}
}
break;
case Reverse_Adjusting_Right:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
// GentleLeft(-6 - .3); // backwards left = front of robot pivoting right
GentleRight(-6 - .3);
ES_Timer_InitTimer(replungetimer, replungetimervalue);
break;
// case TAPE:
// if ((ThisEvent.EventParam & 0b111110) == 0b000000) { // F
// nextState = Stopping_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// } else {
// ThisEvent.EventType = ES_NO_EVENT;
// }
// break;
case ES_EXIT:
ES_Timer_StopTimer(replungetimer);
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == replungetimer) {
nextState = Forwarding_AC;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default:
break;
}
}
break;
case Adjusting_Right_AC:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
GentleRight(speed);
break;
case TAPE:
if ((ThisEvent.EventParam & 0b011100) == 0b011100) { // L, C, R
nextState = Reverse_Adjusting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_EXIT:
break;
}
}
break;
case Reverse_Turning_Left:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
//ES_Timer_InitTimer(pivotingtimer, pivotingtimervalue);
ReverseLeft(speed); // fiz thinks this means reverse right // previously 5
ES_Timer_InitTimer(ammofindtimer, ammofindtimervalue);
break;
// case TAPE:
// if ((ThisEvent.EventParam & 0b011100) == 0b011100){ // L, C, R
// nextState = Reverse_Adjusting_Right;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
// break;
//
case ES_TIMEOUT:
if (ThisEvent.EventParam == ammofindtimer) {
Tcontrol.EventType = MISSED_T;
Tcontrol.EventParam = 0;
PostDagobotHSM(Tcontrol);
ThisEvent.EventType = ES_NO_EVENT;
}
// else if (plungecount == 4){
// debug("Oh, shit!");
// nextState = Stopping_AC;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// //TODO: Deal with later!
// // post event to pop into higher state
// }
break;
case ES_EXIT:
ES_Timer_StopTimer(ammofindtimer);
break;
}
}
break;
default: // all unhandled states fall into here
break;
} // end switch on Current State
if (makeTransition == TRUE) {
RunAlignCollect_SubHSM(EXIT_EVENT); // <- rename to your own Run function
CurrentState = nextState;
RunAlignCollect_SubHSM(ENTRY_EVENT); // <- rename to your own Run function
}
ES_Tail(); // trace call stack end
return ThisEvent;
}
/**
* @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;
}
/**
* @Function RunEngage_DestroySubHSM(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 RunEngage_DestroySubHSM(ES_Event ThisEvent) {
uint8_t makeTransition = FALSE; // use to flag transition
SubEngage_DestroyState_t nextState;
ES_Event eventtopost, donefiring;
ES_Tattle(); // trace call stack
switch (CurrentState) {
case Init_ED: // If current state is initial Psedudo State
if (ThisEvent.EventType == ES_INIT) {// only respond to ES_Init
//call all substate run functions here
InitEngage_Destroy_Wall_Hugging_SubHSM();
nextState = Engage_Destroy_Searching;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case Engage_Destroy_Searching:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
// ES_Timer_InitTimer(15, 2500); //TODO: Move to exit from acquiring_ammo
// PivotRight(speed);
// PivotRight(speed);
StraightForward(speed); //speed // move forwards away from the ammo dump
//pivot timer
// start pivoting
Dagobot_SetLoaderServo(1200);
ThisEvent.EventType = ES_NO_EVENT;
// ThisEvent.EventParam = 0;
// PostDagobotHSM(ThisEvent);
//TODO: Add flag to only go forwards if just from Acquiring_Ammo?
break;
// case AMMO:
// ThisEvent.EventType = ES_NO_EVENT;
// break;
case ES_EXIT:
FullStop(); // aimed at the enemy: stop and fire
ES_Timer_StopTimer(15);
// ES_Timer_StopTimer(forwardtimer);
break;
case SNIPED:
// debug("check event");
FullStop();
nextState = Firing;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
break;
case BUMPED:
nextState = Engage_Destroy_Wall_Hugging;
makeTransition = TRUE;
eventtopost.EventType = BUMP_CHECK;
eventtopost.EventParam = ThisEvent.EventParam;
// ThisEvent.EventType = BUMP_CHECK;
// ThisEvent.EventParam = ThisEvent.EventParam;
PostDagobotHSM(eventtopost);
break;
//
// case TAPE:
// nextState = Redirecting;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// break;
case ES_TIMEOUT:
//if (ThisEvent.EventParam == forwardtimer) {
// ES_Timer_InitTimer(pivotingtimer, 800);
// PivotRight(newspeed);
//}
// if (ThisEvent.EventParam == 15) {
// FullStop(); // stop after spinning
// StraightForward(speed*1.25); // didn't find enemy, head towards obstacle (or at least forwards)
// }
// ThisEvent.EventType = ES_NO_EVENT;
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Firing:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is active
switch (ThisEvent.EventType) {
case ES_ENTRY:
// int AmmoState = Dagobot_ReadLoaderTape();
// if (AmmoState == 1) { // have ammo!
// debug("check");
FullStop();
ES_Timer_InitTimer(ammofindtimer, 2000); // head towards deadbot
StraightForward(speed);
// }
break;
// case SNIPED:
// if(ThisEvent.EventParam == 0)
// {
//
// }
// case BUMPED:
// if ((ThisEvent.EventParam & FRONT_LEFT_BUMP) == FRONT_LEFT_BUMP) { // front left bump
//
// ThisEvent.EventType = ES_NO_EVENT;
// } else if ((ThisEvent.EventParam & FRONT_RIGHT_BUMP) == FRONT_RIGHT_BUMP) { // front right bump
// nextState = Reversing_WH;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// } else if ((ThisEvent.EventParam & FRONT_BUMPS) == FRONT_BUMPS) { // both front bumpers
// nextState = Reversing_WH;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
// else {
// ThisEvent.EventType = ES_NO_EVENT;
// }
// break;
case ES_EXIT:
// ES_TIMER_StopTimer(motortimer);
Dagobot_SetLoaderServo(1200);
ES_Timer_StopTimer(shootingtimer);
ES_Timer_StopTimer(ammofindtimer);
// ThisEvent.EventType = ES_NO_EVENT; //TODO: remove??
break;
case ES_TIMEOUT:
if(ThisEvent.EventParam == ammofindtimer){
FullStop();
int i;
for(i = 0; i < 30000; i++)
{
asm("nop");
}
Dagobot_SetLoaderServo(2000); // start running up motor
ES_Timer_InitTimer(shootingtimer, shootingtimervalue);
ThisEvent.EventType = ES_NO_EVENT;
}
// if (ThisEvent.EventParam == motortimervalue) {//done spinning up motor
// Dagobot_SetLoaderServo(servorelease); // drop balls into shooter
// ES_Timer_InitTimer(shootingtimer, shootingtimervalue);
// }
if (ThisEvent.EventParam == shootingtimer) {
donefiring.EventType = DONE_FIRING; // upper level deal with getting to portal
donefiring.EventParam = 0;
PostDagobotHSM(donefiring);
ThisEvent.EventType = ES_NO_EVENT;
}
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Engage_Destroy_Wall_Hugging:
ThisEvent = RunEngage_Destroy_Wall_Hugging_SubHSM(ThisEvent); // run sub-state machine for this state
//TODO: make new wall_hugging file/fn?
// and/or deal with entry from differet events
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is active
switch (ThisEvent.EventType) {
case ES_ENTRY:
// this is where you would put any actions associated with the
// entry to this state
// ES_Timer_InitTimer(wallhuggingtimer, 8000);
break;
case SNIPED:
FullStop();
nextState = Firing;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
break;
// case TAPE:
// if((ThisEvent.EventParam & 0x000010) != 0x000010) {
// nextState = Redirecting;
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
// }
// break;
case ES_EXIT:
// this is where you would put any actions associated with the
// exit from this state
// ES_Timer_StopTimer(wallhuggingtimer);
FullStop();
break;
case ES_TIMEOUT:
// if (ThisEvent.EventParam == wallhuggingtimer) { // timer to check for enemy every once in a while while bumping around
// //TODO: make new checkin timer?
// nextState = Engage_Destroy_Searching; // search
// makeTransition = TRUE;
// ThisEvent.EventType = ES_NO_EVENT;
//
// }
break;
default: // all unhandled events pass the event back up to the next level
break;
}
}
break;
case Redirecting:
if (ThisEvent.EventType != ES_NO_EVENT) {
switch (ThisEvent.EventType) {
case ES_ENTRY:
PivotRight(speed);
ES_Timer_InitTimer(pivotingtimer, pivotingtimervalue);
break;
case ES_EXIT:
ES_Timer_StopTimer(pivotingtimer);
// ThisEvent.EventType = ES_NO_EVENT;
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam = pivotingtimer) {
nextState = Engage_Destroy_Searching; // search
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT; // head for whatever's in the future
}
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
RunEngage_DestroySubHSM(EXIT_EVENT); // <- rename to your own Run function
CurrentState = nextState;
RunEngage_DestroySubHSM(ENTRY_EVENT); // <- rename to your own Run function
}
ES_Tail(); // trace call stack end
return ThisEvent;
}
/****************************************************************************
Function
RunLobbyistSM
Parameters
ES_Event : the event to process
Returns
ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise
Description
Deals with the high level state transitions of the Lobbyist state machine.
****************************************************************************/
ES_Event RunLobbyistSM( ES_Event ThisEvent )
{
ES_Event ReturnEvent;
ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
switch ( CurrentState )
{
case InitPState : // If current state is initial Psedudo State
if ( ThisEvent.EventType == ES_INIT ) // only respond to ES_Init
{
//Prep the timer hardware.
printf("Init Steering \r\n");
InitSteering(); // Initialize the steering systems
printf("Init Packets \r\n");
InitPackets(); // Initialize packet array
printf("Init XBee \r\n");
InitXBee(); // Initialize the comms
printf("Init Lift Fan \r\n");
InitLiftFan(); // Initialize the lift fan
//Initialize the DMC.
CurrentState = WaitingForPair;
}
break;
case WaitingForPair :
switch ( ThisEvent.EventType )
{
//If the event is a pair request, change state to pairing and wait for the key.
case MESSAGE_RECEIVED:
// Read incoming message
message = GetMessageData();
// Check that message is a pair request and that it's meant for us
if(*message == PAIR_REQUEST_HEADER && CheckLobbyistNumber() && GetApiIdentifier() == INCOMING_PACKET_API_IDENTIFIER){ // Ensure that this is not a Tx success receive
printf("\r\n Current Message Size: %d ", GetMessageDataSize());
// If we passed the checks, we're now paired. Start timers.
printf("\n\r\nPair request received in unpaired state.");
ES_Timer_InitTimer(UNPAIR_TIMER, UNPAIR_TIME); //Start the 45s timer
ES_Timer_InitTimer(PAIR_FAIL_TIMER, ONE_SEC); //Start the 1s timer
TurnOnLiftFan(); //Start the lift fan
//Update DMC
// Recognize our new pac daddy
pairedAddress = GetSenderAddress() ;
// Send an ACK back to the sender
TransmitStatusPacket(PAIRED);
// Set the pair time
PairTimeLeft = 45;
CurrentState = PairedAwaitEncryptionKey;
}
break;
default :
;
}
break;
case PairedAwaitEncryptionKey:
switch(ThisEvent.EventType){
case MESSAGE_RECEIVED:
// Check sender and message
message = GetMessageData();
uint16_t sender = GetSenderAddress() ;
if(sender == pairedAddress && *message == KEY_SEND_HEADER){
printf("\n\r\nKey received in pairing state.");
//Save the key
SetKey((message+1)); // Make sure to +1 to start after header byte
ES_Timer_InitTimer(PAIR_FAIL_TIMER, ONE_SEC); //Restart the 1s timer
TransmitStatusPacket(PAIRED); //transmit status with the pairing bit set.
CurrentState = PairedAwaitControl;
printf("\r\n In paired await control");
}
break;
case ES_TIMEOUT: // if there is a timeout on the 1s timer
printf("\n\r\n1s timeout in pairing - moving to awaiting pair.");
TurnOffLiftFan(); //Turn off the lift fan
//Update DMC to avaiable for pairing
ES_Timer_StopTimer(UNPAIR_TIMER); //Disable the 45s pairing timer
ES_Timer_StopTimer(PAIR_FAIL_TIMER); //Disable the 1s transmit timeout timer
TransmitStatusPacket(UNPAIRED); //Transmit status with the pairing bit cleared.
CurrentState = WaitingForPair;
break;
}
break;
case PairedAwaitControl:
switch(ThisEvent.EventType){
case MESSAGE_RECEIVED:
// See who sent the message
uint16_t sender = GetSenderAddress() ;
if(sender == pairedAddress){ // Only attempt decrypt if from sender
message = DecryptMessage(GetMessageData(),GetMessageDataSize());
RotateCipher();
// printf("\r\n Rotating Key");
if(*message == CNTRL_REQUEST_HEADER){
//printf("\n\r\nCommand received in the paired state.");
ES_Timer_InitTimer(PAIR_FAIL_TIMER, ONE_SEC); //Restart the control command timer.
ExecuteControl(); //Execute the control command.
TransmitStatusPacket(PAIRED);// ACK YAY. Transmit the status with pairing bit set.
} else {
printf("\n\r Not a valid control packet received, Header: %d", *message);
}
}
break;
case ES_TIMEOUT: //Either the 1s or 45s timer
case MANUAL_UNPAIR: // Or if the pair is manual
case DECRYPT_FAIL: // Or if the decryption failed
if(ThisEvent.EventParam == UNPAIR_TIMER){
// Keep track of how long is left on UNPAIR_TIMER
PairTimeLeft--;
printf("\n\r\nTime until unpair: %ds", PairTimeLeft);
// If timer has run for 45 s
if(PairTimeLeft == 0){
CurrentState = WaitingForPair;
TurnOffLiftFan(); //Turn off the lift fan
UpdateSteering(OFF,OFF); //Turn off the propulsion fans
//Update DMC to avaiable for pairing
ES_Timer_StopTimer(UNPAIR_TIMER); //Disable the 45s pairing timer
ES_Timer_StopTimer(PAIR_FAIL_TIMER); //Disable the 1s transmit timeout timer
printf("\n\r\nUnpairing.");
// If timer is not at 0, reinitialize the timer again for 1s to keep counting
}else{
ES_Timer_InitTimer(UNPAIR_TIMER, UNPAIR_TIME); //Start the 45s timer
}
} else {
// If we have an event NOT on the 45s timer, unpair
printf("\n\r\nTimeout in the paired state. Moving to waiting for pair \r\n");
TurnOffLiftFan(); //Turn off the lift fan
UpdateSteering(OFF,OFF); //Turn off the propulsion fans
//Update DMC to avaiable for pairing
ES_Timer_StopTimer(UNPAIR_TIMER); //Disable the 45s pairing timer
ES_Timer_StopTimer(PAIR_FAIL_TIMER); //Disable the 1s transmit timeout timer
if(ThisEvent.EventType == DECRYPT_FAIL){ // If this event came from a decryption failure //// WEHERE DOES THIS COME FROM?
TransmitStatusPacket(DECRYPT_FAILURE); // Transmit status with the decryption fail bit set
} else {
TransmitStatusPacket(UNPAIRED); //Transmit status with the pairing bit cleared.
}
CurrentState = WaitingForPair;
}
break;
}
break;
default :
break;
}
return ReturnEvent;
}
/**
* @Function RunLine_FollowingSubHSM(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 Run_AA_LineFollowing_SubHSM(ES_Event ThisEvent) {
uint8_t makeTransition = FALSE; // use to flag transition
AA_LineFollowing_subHSM_t nextState; // <- change type to correct enum
ES_Tattle(); // trace call stack
switch (CurrentState) {
case Init_AA_LineFollowing: // If current state is initial Psedudo State
if (ThisEvent.EventType == ES_INIT)// only respond to ES_Init
{
nextState = EntryLineFollowing;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case EntryLineFollowing:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
StraightForward(speed);
break;
case ES_EXIT:
break;
case TAPE:
if (ThisEvent.EventParam == 0b100000) { // LL: 20, 100000
FullStop();
nextState = Adjusting_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if (ThisEvent.EventParam == 0b000010) { // RR: 02
FullStop();
nextState = Pivoting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b110010) == 0b110010) { // LL, L, RR
FullStop();
nextState = Reversing_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b001001) == 0b001001) { // F, B
FullStop();
nextState = Adjusting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
}
}
break;
case Adjusting_Left:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
TurnLeft(speed * 1);
break;
case TAPE:
if ((ThisEvent.EventParam & 0b000010) == 0b000010) { // RR (corner)
nextState = Pivoting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b001000) == 0b001000) { //F
FullStop();
nextState = Adjusting_Right; //Adjusting_Right
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_EXIT:
break;
}
}
break;
case Adjusting_Right:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
TurnRight(speed * 1);
break;
case TAPE:
if ((ThisEvent.EventParam & 0b010000) == 0b010000) { // L
FullStop();
nextState = Adjusting_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b100000) == 0b100000) { // LL
FullStop();
nextState = Adjusting_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b000010) == 0b000010) { // RR (corner)
nextState = Pivoting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b000110) == 0b000110) { //R, RR (corner)
nextState = Pivoting_Right;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_EXIT:
break;
}
}
break;
case Pivoting_Right:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
ES_Timer_InitTimer(ammofindtimer, 2000);
PivotRight(speed);
break;
case ES_EXIT:
ES_Timer_StopTimer(ammofindtimer);
break;
case TAPE:
if ((ThisEvent.EventParam & 0b110010) == 0b110010) { // LL, L, RR
nextState = Reversing_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b010000) == 0b010000) { // L
FullStop();
nextState = Adjusting_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b100001) == 0b100001) { // LL, B
nextState = Adjusting_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else if ((ThisEvent.EventParam & 0b100000) == 0b100000) { // LL
nextState = Adjusting_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
else {
ThisEvent.EventType = ES_NO_EVENT;
}
break;
case ES_TIMEOUT:
if (ThisEvent.EventParam == ammofindtimer) {
nextState = EntryLineFollowing;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
}
break;
}
}
break;
case Reversing_Left:
if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
switch (ThisEvent.EventType) {
case ES_ENTRY:
ES_Timer_InitTimer(turningtimer, turningtimervalue);
TurnLeft(-(speed));
break;
case ES_EXIT:
ES_Timer_StopTimer(turningtimer);
break;
case ES_TIMEOUT:
nextState = Adjusting_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
case TAPE:
if ((ThisEvent.EventParam & 0b100000) == 0b100000) { // LL
nextState = Adjusting_Left;
makeTransition = TRUE;
ThisEvent.EventType = ES_NO_EVENT;
} else {
ThisEvent.EventType = ES_NO_EVENT;
}
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
Run_AA_LineFollowing_SubHSM(EXIT_EVENT);
CurrentState = nextState;
Run_AA_LineFollowing_SubHSM(ENTRY_EVENT);
}
ES_Tail(); // trace call stack end
return ThisEvent;
}