Sensors Pseudocode
Sensors State Machine Function: StartSensorsSM Initialize Bump, Tape, Other Sensors Module Function: RunSensorsSM State Running: Run during function for Running Function: DuringRunning Entry: Start Beacon Detection state machine Entry/Exit: Run Beacon Detection state machine Beacon Detection State Machine Function: ClearBeaconDirection Wipe history of direction each beacon was last seen in Function: BeaconDetectionInit Initialize input capture for both detectors Initialize output compare for timeouts for both detectors Function: UpdateTracking Update both detectors individually If the detectors agree on which beacon they are tracking: If a beacon is visible: If both detectors see it That beacon's direction is straight ahead If only the left detector sees it That beacon's direction is to the left If only the right detector sees it That beacon's direction is to the right if a beacon is not visible: If the last beacon visible was to the right It is now lost to the right If the last beacon visible was to the left It is now lost to the left Function: UpdateDetector If there has been a new signal edge Determine high time and period From high time and period, run DetermineBeacon to find which beacon we see Otherwise there is no beacon visible If the detector has seen this particular beacon for longer than debounce time This detector is tracking this beacon If the detector has seen any beacon for longer than (a shorter) debounce time This detector sees a beacon Function: DetermineBeacon Parameters: unsigned int Period, unsigned int OnTime Round Period and OnTime to most likely valid values Identify beacon based on these values If the values are valid, return corresponding beacon, otherwise no beacon Function: RunBeaconDetectionSM State Running: Run during function for Running Function: DuringRunning Entry: Run BeaconDetectionInit Else: UpdateTracking of detectors Interrupt: DetectorInterrupt On rising or falling edge from detector input: Record time of new edge If time since previous edge is greater than debounce time Record most recent interval (and save previous interval) Read input port to see if edge was rising or falling to determine which interval is high/low Set timeout flag to be further than one period in the future Clear flag Interrupt: DetectorTimeout On output compare: Set flag indicating we no longer see a beacon Set timeout flag to be further than one period in the future Clear flag Bump, Tape, Other Inputs Module Function: BumpTapeInit Initialize AD ports for tape and bump sensing Initialize P ports for switch inputs Initialize U ports for LED outputs Function: ReadColor Read and return status of Color switch (0 = red, 1 = green) If the game is off, blink LED of our color: If LED is off, and time interval passed, turn LED on If LED is on, and time interval passed, turn LED off If the game is on, keep our LED on Turn on LED for our color, if it should be on Turn off LED for other team color Function: TurnOnLEDs Turn on both color LEDs Function: ReadHalfCourt Read and return the status of the half court switch Function: CheckBumpSensors If debounce time has passed If state of bump sensors has changed If bump sensor is depressed Return 1 for new bump event otherwise return 0 for no new bump event Function: CheckForBall If sample time has passed Read state of ball-sensing tape sensor If state is same as previous If debounce time has passed Debounced State is Current State If Current State is 0, then Debounced State is set to 0 regardless of debounce time Return the debounced state Function: ClearFrontTapeCache / ClearRearTapeCache Set all entries of relevant tape cache to 0 Function: CheckFrontTapeCache / CheckRearTapeCache Check whether each relevant tape sensor sees tape, if so store a 1 in it's cache location If the relevant cache is all 1s, and has not been checked before return 1 for tape cache full event otherwise return 0