/**************************************************************************** Module SMToDispenser.c Description state machine for traveling to a dispenser, and orienting once there. Uses beacon aligning and tracking functions to get to dispenser. Once the bot hits a wall, it backs up slightly, turns towards the nearest hoop, and drives until it finds the 3pt line. If it can't, it tries again from the aligning phase. ****************************************************************************/ /*----------------------------- Include Files -----------------------------*/ #include "GlobalHeader.h" #include "SMGameplay.h" #include "SMToDispenser.h" #include "SMMaster.h" #include "SMDrivetrain.h" /*----------------------------- Module Defines ----------------------------*/ /*---------------------------- Module Functions ---------------------------*/ static Event_t DuringAligning( Event_t Event); static Event_t DuringTracking( Event_t Event); static Event_t DuringReversing( Event_t Event); static Event_t DuringBlindTurn( Event_t Event); static Event_t DuringOrienting( Event_t Event); static Event_t DuringLineFinding( Event_t Event); /*---------------------------- Module Variables ---------------------------*/ static ToState_t CurrentState; static unsigned char TrackingCrossings = 0; //how many tape lines we've seen static unsigned char CrossingsThreshold; //how many we are waiting to see static unsigned char AtDispenserFlag = 0; //whether we've arrived at dispenser /*------------------------------ Module Code ------------------------------*/ /**************************************************************************** Function QueryAtDispenserFlag Parameters none Returns 1 if at dispenser, 0 otherwise Description checks to see if we have arrived at a dispenser. ****************************************************************************/ unsigned char QueryAtDispenserFlag (void) { unsigned char ReturnVal = AtDispenserFlag; AtDispenserFlag = 0; return ReturnVal; } /**************************************************************************** Function RunToDispenserSM Parameters Event_t Returns Event_t Description Runs ToDispenser state machine. transitions between aligning and tracking a beacon, to a sequence of fixed actions once arrived. ****************************************************************************/ Event_t RunToDispenserSM( Event_t CurrentEvent ) { unsigned char MakeTransition = FALSE;/* are we making a state transition? */ PlayState_t NextState = CurrentState; switch ( CurrentState ) { case To_Aligning : CurrentEvent = DuringAligning(CurrentEvent); //process any events if ( CurrentEvent != EV_NO_EVENT ) //If an event is active { switch (CurrentEvent) { case EV_Drive_BeaconFound: //if we see the beacon ClearFrontTapeCache(); //start looking for tape crossings NextState = To_Tracking; //track beacon MakeTransition = TRUE; break; } } break; // repeat state pattern as required for other states case To_Tracking : CurrentEvent = DuringTracking(CurrentEvent); //process any events if ( CurrentEvent != EV_NO_EVENT ) //If an event is active { switch (CurrentEvent) { case EV_Sensor_TapeFront: //if we cross tape ClearFrontTapeCache(); TrackingCrossings += 1; //add one to tape crossings if (TrackingCrossings >= CrossingsThreshold) { //if we have crossed enough tape ReduceTrackingPower(3); //slow down ReduceTrackingAngle(4); //aim turret straighter } break; case EV_Sensor_Bump: //if we hit wall, or something NextState = To_Reversing; //back up MakeTransition = TRUE; break; } } break; case To_Reversing : CurrentEvent = DuringReversing(CurrentEvent); //process any events if ( CurrentEvent != EV_NO_EVENT ) //If an event is active { switch (CurrentEvent) { case EV_Drive_TimerExpired: //if done backing up NextState = To_BlindTurn; //turn so that we aren't looking at beacon from short range MakeTransition = TRUE; break; } } break; case To_BlindTurn : CurrentEvent = DuringBlindTurn(CurrentEvent); //process any events if ( CurrentEvent != EV_NO_EVENT ) //If an event is active { switch (CurrentEvent) { case EV_Drive_TimerExpired: //if done turning NextState = To_Orienting; //aim towards the hoop MakeTransition = TRUE; break; } } break; case To_Orienting : CurrentEvent = DuringOrienting(CurrentEvent); //process any events if ( CurrentEvent != EV_NO_EVENT ) //If an event is active { switch (CurrentEvent) { case EV_Drive_BeaconFound: //if we found the hoop NextState = To_LineFinding; //find 3pt line MakeTransition = TRUE; break; } } break; case To_LineFinding : CurrentEvent = DuringLineFinding(CurrentEvent); //process any events if ( CurrentEvent != EV_NO_EVENT ) //If an event is active { switch (CurrentEvent) { case EV_Sensor_TapeBack: //if we foiund the 3 pt line Drive_Stop(); //stop moving AtDispenserFlag = 1; //indicate we are at dispenser break; case EV_Drive_TimerExpired: //if we can't find tape NextState = To_Aligning; //try again CrossingsThreshold = 1; //slow down at first tape, though MakeTransition = TRUE; SetLastDispenserNumber(QueryCurrentDispenserNumber()); //don't angle turret, though } } break; } // If we are making a state transition if (MakeTransition == TRUE) { // Execute exit function for current state (void)RunToDispenserSM(EV_EXIT); CurrentState = NextState; //Modify state variable printf("next todispenser state %d\r\n",CurrentState); // Execute entry function for new state (void)RunToDispenserSM(EV_ENTRY); } return(CurrentEvent); } /**************************************************************************** Function StartToDispenserSM Parameters Event_t Returns none Description Starts ToDispenserSM. ****************************************************************************/ void StartToDispenserSM ( Event_t CurrentEvent ) { CurrentState = To_Aligning; CrossingsThreshold = 2; //start by slowing down after 2nd tape crossing // call the entry function (if any) for the ENTRY_STATE (void)RunToDispenserSM(EV_ENTRY); } /**************************************************************************** Function QueryToDispenserSM Parameters none Returns ToState_t current state machine state Description returns state of state machine ****************************************************************************/ ToState_t QueryToDispenserSM ( void ) { return(CurrentState); } /*************************************************************************** private functions ***************************************************************************/ /**************************************************************************** Function DuringAligning Parameters Event_t Returns Event_t Description On entry, Align to target dispenser beacon, with turret angle specified by QueryTrackingAngle() ****************************************************************************/ static Event_t DuringAligning( Event_t Event) { // process EV_ENTRY & EV_EXIT events if ( Event == EV_ENTRY) { Drive_AlignToBeacon(QueryCurrentDispenserBeacon(), QueryTrackingAngle(QueryLastDispenserNumber(), QueryCurrentDispenserNumber()), 50); TrackingCrossings = 0; }else if ( Event == EV_EXIT) { }else // do the 'during' function for this state { } return(Event); } /**************************************************************************** Function DuringTracking Parameters Event_t Returns Event_t Description On entry, track target dispenser beacon, with turret angle specified by QueryTrackingAngle() ****************************************************************************/ static Event_t DuringTracking( Event_t Event) { // process EV_ENTRY & EV_EXIT events if ( Event == EV_ENTRY) { Drive_TrackBeacon(QueryCurrentDispenserBeacon(), QueryTrackingAngle(QueryLastDispenserNumber(), QueryCurrentDispenserNumber()), 85); }else if ( Event == EV_EXIT) { }else // do the 'during' function for this state { } return(Event); } /**************************************************************************** Function DuringReversing Parameters Event_t Returns Event_t Description On entry, initiate brief drive in reverse ****************************************************************************/ static Event_t DuringReversing( Event_t Event) { // process EV_ENTRY & EV_EXIT events if ( Event == EV_ENTRY) { Drive_Timed(0, -50, 100); //back up slightly }else if ( Event == EV_EXIT) { }else // do the 'during' function for this state { } return(Event); } /**************************************************************************** Function DuringBlindTurn Parameters Event_t Returns Event_t Description On Entry, initiate brief timed turn to turn away from a close beacon. On exit, clear memory of all beacon directions so we don't turn back towards dispenser ****************************************************************************/ static Event_t DuringBlindTurn( Event_t Event) { // process EV_ENTRY & EV_EXIT events if ( Event == EV_ENTRY) { Drive_Timed(100, 50, 500); }else if ( Event == EV_EXIT) { ClearBeaconDirection(); //clear memory so we don't catch edges of close beacon }else // do the 'during' function for this state { } return(Event); } /**************************************************************************** Function DuringOrienting Parameters Event_t Returns Event_t Description On entry, align to nearest hoop ****************************************************************************/ static Event_t DuringOrienting( Event_t Event) { // process EV_ENTRY & EV_EXIT events if ( Event == EV_ENTRY) { Drive_AlignToBeacon(QueryNearestHoop(QueryCurrentDispenserNumber()),0,50); }else if ( Event == EV_EXIT) { }else // do the 'during' function for this state { } return(Event); } /**************************************************************************** Function During Parameters Event_t Returns Event_t Description On entry, clear tape cache and start driving forward. ****************************************************************************/ static Event_t DuringLineFinding( Event_t Event) { // process EV_ENTRY & EV_EXIT events if ( Event == EV_ENTRY) { ClearBackTapeCache(); Drive_Timed(0, 25,1000); }else if ( Event == EV_EXIT) { }else // do the 'during' function for this state { } return(Event); }