예제 #1
0
/**
 * @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;
}
예제 #2
0
/**
 * @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;
}
예제 #4
0
/**
 * @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;
}