/**
* @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 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 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;
}