/**************************************************************************** Module SMBeaconDetection.c Description State Machine for beacon detection Notes Each of the two beacon detectors' outputs is fed into an input capture port, where high and low times are recorded. The period and on-time of the signal that each detector sees is identified as one of the six beacons, or as no beacon. Which beacon each detector sees, and whether or not it sees a beacon at all, are debounced separately. Based on whether both or just one detector can see a beacon, that beacon's angular direction can be inferred. A history is kept for the direction of each of the six beacons. ****************************************************************************/ /*----------------------------- Include Files -----------------------------*/ #include "GlobalHeader.h" #include "SMBeaconDetection.h" /*----------------------------- Module Defines ----------------------------*/ #define LARGE_PERIOD 1700 //in 0.66us. 1680 is 1120us. #define BEACON_DEBOUNCE 5 //cycles to debounce which beacon is tracked #define VISIBLE_DEBOUNCE 2 //cycles to debounce whether a detector sees a beacon #define EDGE_DEBOUNCE_TIME 120 //in 0.66us. 120 is 80us. time to debounce edges. //Period Measurement #define PeriodDivisor 240 #define NominalPeriodShort 3 //880 us #define NominalPeriodLong 4 //1120 us #define ThresholdShiftPeriod 40 //shift from -160/+80 //On Time Measurement #define OnTimeDivisor 160 #define NominalDispenser2 1//240 #define NominalDispenser3 2//400 #define NominalGreenHoop 3//560 #define NominalDispenser0 5//880 #define NominalDispenser1 4//720 #define NominalRedHoop 3//560 #define ThresholdShiftOnTime (-20) //shift from +/-80 typedef enum { Beacon_Visible = 1, Beacon_NotVisible = 0 } BeaconVisible_t; //whether each detector sees a beacon or not /*---------------------------- Module Functions ---------------------------*/ static Event_t DuringRunning( Event_t Event); static void BeaconDetectionInit(void); static void UpdateTracking(void); static void UpdateDetector(BeaconDetector_t Detector); static Beacon_t DetermineBeacon(unsigned int Period, unsigned int OnTime); /*---------------------------- Module Variables ---------------------------*/ // everybody needs a state variable, you may need others as well static BeaconState_t CurrentState; //interrupt variables static unsigned char DetectorFlag[2] = {0,0}; //whether there is a new edge static unsigned char DetectorState[2] = {0,0}; //to determine high vs low time static unsigned int DetectorInterval1[2] = {0,0}; //most recent interval static unsigned int DetectorInterval2[2] = {0,0}; //previos interval //individual detector specific beacon tracking and debounce static Beacon_t DetectorTracking[2] = {Beacon_None, Beacon_None}; static Beacon_t DetectorLastBeacon[2] = {Beacon_None, Beacon_None}; static unsigned int DetectorTrackingDebounce[2] = {0,0}; //individual detector beacon visible and debounce static BeaconVisible_t DetectorVisible[2] = {Beacon_NotVisible, Beacon_NotVisible}; static BeaconVisible_t DetectorLastVisible[2] = {Beacon_NotVisible, Beacon_NotVisible}; static unsigned int DetectorVisibleDebounce[2] = {0,0}; //detector pair beacon tracking static Beacon_t DetectorPairTracking = Beacon_None; static Beacon_t DetectorPairLastTracked = Beacon_None; //beacon direction history. static BeaconDirection_t BeaconDirection[7] = {0, 0, 0, 0, 0, 0, 0}; //0 = NeverSeen /*------------------------------ Module Code ------------------------------*/ /**************************************************************************** Function ClearBeaconDirection Parameters none Returns none Description Resets the history of each beacon direction. Useful for clearing possible identification errors due to being very close to a beacon. ****************************************************************************/ void ClearBeaconDirection(void) { BeaconDirection[0] = Beacon_NeverSeen; BeaconDirection[1] = Beacon_NeverSeen; BeaconDirection[2] = Beacon_NeverSeen; BeaconDirection[3] = Beacon_NeverSeen; BeaconDirection[4] = Beacon_NeverSeen; BeaconDirection[5] = Beacon_NeverSeen; BeaconDirection[6] = Beacon_NeverSeen; } /**************************************************************************** Function QueryBeaconTracking Parameters none Returns Beacon_t - the current beacon being tracked by both detectors Notes Will return Beacon_None if no beacon is currently identified ****************************************************************************/ Beacon_t QueryBeaconTracking (void) { return DetectorPairTracking; } /**************************************************************************** Function QueryBeaconLastTracked Parameters none Returns Beacon_t - the last beacon that was being tracked by both detectors Notes Identical to QueryBeaconTracking, but will return a valid beacon once one has been identified. ****************************************************************************/ Beacon_t QueryBeaconLastTracked (void) { return DetectorPairLastTracked; } /**************************************************************************** Function QueryBeaconDirection Parameters Beacon_t - the beacon whose direction we wish to check Returns BeaconDirection_t - the direction of the beacon, as recorded in the history Description Checks history for beacon's direction ****************************************************************************/ BeaconDirection_t QueryBeaconDirection (Beacon_t Beacon) { return BeaconDirection[Beacon]; } /**************************************************************************** Function UpdateTracking Parameters none Returns none Description Upates tracking and direction history based on debounced states of each detector. ****************************************************************************/ static void UpdateTracking(void) { Beacon_t BeaconTracking1; Beacon_t BeaconTracking2; BeaconVisible_t State1; BeaconVisible_t State2; UpdateDetector(Beacon_DetectorLeft); //update both detectors UpdateDetector(Beacon_DetectorRight); BeaconTracking1 = DetectorTracking[Beacon_DetectorLeft]; //find which beacon each detector is tracking BeaconTracking2 = DetectorTracking[Beacon_DetectorRight]; State1 = DetectorVisible[Beacon_DetectorLeft]; //State = 1 if beacon, 0 if notbeacon State2 = DetectorVisible[Beacon_DetectorRight]; if ((BeaconTracking1 == BeaconTracking2) || (State1 == Beacon_NotVisible) || (State2 == Beacon_NotVisible)) //beacons are same, or notbeacon { Beacon_t Beacon = ((State1 == Beacon_Visible) ? BeaconTracking1 : BeaconTracking2); //determine which beacon we see DetectorPairTracking = Beacon; if (Beacon != Beacon_None) //if we see a beacon { DetectorPairLastTracked = Beacon; if ((State1 == Beacon_Visible) && (State2 == Beacon_Visible)) BeaconDirection[Beacon] = Beacon_Straight; if ((State1 == Beacon_Visible) && (State2 == Beacon_NotVisible)) BeaconDirection[Beacon] = Beacon_Left; if ((State1 == Beacon_NotVisible) && (State2 == Beacon_Visible)) BeaconDirection[Beacon] = Beacon_Right; } if ((State1 == Beacon_NotVisible) && (State2 == Beacon_NotVisible)) //if we no longer see a beacon { Beacon_t OldBeacon = DetectorPairLastTracked; //update history for previous beacon if (BeaconDirection[OldBeacon] == Beacon_Right) BeaconDirection[OldBeacon] = Beacon_LostRight; //if beacon was on right, now gone, then was lost right. etc. if (BeaconDirection[OldBeacon] == Beacon_Left) BeaconDirection[OldBeacon] = Beacon_LostLeft; if (BeaconDirection[OldBeacon] == Beacon_Straight) BeaconDirection[OldBeacon] = Beacon_NeverSeen; } } } /**************************************************************************** Function UpdateDetector Parameters BeaconDetector_t Detector - number of detector to update Returns none Description Updates whether each detector sees a beacon and which beacon it sees. Implements debounce for each separately. ****************************************************************************/ static void UpdateDetector(BeaconDetector_t Detector) { if (DetectorFlag[Detector] == 1) //if there has been a new interrupt { unsigned char State; unsigned int NewInterval; unsigned int OldInterval; Beacon_t Beacon = Beacon_None; BeaconVisible_t BeaconVisible = Beacon_NotVisible; DisableInterrupts; State = DetectorState[Detector]; NewInterval = DetectorInterval1[Detector]; OldInterval = DetectorInterval2[Detector]; EnableInterrupts; DetectorFlag[Detector] = 0; if (State > 0) //if the intterupt was from a signal edge, not a timeout { unsigned int Period; unsigned int OnTime; NewInterval = NewInterval*2/3; //calculate high and low times OldInterval = OldInterval*2/3; Period = (NewInterval + OldInterval); OnTime = ((State == 2) ? OldInterval : NewInterval); //State is 1 if Interval1 is high time, since the previous //interval is opposite the one for which this measurement is taken Beacon = DetermineBeacon(Period, OnTime); BeaconVisible = Beacon_Visible; } //Debounce to update Tracking and Visible states if (Beacon == DetectorLastBeacon[Detector]) { DetectorTrackingDebounce[Detector] += ((DetectorTrackingDebounce[Detector] < BEACON_DEBOUNCE) ? (1+(State==0)) : 0); //weights timeouts 2X since they are half as frequent if (DetectorTrackingDebounce[Detector] >= BEACON_DEBOUNCE) //if we are debounced, update DetectorTracking[Detector] = Beacon; } else DetectorTrackingDebounce[Detector] = 0; if (BeaconVisible == DetectorLastVisible[Detector]) { DetectorVisibleDebounce[Detector] += ((DetectorVisibleDebounce[Detector] < VISIBLE_DEBOUNCE) ? (1+(State==0)) : 0); //weights timeouts 2X since they are half as frequent if (DetectorVisibleDebounce[Detector] >= VISIBLE_DEBOUNCE) //if we are debounced, update DetectorVisible[Detector] = BeaconVisible; } else DetectorVisibleDebounce[Detector] = 0; //Update LastBeacon and LastVisible states DetectorLastBeacon[Detector] = Beacon; DetectorLastVisible[Detector] = BeaconVisible; } } /**************************************************************************** Function DetermineBeacon Parameters unsigned int Period, unsigned int OnTime (both in us) Returns Beacon_t - the beacon that corresponds to the inputted period and ontime Description Interprets period and ontime, returns corresponding beacon ****************************************************************************/ static Beacon_t DetermineBeacon(unsigned int Period, unsigned int OnTime) { //apply shifts and scaling defined above: unsigned int PeriodCode = (Period - ThresholdShiftPeriod)/PeriodDivisor; unsigned int OnTimeCode = (OnTime - ThresholdShiftOnTime)/OnTimeDivisor; Beacon_t Beacon = Beacon_None; //interpret period and ontime switch (PeriodCode) { case NominalPeriodShort: switch (OnTimeCode) { case NominalDispenser2: Beacon = Beacon_Dispenser2; break; case NominalDispenser3: Beacon = Beacon_Dispenser3; break; case NominalGreenHoop: Beacon = Beacon_GreenHoop; break; } break; case NominalPeriodLong: switch (OnTimeCode) { case NominalRedHoop: Beacon = Beacon_RedHoop; break; case NominalDispenser1: Beacon = Beacon_Dispenser1; break; case NominalDispenser0: Beacon = Beacon_Dispenser0; break; } break; } return Beacon; } /**************************************************************************** Function BeaconDetectionInit Parameters none Returns none Description Initializes input capture and output compare for beacon detection ****************************************************************************/ static void BeaconDetectionInit(void) { DDRT |= (BIT2HI | BIT3HI); DDRT &= (BIT0LO & BIT1LO); //Input Capture TIM0_TSCR1 |= _S12_TEN; TIM0_TSCR2 |= (_S12_PR2); //prescale of 16. 1.5MHz TIM0_TIOS &= ~(_S12_IOS4 | _S12_IOS5); //T0 and T1 input capture TIM0_TIE |= (_S12_C4I | _S12_C5I ); //enable interruot TIM0_TCTL3 |= (_S12_EDG4A | _S12_EDG4B | _S12_EDG5A | _S12_EDG5B); //capture both edges TIM0_TFLG1 = (_S12_C4F | _S12_C5F); //clear flag //Output Compare TIM0_TCTL1 &= ~(_S12_OL6 | _S12_OM6 | _S12_OL7 | _S12_OM7); //no output change TIM0_TIOS |= (_S12_IOS6 | _S12_IOS7); //T2 and T3 output compare TIM0_TIE |= (_S12_C6I | _S12_C7I); //Enable OC interrupt TIM0_TC6 = TIM0_TCNT + LARGE_PERIOD; //Set time TIM0_TC7 = TIM0_TCNT + LARGE_PERIOD; //Set time TIM0_TFLG1 = (_S12_C6F | _S12_C7F); //clear flags EnableInterrupts; } /**************************************************************************** Function RunBeaconDetectionSM Parameters Event_t CurrentEvent Returns none Description Runs BeaconDetection state machine on current event ****************************************************************************/ Event_t RunBeaconDetectionSM( Event_t CurrentEvent ) { unsigned char MakeTransition = FALSE;/* are we making a state transition? */ BeaconState_t NextState = CurrentState; switch ( CurrentState ) { case Beacon_Running : CurrentEvent = DuringRunning(CurrentEvent); //during function //process any events if ( CurrentEvent != EV_NO_EVENT ) //If an event is active { switch (CurrentEvent) { } } break; } // If we are making a state transition if (MakeTransition == TRUE) { // Execute exit function for current state (void)RunBeaconDetectionSM(EV_EXIT); CurrentState = NextState; //Modify state variable // Execute entry function for new state (void)RunBeaconDetectionSM(EV_ENTRY); } return(CurrentEvent); } /**************************************************************************** Function StartBeaconDetectionSM Parameters Event_t CurrentEvent Returns none Description Starts Beacon Detection state machine ****************************************************************************/ void StartBeaconDetectionSM ( Event_t CurrentEvent ) { CurrentState = Beacon_Running; // call the entry function (if any) for the ENTRY_STATE //printf("Beacon Started\r\n"); (void)RunBeaconDetectionSM(EV_ENTRY); } /**************************************************************************** Function QueryBeaconDetectionSM Parameters none Returns BeaconSatate_t Current State of SM Description gives state of machine ****************************************************************************/ BeaconState_t QueryBeaconDetectionSM ( void ) { return(CurrentState); } /*************************************************************************** private functions ***************************************************************************/ /**************************************************************************** Function DurringRunning Parameters Event_t Event Returns Event_t Event Description Executes Beacon Detection initialization and updates tracking every cycle ****************************************************************************/ static Event_t DuringRunning( Event_t Event) { // process EV_ENTRY & EV_EXIT events if ( Event == EV_ENTRY) { BeaconDetectionInit(); //printf("entering beacon running \r\n"); }else if ( Event == EV_EXIT) { }else // do the 'during' function for this state { UpdateTracking(); } return(Event); } /**************************************************************************** Interrupt Detector0Interrupt Description Input Capture for Detector0. Saves previous and newest widths. ****************************************************************************/ void interrupt _Vec_tim0ch4 Detector0Interrupt(void) //port T0 { static unsigned int LastEdge; unsigned int NewEdge; NewEdge = TIM0_TC4; if ((NewEdge - LastEdge) > EDGE_DEBOUNCE_TIME) { DetectorInterval2[DETECTOR0] = DetectorInterval1[DETECTOR0]; DetectorInterval1[DETECTOR0] = NewEdge - LastEdge; DetectorState[DETECTOR0] = ((PTT & BIT0HI) == BIT0HI) + 1; //State is 1 if Interval1 is high time, since the previous //interval is opposite the one for which this measurement is taken LastEdge = NewEdge; } TIM0_TC6 = TIM0_TCNT + LARGE_PERIOD; TIM0_TFLG1 = _S12_C4F; DetectorFlag[DETECTOR0] = 1; } /**************************************************************************** Interrupt Detector0Timeout Description Output Compare for Detector0. If triggered, we haven't seen a signal recently. ****************************************************************************/ void interrupt _Vec_tim0ch6 Detector0Timeout(void) //port T2 { DetectorState[DETECTOR0] = 0; TIM0_TC6 += LARGE_PERIOD; TIM0_TFLG1 = _S12_C6F; DetectorFlag[DETECTOR0] = 1; } /**************************************************************************** Interrupt Detector1Interrupt Description Input Capture for Detector1. Saves previous and newest widths. ****************************************************************************/ void interrupt _Vec_tim0ch5 Detector1Interrupt(void) //port T1 { static unsigned int LastEdge; unsigned int NewEdge; NewEdge = TIM0_TC5; if ((NewEdge - LastEdge) > EDGE_DEBOUNCE_TIME) { DetectorInterval2[DETECTOR1] = DetectorInterval1[DETECTOR1]; DetectorInterval1[DETECTOR1] = NewEdge - LastEdge; DetectorState[DETECTOR1] = ((PTT & BIT1HI) == BIT1HI) + 1; LastEdge = NewEdge; } TIM0_TC7 = TIM0_TCNT + LARGE_PERIOD; TIM0_TFLG1 = _S12_C5F; DetectorFlag[DETECTOR1] = 1; } /**************************************************************************** Interrupt Detector1Timeout Description Output Compare for Detector1. If triggered, we haven't seen a signal recently. ****************************************************************************/ void interrupt _Vec_tim0ch7 Detector1Timeout(void) //port T3 { DetectorState[DETECTOR1] = 0; TIM0_TC7 += LARGE_PERIOD; TIM0_TFLG1 = _S12_C7F; DetectorFlag[DETECTOR1] = 1; }