예제 #1
0
static ES_Event DuringAttack_t( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assme no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) || (Event.EventType == ES_ENTRY_HISTORY) )
    {
        // implement any entry actions required for this state machine
				// Stop positioning when in the attack SM
				ES_Timer_StopTimer(POSITION_CHECK);
        // after that start any lower level machines that run in this state
        //StartLowerLevelSM( Event );
				StartAttackSM(Event);
        // repeat the StartxxxSM() functions for concurrent state machines
        // on the lower level
    }
    else if ( Event.EventType == ES_EXIT )
		{
			// on exit, give the lower levels a chance to clean up first
			//RunLowerLevelSM(Event);
			ReturnEvent = RunAttackSM(Event);
			// repeat for any concurrently running state machines
			// now do any local exit functionality
			
			// Count the number of shots, and don't start a next shot timer
			// if we are out of ammo
			shotCounter++;
			if (shotCounter < 5)
			{
				ES_Timer_InitTimer(ATTACK_PHASE_TIMER, NEXT_SHOT_T);
			}
			
			// Reset positioning
			ES_Event ResetEvent;
			ResetEvent.EventType = ES_RESET_DESTINATION;
			PostMasterSM(ResetEvent);
    }else
    // do the 'during' function for this state
    {
        // run any lower level state machine
        // ReturnEvent = RunLowerLevelSM(Event);
				ReturnEvent = RunAttackSM(Event);
        // repeat for any concurrent lower level machines
      
        // do any activity that is repeated as long as we are in this state
			
    }
    // return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}
예제 #2
0
/****************************************************************************
 Function
    RunPhotoTransistorService
 Parameters
   ES_Event : the event to process
 Returns
   ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise
****************************************************************************/
ES_Event RunPhotoTransistorService( ES_Event ThisEvent )
{
  ES_Event ReturnEvent;
  ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
  
	// If we have stopped seeing pulses, it's time to evaluate whether we saw a beacon
	if ((ThisEvent.EventType == ES_TIMEOUT) && (ThisEvent.EventParam == AVERAGE_BEACONS_TIMER))
	{
		// If the beacon we were interested in had enough pulses
		if (numSamples[LastBeacon] >= NUMBER_PULSES_TO_BE_ALIGNED)
		{
			// set the last update time for the beacon
			beacons[LastBeacon].lastUpdateTime = captureInterrupt(PHOTOTRANSISTOR_INTERRUPT_PARAMATERS);
			
			// set the angle to the beacon based on the average of all pulses measured
			beacons[LastBeacon].lastEncoderAngle = CalculateAverage(LastBeacon);
			
			// Store this beacon as the last updated beacon
			LastUpdatedBeacon = LastBeacon;
			
			// Determine if we should recalculate our position and angle based on whether or not we have received 3 consecutive pulses
			if (TimeForUpdate())
			{
				ES_Event NewEvent;
				NewEvent.EventType = ES_CALCULATE_POSITION;
				PostPositionLogicService(NewEvent);
			}
			
		}
		
		// Reset stored average information
		ResetAverage();
		
		// Reallow new beacons to be recorded
		Bucketing = true;
		
		LastBeacon = NULL_BEACON;
		
		ES_Timer_StopTimer(AVERAGE_BEACONS_TIMER);
	}
	else if (ThisEvent.EventType == ES_ALIGN_TO_BUCKET)
	{
		// If we are trying to align to the bucket for a shot, 
		//  shift the way we handle interrupts
		AligningToBucket = true;
		enableCaptureInterrupt(PHOTOTRANSISTOR_INTERRUPT_PARAMATERS);
	}
	
  return ReturnEvent;
}
예제 #3
0
//The interrupt response for our phototransistor
void PhotoTransistor_InterruptResponse(void)
{
	// Clear Interrupt
	clearCaptureInterrupt(PHOTOTRANSISTOR_INTERRUPT_PARAMATERS);
	
	// Determine Capture time
	uint32_t ThisCapture = captureInterrupt(PHOTOTRANSISTOR_INTERRUPT_PARAMATERS);
	
	// Calculate period
	uint32_t Period = ((ThisCapture - LastCapture) * MICROSECONDS_DIVISOR ) / TICKS_PER_MS;
	
	//Store the Last Cpature
	LastCapture = ThisCapture;
	
	//Iterate Through the different frequency options
	for (int i = 0; i < NUMBER_BEACON_FREQUENCIES; i++)
	{
		// If the period matches a beacon period
		if (ToleranceCheck(Period, beacons[i].period, PERIOD_MEASURING_ERROR_TOLERANCE))
		{
			// If we're searching for a beacon
			if (Bucketing)
			{
				buckets[i]++;
			}
			
			// If we're supposed to align to the bucket, and the number of pulses we've seen for
			//  this beacon is greater than our threshold
			if ((AligningToBucket) && (buckets[i] >= NUMBER_PULSES_FOR_BUCKET))
			{
				// If this is the beacon corresponding to our target bucket
				if (((MyColor() == COLOR_BLUE) && (i == BEACON_INDEX_NW)) || ((MyColor() == COLOR_RED) && (i == BEACON_INDEX_SE)))
				{ 
					// stop our drive
					clearDriveAligningToBucket();
					
					// Post an aligned event
					ES_Event AlignedEvent;
					AlignedEvent.EventType = ES_ALIGNED_TO_BUCKET;
					PostMasterSM(AlignedEvent);
					
					// stop aligning
					AligningToBucket = false;
					
					// reset all buckets
					for (int j = 0; j < NUMBER_BEACON_FREQUENCIES; j++)
					{
						buckets[j] = 0;
					}
					
					// reset average information
					ResetAverage();
					
					// return to searching for beacons
					Bucketing = true;
					LastBeacon = NULL_BEACON;
					ES_Timer_StopTimer(AVERAGE_BEACONS_TIMER);
					return;
				}
			}
			// If we're not aligning to a bucket, and the number of pulses we've seen for this
			//  beacon is greater than our threshold
			else if (buckets[i] >= NUMBER_PULSES_TO_BE_ALIGNED && LastBeacon == NULL_BEACON && !AligningToBucket)
			{
				// store the beacon to be recorded
				LastBeacon = i;
				
				// reset all buckets
				for (int j = 0; j < NUMBER_BEACON_FREQUENCIES; j++)
				{
					buckets[j] = 0;
				}
				
				// restart the timer to evaluate the beacon 
				ES_Timer_InitTimer(AVERAGE_BEACONS_TIMER, AVERAGE_BEACONS_T);
				Bucketing = false;
			}
			
			// increment the sum of all encoder angles for this beacon
			sums[i] += GetPeriscopeAngle();
			
			// increment the number of pulses seen for this beacon
			numSamples[i]++;
			
			// If the evaluation timer is already running, restart it
			ES_Timer_SetTimer(AVERAGE_BEACONS_TIMER, AVERAGE_BEACONS_T);
		}
	}
		
}
예제 #4
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;
}
예제 #5
0
/**
 * @Function RunTimerService(ES_Event ThisEvent)
 * @param ES_Event - the event to process
 * @return ES_NO_EVENT or ES_ERROR 
 * @brief  accepts the timer events and updates the state arrays
 * @author Max Dunne   2013.01.04 */
ES_Event RunTimerService(ES_Event ThisEvent) {
    ES_Event ReturnEvent;
    
    ReturnEvent.EventType = ES_NO_EVENT; // assume no errors

    switch (ThisEvent.EventType) {
        case ES_INIT:
            break;
        case ES_TIMEOUT:
        case ES_TIMERACTIVE:
        case ES_TIMERSTOPPED:
            UserTimerStates[ThisEvent.EventParam] = ThisEvent.EventType;
            break;
        default:
            //ReturnEvent.EventType = ES_ERROR;
            break;
    }

#ifdef TIMER_SERVICE_TEST
    {
        uint8_t i;
        switch (timerServiceTestingState) {
            case init:
                if (ThisEvent.EventType == ES_INIT) {
                    printf("Timer Module INITED succesfully\r\n");
                    break;
                }
                printf("Timer %d had event %d at time %d\r\n", ThisEvent.EventParam, ThisEvent.EventType, ES_Timer_GetTime());
                if ((ThisEvent.EventParam == (TIMERS_USED - 1)) && (ThisEvent.EventType == ES_TIMERACTIVE)) {
                    timerServiceTestingState = expired;
                    printf("Testing timer user functions [expired][stopped][active]{state}\r\n");
                }
                break;

            case expired:
                for (i = 0; i < TIMERS_USED; i++) {
                    printf("(%d):[%d,%d,%d]{%d} ", i, IsTimerExpired(i), IsTimerStopped(i), IsTimerActive(i), GetUserTimerState(i));
                }
                printf("\r\n");
                if ((ThisEvent.EventParam == (TIMERS_USED - 1)) && (ThisEvent.EventType == ES_TIMEOUT)) {
                    timerServiceTestingState = runstop;
                    ES_Timer_InitTimer(0, 500);
                    for (i = 1; i < TIMERS_USED; i++) {
                        ES_Timer_SetTimer(i, 1000);
                        ES_Timer_StartTimer(i);
                    }
                }
                break;

            case runstop:
                printf("Timer %d had event %d at time %d\r\n", ThisEvent.EventParam, ThisEvent.EventType, ES_Timer_GetTime());
                if ((ThisEvent.EventParam == (0)) && (ThisEvent.EventType == ES_TIMEOUT)) {
                    for (i = 1; i < TIMERS_USED; i++) {
                        ES_Timer_StopTimer(i);
                    }
                }
                if ((ThisEvent.EventParam == (TIMERS_USED - 1)) && (ThisEvent.EventType == ES_TIMERSTOPPED)) {
                    printf("Testing of User Timer Functions is complete.\r\n");
                }

                break;
            default:
                ReturnEvent.EventType = ES_ERROR;
                break;
        }

    }
#endif
    //ES_Tail();
    return ReturnEvent;
}
예제 #6
0
ES_Event RunLabFSM( ES_Event ThisEvent )
{
  static char speedByte;
  static char dirByte;
  ES_Event ReturnEvent;
  ReturnEvent.EventType = ES_NO_EVENT; // assume no errors                          
  
  switch ( CurrentState )                              
  {    

      case InitPState:
      { 
         if ( ThisEvent.EventType == ES_INIT )// only respond to EF_Init
         {
              initAll();
              ES_Timer_InitTimer(LED_TIMER,LED_TIME);
              CurrentState = NoController;
         }
         
          break;
      } 
      case NoController:
      {
         
         if(ThisEvent.EventType == ES_ControlReq)
         {
            sendControlReq((uchar)ThisEvent.EventParam);
            ES_Timer_InitTimer(TIMEOUT_TIMER,3000); //start the timeout timer 
         }
         if(ThisEvent.EventType==ES_ControlBypass)
         {
            sendControlReq((uchar)ThisEvent.EventParam);
            ES_Timer_InitTimer(TIMEOUT_TIMER,3000); //start the timeout timer
            CurrentState=Controlling;
            ES_Timer_StopTimer(LED_TIMER);
            offAllLEDs();
            printf("bypass- move to Control State\r\n");
            debugMode=1;
         }
         
         if(ThisEvent.EventType == ES_ControlAck)
         {         
            if(currHzvNum< NUM_HZVS)
            {
               CurrentState=Controlling;
               offAllLEDs();
               ES_Timer_StopTimer(LED_TIMER);
               sendMessage(sendData,2,0x21, 0x80 + currHzvNum);
               ES_Timer_InitTimer(ACK_TIMER,1000); 
               ES_Timer_InitTimer(COMMAND_TIMER,200);
               printf("Moving to Controlling State\r\n");
            } 
            else
            {
               printf("hzv num bad, got cont ack before sent req %i\r\n", (int) currHzvNum);
            }
         }
         if(ThisEvent.EventType==ES_TIMEOUT) //retry sending controlREq command
         {
            if(ThisEvent.EventParam==COMMAND_TIMER)
            {
               sendControlReq(currHzvNum);
               printf("retrying\r\n");
            }
            else if(ThisEvent.EventParam==TIMEOUT_TIMER)
            {
               currHzvNum=NUM_HZVS;
               ES_Timer_StopTimer(COMMAND_TIMER);
               printf("time out!\r\n");
            } 
            else if(ThisEvent.EventParam==LED_TIMER)
            {
               if(bgIsDone())
                   advanceLEDs();
               else
                   advanceCalibLEDs();
               ES_Timer_InitTimer(LED_TIMER,LED_TIME);
            }
         }
         
         
         break;
      } 
      case Controlling:
      {
         if(ThisEvent.EventType==ES_Ack)
         {
            ES_Timer_InitTimer(ACK_TIMER,1000); 
            printf("SM GOT ACK\r\n");
         }
         if(ThisEvent.EventType==ES_TIMEOUT)
         {
            if(ThisEvent.EventParam==ACK_TIMER && !debugMode) //dont time out on debug mode
            {
               printf("TIMED OUT!--------------------------------\r\n");
               CurrentState=NoController; 
               currHzvNum=NUM_HZVS;
			   ES_Timer_InitTimer(LED_TIMER,LED_TIME);			   
            } 
            else if(ThisEvent.EventParam==COMMAND_TIMER)
            {
               
               sendData[0]=0x02;
               
              // getCommands(sendData+1,sendData+2); //sets commands 1 and 2
               lightDrumLEDs(getBucket(LEFT),getBucket(RIGHT));
               getCommands(&speedByte,&dirByte) ;
               sendData[1]=speedByte;
               sendData[2]=dirByte;
               sendData[3]=shouldAttack(); //HI when atk pin is hi
               sendData[4]=0; //custom data is all 0s
               sendData[5]=0;
               sendData[6]=0;  
               sendMessage(sendData,7,0x21,0x80+currHzvNum);
               ES_Timer_InitTimer(COMMAND_TIMER,200);
               printf("Send: s:%i t:%i a:%i\r\n",(char) sendData[1],(char) sendData[2], (char) sendData[3]);
            }
         }
         if(ThisEvent.EventType==ES_ControlBypass)
         {
            printBgSaves();
         }
         break;
      } 
      
      
  }    
   return ReturnEvent;
}
예제 #7
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;
}
예제 #9
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;
}
예제 #11
0
/****************************************************************************
 Function
    RunLobbyistSM

 Parameters
   ES_Event : the event to process

 Returns
   ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise

 Description
   Deals with the high level state transitions of the Lobbyist state machine.
****************************************************************************/
ES_Event RunLobbyistSM( ES_Event ThisEvent )
{
  ES_Event ReturnEvent;
  ReturnEvent.EventType = ES_NO_EVENT; // assume no errors

  switch ( CurrentState )
  {
    case InitPState :       // If current state is initial Psedudo State
        if ( ThisEvent.EventType == ES_INIT ) // only respond to ES_Init
        {
            //Prep the timer hardware.
						printf("Init Steering \r\n");
						InitSteering();					// Initialize the steering systems
						printf("Init Packets \r\n");					
						InitPackets(); 					// Initialize packet array
						printf("Init XBee \r\n");
            InitXBee(); 						// Initialize the comms
						printf("Init Lift Fan \r\n");
						InitLiftFan();					// Initialize the lift fan
            //Initialize the DMC.
            CurrentState = WaitingForPair;
        }
    break;

    case WaitingForPair :
      switch ( ThisEvent.EventType )
      {
        //If the event is a pair request, change state to pairing and wait for the key.
        case MESSAGE_RECEIVED:
					// Read incoming message
					message = GetMessageData();
					// Check that message is a pair request and that it's meant for us
					if(*message == PAIR_REQUEST_HEADER && CheckLobbyistNumber() && GetApiIdentifier() == INCOMING_PACKET_API_IDENTIFIER){  // Ensure that this is not a Tx success receive
						printf("\r\n Current Message Size: %d ", GetMessageDataSize());
						// If we passed the checks, we're now paired. Start timers.
						printf("\n\r\nPair request received in unpaired state.");
						ES_Timer_InitTimer(UNPAIR_TIMER, UNPAIR_TIME); //Start the 45s timer
						ES_Timer_InitTimer(PAIR_FAIL_TIMER, ONE_SEC); //Start the 1s timer
						
						
						TurnOnLiftFan();     					//Start the lift fan
						//Update DMC
						
						// Recognize our new pac daddy
						pairedAddress = GetSenderAddress() ;
						// Send an ACK back to the sender
						TransmitStatusPacket(PAIRED); 
						// Set the pair time
						PairTimeLeft = 45;
						CurrentState = PairedAwaitEncryptionKey;
				}
        break;
        default :
            ; 
      }
    break;

    case PairedAwaitEncryptionKey:
      switch(ThisEvent.EventType){
        case MESSAGE_RECEIVED:
					// Check sender and message
					message = GetMessageData();
					uint16_t sender = GetSenderAddress() ;
					if(sender == pairedAddress && *message == KEY_SEND_HEADER){
						printf("\n\r\nKey received in pairing state.");
						//Save the key 
						SetKey((message+1)); // Make sure to +1 to start after header byte
						
						ES_Timer_InitTimer(PAIR_FAIL_TIMER, ONE_SEC); //Restart the 1s timer
						TransmitStatusPacket(PAIRED); //transmit status with the pairing bit set.
						CurrentState = PairedAwaitControl;
						printf("\r\n In paired await control");
				}
        break;
        case ES_TIMEOUT: // if there is a timeout on the 1s timer
					printf("\n\r\n1s timeout in pairing - moving to awaiting pair.");
					TurnOffLiftFan(); 										//Turn off the lift fan
					//Update DMC to avaiable for pairing
					ES_Timer_StopTimer(UNPAIR_TIMER); 		//Disable the 45s pairing timer
					ES_Timer_StopTimer(PAIR_FAIL_TIMER); 	//Disable the 1s transmit timeout timer
					TransmitStatusPacket(UNPAIRED); 			//Transmit status with the pairing bit cleared.
					CurrentState = WaitingForPair;
        break;
      }
    break;

    case PairedAwaitControl:
      switch(ThisEvent.EventType){
        case MESSAGE_RECEIVED:
					// See who sent the message
					uint16_t sender = GetSenderAddress() ;
					if(sender == pairedAddress){  // Only attempt decrypt if from sender
						message = DecryptMessage(GetMessageData(),GetMessageDataSize());
						RotateCipher();
	//					printf("\r\n Rotating Key");
						if(*message == CNTRL_REQUEST_HEADER){
							//printf("\n\r\nCommand received in the paired state.");
							ES_Timer_InitTimer(PAIR_FAIL_TIMER, ONE_SEC); //Restart the control command timer.
							ExecuteControl(); //Execute the control command.
							TransmitStatusPacket(PAIRED);// ACK YAY. Transmit the status with pairing bit set.
						} else {
							printf("\n\r Not a valid control packet received, Header: %d", *message);
						}
				}
        break;

        case ES_TIMEOUT: //Either the 1s or 45s timer
				case MANUAL_UNPAIR: // Or if the pair is manual
				case DECRYPT_FAIL:  // Or if the decryption failed
          if(ThisEvent.EventParam == UNPAIR_TIMER){
						// Keep track of how long is left on UNPAIR_TIMER
              PairTimeLeft--;
              printf("\n\r\nTime until unpair: %ds", PairTimeLeft);
						// If timer has run for 45 s
              if(PairTimeLeft == 0){    
                CurrentState = WaitingForPair;
								TurnOffLiftFan(); 										//Turn off the lift fan
								UpdateSteering(OFF,OFF);							//Turn off the propulsion fans
								//Update DMC to avaiable for pairing
								ES_Timer_StopTimer(UNPAIR_TIMER); 		//Disable the 45s pairing timer
								ES_Timer_StopTimer(PAIR_FAIL_TIMER); 	//Disable the 1s transmit timeout timer
                printf("\n\r\nUnpairing.");
						// If timer is not at 0, reinitialize the timer again for 1s to keep counting
              }else{
								 ES_Timer_InitTimer(UNPAIR_TIMER, UNPAIR_TIME); //Start the 45s timer
							}								
						} else {  
							// If we have an event NOT on the 45s timer, unpair
								printf("\n\r\nTimeout in the paired state. Moving to waiting for pair \r\n");
								TurnOffLiftFan(); 										//Turn off the lift fan
								UpdateSteering(OFF,OFF);							//Turn off the propulsion fans
								//Update DMC to avaiable for pairing
								ES_Timer_StopTimer(UNPAIR_TIMER); 		//Disable the 45s pairing timer
								ES_Timer_StopTimer(PAIR_FAIL_TIMER); 	//Disable the 1s transmit timeout timer
								if(ThisEvent.EventType == DECRYPT_FAIL){	// If this event came from a decryption failure 		//// WEHERE DOES THIS COME FROM?
									TransmitStatusPacket(DECRYPT_FAILURE);	// Transmit status with the decryption fail bit set
								} else {
									TransmitStatusPacket(UNPAIRED); 				//Transmit status with the pairing bit cleared.
								}
								CurrentState = WaitingForPair;
              }

        break;
      }

    break;

  default :
    break;
  }

  return ReturnEvent;
}
/**
 * @Function RunLine_FollowingSubHSM(ES_Event ThisEvent)
 * @param ThisEvent - the event (type and param) to be responded.
 * @return Event - return event (type and param), in general should be ES_NO_EVENT
 * @brief This function is where you implement the whole of the heirarchical state
 *        machine, as this is called any time a new event is passed to the event
 *        queue. This function will be called recursively to implement the correct
 *        order for a state transition to be: exit current state -> enter next state
 *        using the ES_EXIT and ES_ENTRY events.
 * @note Remember to rename to something appropriate.
 *       The lower level state machines are run first, to see if the event is dealt
 *       with there rather than at the current level. ES_EXIT and ES_ENTRY events are
 *       not consumed as these need to pass pack to the higher level state machine.
 * @author J. Edward Carryer, 2011.10.23 19:25
 * @author Gabriel H Elkaim, 2011.10.23 19:25 */
ES_Event Run_AA_LineFollowing_SubHSM(ES_Event ThisEvent) {
    uint8_t makeTransition = FALSE; // use to flag transition
    AA_LineFollowing_subHSM_t nextState; // <- change type to correct enum

    ES_Tattle(); // trace call stack

    switch (CurrentState) {
        case Init_AA_LineFollowing: // If current state is initial Psedudo State
            if (ThisEvent.EventType == ES_INIT)// only respond to ES_Init
            {
                nextState = EntryLineFollowing;
                makeTransition = TRUE;
                ThisEvent.EventType = ES_NO_EVENT;
            }
            break;

        case EntryLineFollowing:
            if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
                switch (ThisEvent.EventType) {
                    case ES_ENTRY:
                        StraightForward(speed);
                        break;

                    case ES_EXIT:
                        break;

                    case TAPE:
                        if (ThisEvent.EventParam == 0b100000) { // LL: 20, 100000
                            FullStop();
                            nextState = Adjusting_Left;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else if (ThisEvent.EventParam == 0b000010) { // RR: 02
                            FullStop();
                            nextState = Pivoting_Right;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else if ((ThisEvent.EventParam & 0b110010) == 0b110010) { // LL, L,  RR
                            FullStop();
                            nextState = Reversing_Left;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else if ((ThisEvent.EventParam & 0b001001) == 0b001001) { // F, B
                            FullStop();
                            nextState = Adjusting_Right;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else {
                            ThisEvent.EventType = ES_NO_EVENT;
                        }
                        break;
                }
            }
            break;

        case Adjusting_Left:
            if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
                switch (ThisEvent.EventType) {
                    case ES_ENTRY:
                        TurnLeft(speed * 1);
                        break;

                    case TAPE:
                        if ((ThisEvent.EventParam & 0b000010) == 0b000010) { // RR (corner)
                            nextState = Pivoting_Right;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else if ((ThisEvent.EventParam & 0b001000) == 0b001000) { //F
                            FullStop();
                            nextState = Adjusting_Right; //Adjusting_Right
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else {
                            ThisEvent.EventType = ES_NO_EVENT;
                        }
                        break;

                    case ES_EXIT:
                        break;
                }
            }
            break;

        case Adjusting_Right:
            if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
                switch (ThisEvent.EventType) {
                    case ES_ENTRY:
                        TurnRight(speed * 1);
                        break;

                    case TAPE:
                        if ((ThisEvent.EventParam & 0b010000) == 0b010000) { // L
                            FullStop();
                            nextState = Adjusting_Left;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else if ((ThisEvent.EventParam & 0b100000) == 0b100000) { // LL
                            FullStop();
                            nextState = Adjusting_Left;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else if ((ThisEvent.EventParam & 0b000010) == 0b000010) { // RR (corner)
                            nextState = Pivoting_Right;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else if ((ThisEvent.EventParam & 0b000110) == 0b000110) { //R, RR (corner)
                            nextState = Pivoting_Right;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else {
                            ThisEvent.EventType = ES_NO_EVENT;
                        }
                        break;

                    case ES_EXIT:
                        break;
                }
            }
            break;

        case Pivoting_Right:
            if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
                switch (ThisEvent.EventType) {
                    case ES_ENTRY:
                        ES_Timer_InitTimer(ammofindtimer, 2000);
                        PivotRight(speed);
                        break;

                    case ES_EXIT:
                        ES_Timer_StopTimer(ammofindtimer);
                        break;

                    case TAPE:

                        if ((ThisEvent.EventParam & 0b110010) == 0b110010) { // LL, L, RR
                            nextState = Reversing_Left;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else if ((ThisEvent.EventParam & 0b010000) == 0b010000) { // L
                            FullStop();
                            nextState = Adjusting_Left;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else if ((ThisEvent.EventParam & 0b100001) == 0b100001) { // LL, B
                            nextState = Adjusting_Left;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else if ((ThisEvent.EventParam & 0b100000) == 0b100000) { // LL
                            nextState = Adjusting_Left;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        }
                        else {
                            ThisEvent.EventType = ES_NO_EVENT;
                        }
                        break;

                    case ES_TIMEOUT:
                        if (ThisEvent.EventParam == ammofindtimer) {
                            nextState = EntryLineFollowing;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        }
                        break;
                }
            }
            break;

        case Reversing_Left:
            if (ThisEvent.EventType != ES_NO_EVENT) { // An event is still active
                switch (ThisEvent.EventType) {
                    case ES_ENTRY:
                        ES_Timer_InitTimer(turningtimer, turningtimervalue);
                        TurnLeft(-(speed));
                        break;

                    case ES_EXIT:
                        ES_Timer_StopTimer(turningtimer);
                        break;

                    case ES_TIMEOUT:
                        nextState = Adjusting_Left;
                        makeTransition = TRUE;
                        ThisEvent.EventType = ES_NO_EVENT;


                    case TAPE:
                        if ((ThisEvent.EventParam & 0b100000) == 0b100000) { // LL
                            nextState = Adjusting_Left;
                            makeTransition = TRUE;
                            ThisEvent.EventType = ES_NO_EVENT;
                        } else {
                            ThisEvent.EventType = ES_NO_EVENT;
                        }
                        break;
                }
            }
            break;

        default: // all unhandled states fall into here
            break;
    } // end switch on Current State


    if (makeTransition == TRUE) { // making a state transition, send EXIT and ENTRY
        // recursively call the current state with an exit event
        Run_AA_LineFollowing_SubHSM(EXIT_EVENT);
        CurrentState = nextState;
        Run_AA_LineFollowing_SubHSM(ENTRY_EVENT);
    }

    ES_Tail(); // trace call stack end
    return ThisEvent;
}