/**************************************************************************** Module SimpleDriveToBSM.c Revision 2.0.1 Description This is a template file for implementing state machines. Notes 02/14/99 10:34 jec Began Coding ****************************************************************************/ /*----------------------------- Include Files -----------------------------*/ // Basic includes for a program using the Events and Services Framework #include "ES_Configure.h" #include "ES_Framework.h" /* include header files for this state machine as well as any machines at the next lower level in the hierarchy that are sub-machines to this machine */ #include "SimpleDriveToBSM.h" #include "DriveMotors.h" #include "UltrasonicControl.h" #include "Pins.h" #include "MasterSM.h" #include "GameSM.h" /*----------------------------- Module Defines ----------------------------*/ // define constants for the states for this machine // and any other local defines /*---------------------------- Module Functions ---------------------------*/ /* prototypes for private functions for this machine, things like during functions, entry & exit functions.They should be functions relevant to the behavior of this state machine */ static ES_Event DuringDriveToB( ES_Event Event); static ES_Event DuringStop( ES_Event Event); /*---------------------------- Module Variables ---------------------------*/ // everybody needs a state variable, you may need others as well static SimpleDriveToBState_t CurrentState; /*------------------------------ Module Code ------------------------------*/ /**************************************************************************** Function RunSimpleDriveToBSM Parameters ES_Event: the event to process Returns ES_Event: an event to return Description add your description here Notes uses nested switch/case to implement the machine. Author Chih-Wei Tang, 2/11/05, 10:45AM ****************************************************************************/ ES_Event RunSimpleDriveToBSM( ES_Event CurrentEvent ) { unsigned char MakeTransition = false;/* are we making a state transition? */ SimpleDriveToBState_t NextState = CurrentState; ES_Event EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state ES_Event ReturnEvent = CurrentEvent; // assume we are not consuming event static int leftDriveSpeed = LEFT_FORWARD; static int rightDriveSpeed = RIGHT_FORWARD; static float currentKp = Kp_FULL; static float currentKd = Kd_FULL; switch ( CurrentState ) { case ST_DriveToB : // If current state is state one // Execute During function for state one. ES_ENTRY & ES_EXIT are // processed here allow the lowere level state machines to re-map // or consume the event CurrentEvent = DuringDriveToB(CurrentEvent); //process any events if ( ES_NO_EVENT != CurrentEvent.EventType ) //If an event is active { switch (CurrentEvent.EventType) { case EV_FRONT_WALL_REACHED: //If we reach the wall // Execute action function for state one : event one NextState = ST_Stop_B;//Go to stop state // for internal transitions, skip changing MakeTransition MakeTransition = true; //mark that we are taking a transition //consume event here ReturnEvent.EventType = ES_NO_EVENT; break; case ES_TIMEOUT: // if PID timer time out if (PID_TIMER == CurrentEvent.EventParam) { //PD Control Driving Forward Forward_PID(leftDriveSpeed, rightDriveSpeed, currentKp, currentKd); //Init PID Timer ES_Timer_InitTimer(PID_TIMER, PID_INTERVAL); ReturnEvent.EventType = ES_NO_EVENT; } else if(ULTRASONIC_TIMER == CurrentEvent.EventParam){ //Check if in range of wall int frontDistance = QueryFrontDistance(); int backDistance = QueryBackDistance(); GameState_t gameState = QueryGameSM(); bool matchUnderway = (gameState == Round1) || (gameState == Round2) || (gameState == Round3) || (gameState == SuddenDeath); //If in range, slow down if(FRONT_WALL_DISTANCE >= frontDistance){ leftDriveSpeed = LEFT_REDUCED_SPEED; rightDriveSpeed = RIGHT_REDUCED_SPEED; currentKp = Kp_REDUCED; currentKd = Kd_REDUCED; printf("Slow Down!!!!! \n\r"); } // else, start timer else{ //Start Ultrasonic Timer ES_Timer_InitTimer(ULTRASONIC_TIMER, ULTRASONIC_INTERVAL); } // If passed the first red tape - start jousting if((BACK_WALL_DISTANCE <= backDistance)&& matchUnderway){ ES_Event JoustEvent = {EV_START_JOUSTING, 0}; PostMasterSM(JoustEvent); } } break; }// end switch EventType }// end if there is Event break; case ST_Stop_B : // If current state is state one // Execute During function for state one. ES_ENTRY & ES_EXIT are // processed here allow the lowere level state machines to re-map // or consume the event CurrentEvent = DuringStop(CurrentEvent); //process any events if ( ES_NO_EVENT != CurrentEvent.EventType ) //If an event is active { //TODO: // print message? }//end if there is an event break; }//end switch CurrentState // If we are making a state transition if (MakeTransition == true) { // Execute exit function for current state CurrentEvent.EventType = ES_EXIT; RunSimpleDriveToBSM(CurrentEvent); CurrentState = NextState; //Modify state variable // set the event type to ES_ENTRY to run the entry function CurrentEvent.EventType = ES_ENTRY; RunSimpleDriveToBSM(CurrentEvent); } return(ReturnEvent); } /**************************************************************************** Function StartSimpleDriveToBSM Parameters None Returns None Description Does any required initialization for this state machine Notes Author Chih-Wei Tang, 2/18/99, 10:38AM ****************************************************************************/ void StartSimpleDriveToBSM ( ES_Event CurrentEvent ) { // local variable to get debugger to display the value of CurrentEvent ES_Event LocalEvent = CurrentEvent; // to implement entry to a history state or directly to a substate // you can modify the initialization of the CurrentState variable // otherwise just start in the entry state every time the state machine // is started if ( ES_ENTRY_HISTORY != CurrentEvent.EventType ) { CurrentState = ST_DriveToB; } // call the entry function (if any) for the ENTRY_STATE RunSimpleDriveToBSM(CurrentEvent); } /**************************************************************************** Function QuerySimpleDriveToBSM Parameters None Returns TemplateState_t The current state of the Template state machine Description returns the current state of the Template state machine Notes Author J. Edward Carryer, 2/11/05, 10:38AM ****************************************************************************/ SimpleDriveToBState_t QuerySimpleDriveToBSM ( void ) { return(CurrentState); } /*************************************************************************** private functions ***************************************************************************/ static ES_Event DuringDriveToB( ES_Event Event) { ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events if ( (Event.EventType == ES_ENTRY) || (Event.EventType == ES_ENTRY_HISTORY) ) { //start PID timer ES_Timer_SetTimer(PID_TIMER, PID_INTERVAL); ES_Timer_StartTimer(PID_TIMER); //Start driving forward DriveForward(); //Start Ultrasonic Timer ES_Timer_InitTimer(ULTRASONIC_TIMER, ULTRASONIC_INTERVAL); printf("Drive Forward \n\r"); } else if ( Event.EventType == ES_EXIT ) { //stop StopMotor(); printf("STOP!!! \n\r"); }else // do the 'during' function for this state { // no during function for 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(Event); } static ES_Event DuringStop( ES_Event Event) { ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events if ( (Event.EventType == ES_ENTRY) || (Event.EventType == ES_ENTRY_HISTORY) ) { //No Entry Function } else if ( Event.EventType == ES_EXIT ) { //No lower level during function }else // do the 'during' function for this state { // no during function for this state } return(Event); }