//#define CHECKOFF_SHOOTING /**************************************************************************** Module Round3DriveSM.c Revision 2.0.1 Description This is a template file for implementing state machines. Notes ****************************************************************************/ /*----------------------------- 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 "Round3DriveSM.h" #include "Pins.h" #include "ServoControl.h" #include "LauncherControl.h" #include "DriveMotors.h" #include "UltrasonicControl.h" #include "SimpleDriveToBSM.h" #include "MasterSM.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 DuringDriveToBTape( ES_Event Event); static ES_Event DuringTurnToGoal( ES_Event Event); static ES_Event DuringShoot( ES_Event Event); static ES_Event DuringTurnToWire(ES_Event Event); static ES_Event DuringBounceToBTape( ES_Event Event); static ES_Event DuringBounceToATape( ES_Event Event); static ES_Event DuringDrivingToB( ES_Event Event); /*---------------------------- Module Variables ---------------------------*/ // everybody needs a state variable, you may need others as well static Round3DriveState_t CurrentState; /*------------------------------ Module Code ------------------------------*/ /**************************************************************************** Function RunRound3DriveSM 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 J. Edward Carryer, 2/11/05, 10:45AM ****************************************************************************/ ES_Event RunRound3DriveSM( ES_Event CurrentEvent ) { unsigned char MakeTransition = false;/* are we making a state transition? */ Round3DriveState_t NextState = CurrentState; ES_Event ReturnEvent = CurrentEvent; // assume we are not consuming event //TODO: check score switch ( CurrentState ) { case ST_DriveToBTape : // 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 = DuringDriveToBTape(CurrentEvent); //process any events if ( ES_NO_EVENT != CurrentEvent.EventType ) //If an event is active { switch (CurrentEvent.EventType) { //TODO: //check score for both knights case ES_TIMEOUT : //if PID TIME OUT if (PID_TIMER == CurrentEvent.EventParam) { //PD Control Driving Forward Forward_PID(LEFT_FORWARD, RIGHT_FORWARD, Kp_FULL, Kd_FULL); //Init PID Timer ES_Timer_InitTimer(PID_TIMER, PID_INTERVAL); ReturnEvent.EventType = ES_NO_EVENT; } else if(ULTRASONIC_TIMER == CurrentEvent.EventParam){ // Query Distance to front wall int frontDistance = QueryFrontDistance(); //If within range, stop and transition state if(frontDistance <= FRONT_WALL_DISTANCE){ NextState = ST_TurnToGoal; ReturnEvent.EventType = ES_NO_EVENT; MakeTransition = true; } //else, init timer else{ ES_Timer_InitTimer(ULTRASONIC_TIMER, ULTRASONIC_INTERVAL); } } #ifdef CHECKOFF_SHOOTING else if(SHOOTING_TIMER == CurrentEvent.EventParam){ TurnOnLauncher(); } #endif break; } }// end if an event is active break; case ST_TurnToGoal: CurrentEvent = DuringTurnToGoal(CurrentEvent); if (ES_NO_EVENT != CurrentEvent.EventType) { switch (CurrentEvent.EventType) { case ES_TIMEOUT: if(TURN_TIMER == CurrentEvent.EventParam){ // Execute action function for state one : event one NextState = ST_Shoot;//Decide what the next state will be // for internal transitions, skip changing MakeTransition MakeTransition = true; //mark that we are taking a transition // if transitioning to a state with history change kind of entry //EntryEventKind.EventType = ES_ENTRY_HISTORY; // optionally, consume or re-map this event for the upper // level state machine ReturnEvent.EventType = ES_NO_EVENT; } break; } } break; case ST_Shoot: CurrentEvent = DuringShoot(CurrentEvent); if (ES_NO_EVENT != CurrentEvent.EventType) { switch (CurrentEvent.EventType) { //TODO: //check score for both knights case ES_TIMEOUT: if ( SHOOTING_TIMER == CurrentEvent.EventParam) { // Execute action function for state one : event one NextState = ST_TurnToWire;//Decide what the next state will be // for internal transitions, skip changing MakeTransition MakeTransition = true; //mark that we are taking a transition // if transitioning to a state with history change kind of entry //EntryEventKind.EventType = ES_ENTRY_HISTORY; // optionally, consume or re-map this event for the upper // level state machine ReturnEvent.EventType = ES_NO_EVENT; } break; } } break; case ST_TurnToWire: CurrentEvent = DuringTurnToWire(CurrentEvent); if (ES_NO_EVENT != CurrentEvent.EventType) { switch (CurrentEvent.EventType) { case ES_TIMEOUT: if(TURN_TIMER == CurrentEvent.EventParam){ // Execute action function for state one : event one NextState = ST_BounceToATape;//Decide what the next state will be // for internal transitions, skip changing MakeTransition MakeTransition = true; //mark that we are taking a transition // if transitioning to a state with history change kind of entry //EntryEventKind.EventType = ES_ENTRY_HISTORY; // optionally, consume or re-map this event for the upper // level state machine ReturnEvent.EventType = ES_NO_EVENT; } break; } } break; case ST_BounceToATape: CurrentEvent = DuringBounceToATape(CurrentEvent); if (ES_NO_EVENT != CurrentEvent.EventType) { switch (CurrentEvent.EventType) { case ES_TIMEOUT: if (PID_TIMER == CurrentEvent.EventParam) { //PD Control Driving Backward Backward_PID(LEFT_FORWARD, RIGHT_FORWARD, Kp_FULL, Kd_FULL); //Init PID Timer ES_Timer_InitTimer(PID_TIMER, PID_INTERVAL); ReturnEvent.EventType = ES_NO_EVENT; } else if(ULTRASONIC_TIMER == CurrentEvent.EventParam){ // Query Distance to back wall int backDistance = QueryBackDistance(); //If within range, stop and transition state if(backDistance <= BACK_WALL_DISTANCE){ NextState = ST_BounceToBTape; ReturnEvent.EventType = ES_NO_EVENT; MakeTransition = true; printf("Move to B \n\r"); } //else, init timer else{ ES_Timer_InitTimer(ULTRASONIC_TIMER, ULTRASONIC_INTERVAL); } } break; } } break; case ST_BounceToBTape: CurrentEvent = DuringBounceToBTape(CurrentEvent); if (ES_NO_EVENT != CurrentEvent.EventType) { switch (CurrentEvent.EventType) { case ES_TIMEOUT: if (PID_TIMER == CurrentEvent.EventParam) { //PD Control Driving Forward Forward_PID(LEFT_FORWARD, RIGHT_FORWARD, Kp_FULL, Kd_FULL); //Init PID Timer ES_Timer_InitTimer(PID_TIMER, PID_INTERVAL); ReturnEvent.EventType = ES_NO_EVENT; } else if(ULTRASONIC_TIMER == CurrentEvent.EventParam){ // Query Distance to back wall int frontDistance = QueryFrontDistance(); //If within range, stop and transition state if(frontDistance <= FRONT_WALL_DISTANCE){ NextState = ST_BounceToATape; ReturnEvent.EventType = ES_NO_EVENT; MakeTransition = true; printf("Move to A \n\r"); } //else, init timer else{ ES_Timer_InitTimer(ULTRASONIC_TIMER, ULTRASONIC_INTERVAL); } } break; } } break; break; case ST_DrivingToB: CurrentEvent = DuringDrivingToB(CurrentEvent); if (ES_NO_EVENT != CurrentEvent.EventType) { switch (CurrentEvent.EventType) { case ES_NO_EVENT: break; } } break; }//end switch different states // If we are making a state transition if (MakeTransition == true) { // Execute exit function for current state CurrentEvent.EventType = ES_EXIT; RunRound3DriveSM(CurrentEvent); CurrentState = NextState; //Modify state variable CurrentEvent.EventType = ES_ENTRY; // Execute entry function for new state // this defaults to ES_ENTRY RunRound3DriveSM(CurrentEvent); } return(ReturnEvent); } /**************************************************************************** Function StartRound3DriveSM Parameters None Returns None Description Does any required initialization for this state machine Notes Author J. Edward Carryer, 2/18/99, 10:38AM ****************************************************************************/ void StartRound3DriveSM ( 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_DriveToBTape; } // call the entry function (if any) for the ENTRY_STATE RunRound3DriveSM(CurrentEvent); } /**************************************************************************** Function QueryRound3DriveSM 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 ****************************************************************************/ Round3DriveState_t QueryRound3DriveSM ( void ) { return(CurrentState); } /*************************************************************************** private functions ***************************************************************************/ static ES_Event DuringDriveToBTape( 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) ) { //Post a Joust Event ES_Event JoustEvent = {EV_START_JOUSTING, 0}; PostMasterSM(JoustEvent); //drive forward DriveForward(); //start PID timer ES_Timer_InitTimer(PID_TIMER, PID_INTERVAL); ES_Timer_InitTimer(ULTRASONIC_TIMER, ULTRASONIC_INTERVAL); #ifdef CHECKOFF_SHOOTING ES_Timer_InitTimer(SHOOTING_TIMER, 3000); #endif } else if ( Event.EventType == ES_EXIT ) { StopMotor(); }else // do the 'during' function for this state { //no lower level state machine } // 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); } static ES_Event DuringTurnToGoal( 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 turning TurnCW(); // Turn on launcher StartLauncherMotor(); // Start Open Loop Turning Timer ES_Timer_InitTimer(TURN_TIMER, TURN_INTERVAL); } else if ( Event.EventType == ES_EXIT ) { StopMotor(); }else // do the 'during' function for this state { //no lower level state machine } // 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); } static ES_Event DuringShoot( 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) ) { //open gate and turn on launcher motor TurnOnLauncher(); ES_Timer_InitTimer(SHOOTING_TIMER, SHOOTING_INTERVAL); } else if ( Event.EventType == ES_EXIT ) { //close gate and turn off launcher motor TurnOffLauncher(); }else // do the 'during' function for this state { //no lower level state machine } // 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); } static ES_Event DuringTurnToWire( 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 turning Clockwise TurnCCW(); // Start Open Loop Turning Timer ES_Timer_InitTimer(TURN_TIMER, TURN_INTERVAL); } else if ( Event.EventType == ES_EXIT ) { StopMotor(); }else // do the 'during' function for this state { //no lower level state machine } // 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); } static ES_Event DuringBounceToATape( 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) ) { //Post a Joust Event ES_Event JoustEvent = {EV_START_JOUSTING, 0}; PostMasterSM(JoustEvent); //drive forward DriveBackward(); //start PID timer ES_Timer_InitTimer(PID_TIMER, PID_INTERVAL); ES_Timer_InitTimer(ULTRASONIC_TIMER, ULTRASONIC_INTERVAL); } else if ( Event.EventType == ES_EXIT ) { // Stop Moving StopMotor(); }else // do the 'during' function for this state { //no lower level state machine } // 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); } static ES_Event DuringBounceToBTape( 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) ) { //Post a Joust Event ES_Event JoustEvent = {EV_START_JOUSTING, 0}; PostMasterSM(JoustEvent); //drive forward DriveForward(); //start PID timer ES_Timer_InitTimer(PID_TIMER, PID_INTERVAL); ES_Timer_InitTimer(ULTRASONIC_TIMER, ULTRASONIC_INTERVAL); } else if ( Event.EventType == ES_EXIT ) { // Stop Moving StopMotor(); }else // do the 'during' function for this state { //no lower level state machine } // 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); } static ES_Event DuringDrivingToB( 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 simple drive to B StartSimpleDriveToBSM(Event); } else if ( Event.EventType == ES_EXIT ) { //stop motor, end of match StopMotor(); }else // do the 'during' function for this state { //no lower level state machine } // 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); }