/**************************************************************************** Module SMGameplay.c Description Module for top-level gameplay and decision-making of robot. Controls whether robot is to stay at dispenser, or drive to a new dispenser. ****************************************************************************/ /*----------------------------- Include Files -----------------------------*/ #include "GlobalHeader.h" #include "SMMaster.h" #include "SMGameplay.h" /*----------------------------- Module Defines ----------------------------*/ //timeouts for machine states #define TODISPENSER_TIMEOUT 15000 #define ATDISPENSER_TIMEOUT 30000 #define DETERMINING_TIMEOUT 6000 /*---------------------------- Module Functions ---------------------------*/ static Event_t DuringDetermining( Event_t Event); static Event_t DuringToDispenser( Event_t Event); static Event_t DuringAtDispenser( Event_t Event); static void SetCurrentDispenser(void); /*---------------------------- Module Variables ---------------------------*/ //state and machine variables static PlayState_t CurrentState; static unsigned int StateStartTime; static unsigned int StateTimeoutTime; //team and gametype-dependent variables static unsigned char HalfCourt = 1; static TeamColor_t MyColor; static Beacon_t MyHoop; static unsigned char DispenserOwned[4] = {0,0,0,0}; //whether we own each dispenser static unsigned char DispenserNearHoop[4] = {0,0,0,0}; //whether we should shoot from each //current dispenser-dependent variables static unsigned char CurrentDeterminingDispenser = 0; //when figuring out our dispensers for full court static unsigned char CurrentDispenserNumber; //current target static Beacon_t CurrentDispenserBeacon; static unsigned char LastDispenserNumber; //previous target, for determining travel path static unsigned char NearHoopFlag; //whether to shoot from current dispenser //calibration variables and mappings static signed char ShotAngle[4] = {-12,13,15,-13}; //how to aim from each dispenser static signed char TrackingAngle[4][4] = {{0,0,-60,20},{0,0,-20,60},{60,-20,0,0},{20,-60,0,0}}; //how to travel between dispensers static Beacon_t NearestHoop[4] = {Beacon_RedHoop, Beacon_RedHoop, Beacon_GreenHoop, Beacon_GreenHoop}; //nearest hoop for alignment static Beacon_t DISPENSER_MAP[4] = {Beacon_Dispenser0, Beacon_Dispenser1, Beacon_Dispenser2, Beacon_Dispenser3}; /*------------------------------ Module Code ------------------------------*/ /**************************************************************************** Function SetColor Parameters none Returns none Description Reads color switch and sets MyColor, MyHoop and DispenserNearHoop accordingly. ****************************************************************************/ void SetColor(void) { MyColor = ReadColor(); switch (MyColor) { case TeamRed: MyHoop = Beacon_RedHoop; DispenserNearHoop[0] = 1; DispenserNearHoop[1] = 1; DispenserNearHoop[2] = 0; DispenserNearHoop[3] = 0; break; case TeamGreen: MyHoop = Beacon_GreenHoop; DispenserNearHoop[0] = 0; DispenserNearHoop[1] = 0; DispenserNearHoop[2] = 1; DispenserNearHoop[3] = 1; break; } } /**************************************************************************** Function SetHalfCourt Parameters none Returns none Description reads half court switch and sets HalfCourt indicator accordingly. ****************************************************************************/ void SetHalfCourt(void) { HalfCourt = ReadHalfCourt(); } /**************************************************************************** Function QueryPlayTimeout Parameters none Returns 1 if state timer has expired, 0 otherwsie ****************************************************************************/ unsigned char QueryPlayTimeout (void) { if ((TMRS12_GetTime() - StateStartTime > StateTimeoutTime)) { return 1; } return 0; } /**************************************************************************** Function QueryToDispenserFlag Parameters none Returns 1 if the robot should make a transition from AtDispenser to ToDispenser, 0 otherwise. Description If state is AtDispenser, and the dispenser is empty, then when we aren't shooting then the function returns 1 indicating we should move. ****************************************************************************/ unsigned char QueryToDispenserFlag(void) { return ((CurrentState == Play_AtDispenser) && (QueryRequestBallSM()==Request_Empty) && ((QueryShootSM() == Shoot_Waiting) || (NearHoopFlag == 0))); } /**************************************************************************** Function SetCurrentDispenser Parameters none Returns none Description Refreshes CurrentDispenser to the next one that we own. ****************************************************************************/ static void SetCurrentDispenser(void) { unsigned int i; unsigned char TestDispenserNumber; LastDispenserNumber = CurrentDispenserNumber; for (i=1; i<=4; i++) //test each dispenser to see if we own it { TestDispenserNumber = ((CurrentDispenserNumber + i)%4); if (DispenserOwned[TestDispenserNumber] == 1) { CurrentDispenserNumber = TestDispenserNumber; CurrentDispenserBeacon = DISPENSER_MAP[CurrentDispenserNumber]; NearHoopFlag = DispenserNearHoop[CurrentDispenserNumber]; printf("Next Dispenser = %d Last Dispenser = %d\r\n", CurrentDispenserNumber, LastDispenserNumber); i = 5; //just in case break; } } } /**************************************************************************** Function QueryCurrentDispenserNumber Parameters none Returns Number of current target dispenser ****************************************************************************/ unsigned char QueryCurrentDispenserNumber(void) { return CurrentDispenserNumber; } /**************************************************************************** Function QueryCurrentDispenserBeacon Parameters none Returns Beacon_t beacon of current dispenser ****************************************************************************/ Beacon_t QueryCurrentDispenserBeacon(void) { return CurrentDispenserBeacon; } /**************************************************************************** Function QueryLastDispenserNumber Parameters none Returns Number of previous target dispenser ****************************************************************************/ unsigned char QueryLastDispenserNumber(void) { return LastDispenserNumber; } /**************************************************************************** Function SetLastDispenserNumber Parameters unsigned char Number Returns none Description Allows the overriding of the last dispenser ****************************************************************************/ void SetLastDispenserNumber(unsigned char Number) { LastDispenserNumber = Number; } /**************************************************************************** Function QueryMyHoop Parameters none Returns Beacon_t beacon of my hoop ****************************************************************************/ Beacon_t QueryMyHoop (void) { return MyHoop; } /**************************************************************************** Function QueryNearestHoop Parameters unsigned char Dispenser Returns Beacon_t beacon of hoop nearest to dispenser, for alignment ****************************************************************************/ Beacon_t QueryNearestHoop(unsigned char Dispenser) { return NearestHoop[Dispenser]; } /**************************************************************************** Function QueryDispenserNearHoop Parameters none Returns 1 if current dispenser is near hoop, 0 otherwsie ****************************************************************************/ unsigned char QueryDispenserNearHoop(void) { return DispenserNearHoop[CurrentDispenserNumber]; } /**************************************************************************** Function QueryTrackingAngle Parameters unsigned chars LastDispenser, NextDispenser Returns signed char Angle - angle to set turret to to track between dispensers ****************************************************************************/ signed char QueryTrackingAngle(unsigned char LastDispenser, unsigned char NextDispenser) { return TrackingAngle[LastDispenser][NextDispenser]; } /**************************************************************************** Function QueryShotAngle Parameters unsigned char Dispenser Returns signed char Angle - angle to set turret to to shoot from Dispenser ****************************************************************************/ signed char QueryShotAngle(unsigned char Dispenser) { return ShotAngle[Dispenser]; } /**************************************************************************** Function RunPlaySM Parameters Event_t Returns Event_t Description Run Play state machine ****************************************************************************/ Event_t RunPlaySM( Event_t CurrentEvent ) { unsigned char MakeTransition = FALSE;/* are we making a state transition? */ PlayState_t NextState = CurrentState; switch ( CurrentState ) { case Play_DeterminingField : CurrentEvent = DuringDetermining(CurrentEvent); //process any events if ( CurrentEvent != EV_NO_EVENT ) //If an event is active { unsigned char Command; unsigned char Response; switch (CurrentEvent) { case EV_OC_NewResponse : //if there has been a response Command = QueryOCLastCommand(); Response = QueryOCResponse(); if ((Command & ~OC_CommandDispenserMask) == OC_QueryCommandBase) //if the command was a dispenser query { if ((Response & ~(OC_ResponseDispenserMask | OC_BallMask)) == OC_QueryResponseBase) //if the response was a number of balls { unsigned char Dispenser = ((Response & OC_ResponseDispenserMask) >> 2); unsigned char BallsAvailable = (Response & OC_BallMask); unsigned char DispenserSum; //printf("Dispenser %d Balls %d\r\n",Dispenser, BallsAvailable); if (BallsAvailable > 0) //if balls were available, we own this dispenser { DispenserOwned[Dispenser] = 1; //printf("I own dispenser %d",Dispenser); } else { DispenserOwned[Dispenser] = 0; //otherwise, we don't own it } DispenserSum = DispenserOwned[0] + DispenserOwned[1] + DispenserOwned[2] + DispenserOwned[3]; //printf("Dispenser Sum %d\r\n",DispenserSum); if (DispenserSum == 2) //make sure we have two dispensers assigned { NextState = Play_ToDispenser; MakeTransition = TRUE; } else //otherwise, keep checking next dispenser { CurrentDeterminingDispenser = ((Dispenser + 1)%4); OC_QueryDispenser(CurrentDeterminingDispenser, 1); //printf("resetting state start time, disp %d\r\n",CurrentDeterminingDispenser); StateStartTime = TMRS12_GetTime(); } } else //if the response was not a number of balls, retry the request { if (((Response & OC_ResponseDispenserMask) >> 2) == CurrentDeterminingDispenser) OC_AddCommand(QueryOCLastCommand(),1); } } break; case EV_OC_CounterExpired : //if the command timed out Command = QueryOCLastCommand(); if ((Command & ~OC_CommandDispenserMask) == OC_QueryCommandBase) { if ((Command & OC_CommandDispenserMask) == CurrentDeterminingDispenser) OC_AddCommand(Command,1); //retry command } break; case EV_Play_Timeout: //if this state has timed out CurrentDeterminingDispenser = ((CurrentDeterminingDispenser + 1)%4); //move on to next dispenser no matter what. OC_QueryDispenser(CurrentDeterminingDispenser, 1); //printf("resetting state start time, disp %d\r\n",CurrentDeterminingDispenser); StateStartTime = TMRS12_GetTime(); break; } } break; case Play_ToDispenser: CurrentEvent = DuringToDispenser(CurrentEvent); //process any events if ( CurrentEvent != EV_NO_EVENT ) //If an event is active { switch (CurrentEvent) { case EV_Play_AtDispenser : //if we have arrived at a dispenser, switch states NextState = Play_AtDispenser; MakeTransition = TRUE; break; case EV_Play_Timeout : //if we have timed out, try traveling to a new dispenser SetCurrentDispenser(); NextState = Play_ToDispenser; MakeTransition = TRUE; break; } } break; case Play_AtDispenser: CurrentEvent = DuringAtDispenser(CurrentEvent); //process any events if ( CurrentEvent != EV_NO_EVENT ) //If an event is active { switch (CurrentEvent) { case EV_Play_Timeout: case EV_Play_ToDispenser : //if we are done with the dispenser or we timeout SetCurrentDispenser(); NextState = Play_ToDispenser; //travel to a new dispenser MakeTransition = TRUE; break; } } break; } // If we are making a state transition if (MakeTransition == TRUE) { // Execute exit function for current state (void)RunPlaySM(EV_EXIT); CurrentState = NextState; //Modify state variable printf("Next Play State %d\r\n",NextState); // Execute entry function for new state (void)RunPlaySM(EV_ENTRY); } return(CurrentEvent); } /**************************************************************************** Function StartPlaySM Parameters Event Returns none Description Start Play state machine ****************************************************************************/ void StartPlaySM ( Event_t CurrentEvent ) { SetColor(); //set color, now that game is on. switch (HalfCourt) { case 0: //full court printf("full court\r\n"); CurrentState = Play_DeterminingField; //determine which dispensers are ours switch (MyColor) { case TeamRed: MyHoop = Beacon_RedHoop; CurrentDispenserNumber = 1; break; case TeamGreen: MyHoop = Beacon_GreenHoop; CurrentDispenserNumber = 2; break; } break; case 1: //half court printf("half court\r\n"); CurrentState = Play_ToDispenser; //travel to dispenser immediately switch (MyColor) //set my dispensers based on color { case TeamRed: MyHoop = Beacon_RedHoop; DispenserOwned[0] = 1; DispenserOwned[1] = 1; DispenserOwned[2] = 0; DispenserOwned[3] = 0; CurrentDispenserNumber = 1; NearHoopFlag = 1; CurrentDeterminingDispenser = 0; break; case TeamGreen: MyHoop = Beacon_GreenHoop; DispenserOwned[0] = 0; DispenserOwned[1] = 0; DispenserOwned[2] = 1; DispenserOwned[3] = 1; CurrentDispenserNumber = 2; NearHoopFlag = 1; CurrentDeterminingDispenser = 2; break; } SetCurrentDispenser(); //choose dispenser to travel to first break; } printf("Starting Play\r\n"); // call the entry function (if any) for the ENTRY_STATE (void)RunPlaySM(EV_ENTRY); } /**************************************************************************** Function QueryPlaySM Parameters none Returns PlayState_t current machine state Description returns current machine state ****************************************************************************/ PlayState_t QueryPlaySM ( void ) { return(CurrentState); } /*************************************************************************** private functions ***************************************************************************/ /**************************************************************************** Function DuringDetermining Parameters Event Returns Event Description On entry, reset dispensers owned, start querying first dispenser. On exit, set first dispenser to travel to. ****************************************************************************/ static Event_t DuringDetermining( Event_t Event) { // process EV_ENTRY & EV_EXIT events if ( Event == EV_ENTRY) { SetColor(); DispenserOwned[0] = 0; DispenserOwned[1] = 0; DispenserOwned[2] = 0; DispenserOwned[3] = 0; StateStartTime = TMRS12_GetTime(); StateTimeoutTime = DETERMINING_TIMEOUT; OC_QueryDispenser(CurrentDeterminingDispenser,1); // printf("Entering Determining\r\n"); }else if ( Event == EV_EXIT) { SetCurrentDispenser(); }else { } return(Event); } /**************************************************************************** Function DuringToDispenser Parameters Event_t Returns Event_t Description On entry, start ToDispenser state machine. run ToDispenser state machine. ****************************************************************************/ static Event_t DuringToDispenser( Event_t Event) { // process EV_ENTRY & EV_EXIT events if ( Event == EV_ENTRY) { StartToDispenserSM(Event); StateStartTime = TMRS12_GetTime(); StateTimeoutTime = TODISPENSER_TIMEOUT; }else if ( Event == EV_EXIT) { (void)RunToDispenserSM(Event); }else // do the 'during' function for this state { (void)RunToDispenserSM(Event); } return(Event); } /**************************************************************************** Function DuringAtDispenser Parameters Event_t Returns Event_t Description On Entry, start RequestBall SM and, if near our hoop, Shoot SM, and run these. ****************************************************************************/ static Event_t DuringAtDispenser( Event_t Event) { // process EV_ENTRY & EV_EXIT events if ( Event == EV_ENTRY) { StartRequestBallSM(Event); if (NearHoopFlag) StartShootSM(Event); StateStartTime = TMRS12_GetTime(); StateTimeoutTime = ATDISPENSER_TIMEOUT; }else if ( Event == EV_EXIT) { (void)RunRequestBallSM(Event); if (NearHoopFlag) (void)RunShootSM(Event); }else // do the 'during' function for this state { (void)RunRequestBallSM(Event); if (NearHoopFlag) (void)RunShootSM(Event); } return(Event); }