Embedded Files
Overview
Overview
This entire project was coded in C following the Events and Services framework.
The code is available on BitBucket.
State Charts
State Charts
Main Service (Top HSM)
Main Service (Top HSM)
Governs the overall system of the project.
Pseudo Code for MainServiceTopHSM
Pseudo Code for MainServiceTopHSM
Module: MainServiceTopHSM
Description: Control module for a hierarchical state machine in a robotic system.
1. Function: InitMainServiceTopHSM(Priority)
Takes in: Priority
Returns: Boolean (True if successful, False otherwise)
Local Variables: ThisEvent (event structure)
- Initialize the screen and log messages.
- Configure button inputs and LED outputs.
- Set ONOFF_LED to the off state.
- Save Priority to MyPriority.
- Set ThisEvent to ES_ENTRY.
- Start the state machine with ThisEvent.
- Return True.
2. Function: PostMainServiceTopHSM(ThisEvent)
Takes in: ThisEvent (event structure)
Returns: Boolean (True if successful, False otherwise)
- Post ThisEvent to the service queue using MyPriority.
- Return the posting operation's success status.
3. Function: RunMainServiceTopHSM(CurrentEvent)
Takes in: CurrentEvent (event structure)
Returns: ES_Event (event structure)
Local Variables: MakeTransition (boolean), NextState (state enum), EntryEventKind (event structure), ReturnEvent (event structure)
- Initialize MakeTransition to False.
- Set NextState to CurrentState.
- Set EntryEventKind to ES_ENTRY and ReturnEvent to ES_NO_EVENT.
- Switch based on CurrentState:
- MAIN_IDLE:
- Print "IDLE!!".
- If event is ES_BUTTON1:
- Turn ONOFF_LED on, start GameTimer with WHOLE_GAMING duration.
- Set NextState to MAIN_PREPARATION and MakeTransition to True.
- If event is ES_BUTTON2:
- Set NextState to FIRSTSHOT and MakeTransition to True.
- If event is ES_TIMEOUT and EventParam matches GameTimer:
- Increment WholeTimeout and check for game end condition. Reset timer if necessary.
- MAIN_PREPARATION:
- Print "PREPARATION!!!".
- Call DuringPreparation with CurrentEvent.
- Handle ES_TIMEOUT for GameTimer similarly as in MAIN_IDLE.
- If event is ES_ONESHOT, set NextState to FIRSTSHOT and MakeTransition to True.
- FIRSTSHOT:
- Print "FIRSTSHOT!!!".
- Call DuringOneShot with CurrentEvent.
- Handle ES_STOP: push blade, start PushTimer.
- If event is ES_DONE, set NextState to TREE and MakeTransition to True.
- Handle ES_TIMEOUT for PushTimer and GameTimer, transitioning states as needed.
- TREE:
- Call DuringTree with CurrentEvent.
- If event is ES_DONE_TREE, toggle TreeFlag, update NextState to DEFENSE or TREE based on TreeFlag, and set MakeTransition to True.
- Handle ES_TIMEOUT for GameTimer similarly as in MAIN_IDLE.
- DEFENSE:
- Call DuringDefense with CurrentEvent.
- Handle ES_TIMEOUT for GameTimer, setting NextState and MakeTransition as needed.
- If MakeTransition is True:
- Set CurrentEvent to ES_EXIT and run the state machine.
- Update CurrentState to NextState.
- Run the state machine with EntryEventKind.
- Return ReturnEvent.
4. Function: StartMainServiceTopHSM(CurrentEvent)
Takes in: CurrentEvent (event structure)
Returns: None
- Set CurrentState to MAIN_IDLE and TreeFlag to False.
- Run the state machine with CurrentEvent.
5. Function: QueryMainServiceTopHSM()
Takes in: None
Returns: MasterState_t (the current state)
- Return CurrentState.
6. Function: QueryTreeInfo(Tree, FlagRe)
Takes in: Pointer to Tree structure, Pointer to FlagRe boolean
Returns: None
- Assign current or next tree info to Tree and FlagRe based on TreeFlag.
7. Function: QueryOpponentInfo(Tree)
Takes in: Array of Tree structures
Returns: None
- Assign opponent tree info to Tree array.
8. During Functions:
a. Function: DuringOneShot(Event)
Takes in: Event (event structure)
Returns: ES_Event (event structure)
Local Variables: ReturnEvent
- ReturnEvent is initially set to the input Event.
- If the Event is ES_ENTRY or ES_ENTRY_HISTORY:
- Call PostBeaconService with ES_ALIGN_FIRE.
- Call PostDriveTrainService with ES_ROTATE_FULL and ROTATE_CCW parameters.
- If the Event is ES_EXIT:
- (Actions to perform when exiting this state, if any)
- Return ReturnEvent.
b. Function: DuringPreparation(Event)
Takes in: Event (event structure)
Returns: ES_Event (event structure)
Local Variables: ReturnEvent, newEvent
- ReturnEvent is initially set to the input Event.
- If the Event is ES_ENTRY or ES_ENTRY_HISTORY:
- Call PostBeaconService with ES_PREPARATION.
- Call PostDriveTrainService with ES_ROTATE_FULL and ROTATE_CW parameters.
- If the Event is ES_EXIT:
- Call QueryDeltaArray to get DeltaArray and FreqArray.
- Call FindMaxIdx on DeltaArray to get the maximum index.
- Determine Tree1 and Tree2 based on the MaxIdx and update corresponding LED indicators.
- Return ReturnEvent.
c. Function: DuringTree(Event)
Takes in: Event (event structure)
Returns: ES_Event (event structure)
Local Variables: ReturnEvent
- ReturnEvent is initially set to the input Event.
- If the Event is ES_ENTRY or ES_ENTRY_HISTORY:
- Start the Tree service state machine with the Event.
- If the Event is ES_EXIT:
- Toggle TreeFlag.
- During the state:
- Run Tree service state machine with Event.
- Return ReturnEvent.
d. Function: DuringDefense(Event)
Takes in: Event (event structure)
Returns: ES_Event (event structure)
Local Variables: ReturnEvent
- ReturnEvent is initially set to the input Event.
- If the Event is ES_ENTRY or ES_ENTRY_HISTORY:
- Start the Defense service state machine with the Event.
- During the state:
- Run Defense service state machine with Event.
- Return ReturnEvent.
9. Utility Function: FindMaxIdx(Array)
Takes in: Array (an array of integers)
Returns: Integer (index of maximum value)
- Iterate through Array to find the index of the maximum value.
- Return the index of this maximum value.
Tree Service (Low HSM)
Tree Service (Low HSM)
Second step of the strategy, collecting all the acorns and shooting them out.
Pseudo Code for TreeServiceHSM
Pseudo Code for TreeServiceHSM
Module: TreeServiceHSM
Description: Manages tree detecting, ACORN grabbing, and pushing operations in a robotic system.
Constants:
- Define states: ALIGN, FORWARD, GRABING, ALIGN_FIRE, PUSH.
- Define rotation directions: ROTATE_CW, ROTATE_CCW.
- Time constants: ONE_SEC, EIGHT_SEC, TWO_SEC.
Module Variables:
- CurrentState: The current state of the Tree state machine.
- GrabCounter: A counter for the number of grabs performed.
- TargetTree: Structure containing information about the target tree.
- TreeFlag: A flag indicating the tree state.
Function: RunTreeServiceSM(CurrentEvent)
Takes in: CurrentEvent (event structure)
Returns: ES_Event (event structure)
- Initialize variables: MakeTransition, NextState, EntryEventKind, ReturnEvent.
- Switch based on CurrentState:
- ALIGN:
- Call DuringAlign with CurrentEvent.
- If ES_TREE_FOUND:
- If TreeFlag is false, rotate 45 degrees and start AlignTreeTimer.
- Else, post ES_FORWARD_FULL to all services.
- If ES_TIMEOUT and AlignTreeTimer expires, post ES_FORWARD_FULL.
- If ES_FORWARD_FULL, transition to FORWARD state.
- FORWARD:
- If ES_STOP, transition to GRABING state.
- GRABING:
- Call DuringGrabing with CurrentEvent.
- If ES_TIMEOUT:
- On PullInTimer expiration, reverse and start ReverseTimer.
- On ReverseTimer expiration, transition to ALIGN_FIRE state.
- ALIGN_FIRE:
- Call DuringAlignFire with CurrentEvent.
- If ES_STOP, transition to PUSH state.
- If ES_TIMEOUT:
- On AlignFireTimer expiration, move forward and start CurtainTimer.
- On CurtainTimer expiration, stop movement.
- PUSH:
- Call DuringPush with CurrentEvent.
- If ES_DONE, prepare for reverse.
- If ES_STOP:
- Increment GrabCounter.
- If GrabCounter equals 3, signal ES_DONE_TREE.
- Transition back to ALIGN state.
- Handle state transitions.
- Return ReturnEvent.
Function: StartTreeServiceSM(CurrentEvent)
Takes in: CurrentEvent (event structure)
Returns: None
- Initialize CurrentState to ENTRY_STATE and reset GrabCounter.
- Call QueryTreeInfo to set TargetTree and TreeFlag.
- Run state machine with CurrentEvent.
Function: QueryTreeServiceSM()
Takes in: None
Returns: CurrentState (TreeState_t)
- Return CurrentState.
During Functions:
- DuringAlign(Event): Handle vehicle rotation, beacon service interaction, and timeouts.
- DuringGrabing(Event): Manage pull-in operation and timers for grabbing and reversing.
- DuringAlignFire(Event): Control vehicle orientation for firing and handle related timeouts.
- DuringPush(Event): Execute pushing operation and manage transition after pushing.
Defense Service (Low HSM)
Defense Service (Low HSM)
Find acorns scattered around the arena, collet them, and shoot them back.
Pseudo Code for DefenseServiceHSM
Pseudo Code for DefenseServiceHSM
Module: DefenseServiceHSM
Description: Manages the defense strategy in a state machine, involving finding, obtaining, and firing actions.
Constants:
- Define states: FINDING, OBTAIN, FIRE.
- Time constant: ONE_SEC.
Module Variables:
- CurrentState: The current state of the Defense state machine.
Function: RunDefenseServiceSM(CurrentEvent)
Takes in: CurrentEvent (event structure)
Returns: ES_Event (event structure)
Local Variables: MakeTransition, NextState, EntryEventKind, ReturnEvent
- Switch based on CurrentState:
- FINDING:
- Call DuringFinding with CurrentEvent.
- If ES_FOUND received, transition to OBTAIN state.
- OBTAIN:
- Call DuringObtain with CurrentEvent.
- If ES_GOT, post ES_CLOSE_DOOR to BladeNPushService.
- If ES_DONE_CLOSE received, transition to FIRE state.
- FIRE:
- If ES_ROTATE45, post to DriveTrainService and start MOTOR_TIMER.
- If ES_TIMEOUT on MOTOR_TIMER, post ES_PUSH to BladeNPushService.
- If ES_DONE, transition back to FINDING state.
- Manage state transitions.
- Return ReturnEvent.
Function: StartDefenseServiceSM(CurrentEvent)
Takes in: CurrentEvent (event structure)
Returns: None
- Initialize CurrentState to ENTRY_STATE.
- Run state machine with CurrentEvent.
Function: QueryDefenseServiceSM()
Takes in: None
Returns: CurrentState (DefenseState_t)
- Return CurrentState.
During Functions:
- DuringFinding(Event):
- On ES_ENTRY, post ES_FINDING to DriveTrainService, and ES_EMIT to AcornDetectorService.
- On ES_EXIT, post ES_FORWARD_FULL to DriveTrainService.
- DuringObtain(Event):
- Handle any activity while in the OBTAIN state.
- On ES_EXIT, post ES_ALIGN_FIRE to BeaconService.
Indicator Service (FSM)
Indicator Service (FSM)
Indicate which side we are on during the whole game.
Pseudo Code for IndicatorServoService
Pseudo Code for IndicatorServoService
Module: IndicatorServoService
Description: Manages a service for operating an indicator servo under the Gen2 Events and Services Framework.
Constants:
- PWM_Period: Sets the PWM frequency for 50 Hz.
- TICS_PER_MS: Ticks per millisecond based on clock rate.
- Servo position constants: FULL_CW, FULL_CCW, MID_POINT_LEFT, MID_POINT_RIGHT, etc.
- TICKS_PER_STEP: Defines the granularity of servo movement.
Module Variables:
- MyPriority: Stores the priority of this service.
- dutyCycle: The duty cycle for the PWM signal.
- CurrentPosition: Tracks the current position of the servo.
- CurrentDC: Tracks the current duty cycle.
- cwLimit, ccwLimit: Limits for the servo rotation.
- timeStep: Time step for changing the speed of servo movement.
- isMoving: Flag to indicate if the servo is moving.
Function: InitIndicatorServoService(Priority)
Takes in: Priority (uint8_t)
Returns: bool (true if successful, false otherwise)
- Set MyPriority to the provided Priority.
- Initialize the servo's position and PWM settings.
- Set the PWM signal for the initial position.
- Post the ES_INIT event.
- Return the status of the initialization.
Function: PostIndicatorServoService(ThisEvent)
Takes in: ThisEvent (ES_Event_t)
Returns: bool (true if successful, false otherwise)
- Post ThisEvent to the service queue using MyPriority.
- Return the success of the posting operation.
Function: RunIndicatorServoService(ThisEvent)
Takes in: ThisEvent (ES_Event_t)
Returns: ES_Event (ES_NO_EVENT if no error, otherwise ES_ERROR)
- Switch on ThisEvent.EventType:
- ES_NEW_KEY: // only for testing
- If 'z' received, post ES_Ed to self.
- If 'x' received, post ES_Karl to self.
- ES_NEW_CMD:
- If ED_YARD, post ES_Ed to self.
- If KARL_YARD, post ES_Karl to self.
- ES_Ed: Operate servo to ED_ANGLE.
- ES_Karl: Operate servo to KARL_ANGLE.
- Return ReturnEvent (typically ES_NO_EVENT).
Private Function: PWM_Operate_Indicator(dutyCycle)
Takes in: dutyCycle (int)
- Set OC4RS to the given dutyCycle value to control the servo.
Note:
- The code assumes specific hardware settings and servo characteristics.
Services
Services
Pseudo Code for BeaconService
Pseudo Code for BeaconService
Module: BeaconService
Description: Implements service for tree beacon detection and posts events to related modules.
Include Files:
- Event and Service Framework headers, MainServiceTopHSM, SPI, HexCode, DriveTrainService, etc.
Module Defines:
- ROTATE_CW, ROTATE_CCW for rotation directions.
- Freq2Tick array maps beacon frequencies to timer ticks.
- Definitions for various servo angle positions.
Module Variables:
- MyPriority: Priority of this service.
- DeltaArray, FreqArray: Arrays to store frequency and delta values for beacon detection.
- Various flags and counters for service operation.
Function: InitBeaconService(Priority)
Takes in: Priority (uint8_t)
Returns: bool (true if successful, false otherwise)
- Sets up service priority.
- Initializes hardware configurations for beacon detection (e.g., interrupt pins, timers).
- Posts ES_INIT event to service and returns true if successful.
Function: PostBeaconService(ThisEvent)
Takes in: ThisEvent (ES_Event_t)
Returns: bool (true if successful, false otherwise)
- Posts ThisEvent to service queue using MyPriority.
Function: RunBeaconService(ThisEvent)
Takes in: ThisEvent (ES_Event_t)
Returns: ES_Event (ES_NO_EVENT if no error, otherwise ES_ERROR)
- Processes events like ES_INIT, ES_TREE, ES_ALIGN_FIRE, ES_ONESHOT, and ES_PREPARATION.
- Performs actions based on current service state and event.
- Returns an appropriate event based on processing.
Function: QueryDeltaArray(QueryDeltaArray, QueryFreqArray)
Takes in: Pointers to QueryDeltaArray and QueryFreqArray
- Copies DeltaArray and FreqArray contents to the provided arrays.
Private Function: __ISR(_TIMER_2_VECTOR, IPL4SOFT) TimeRolloverISR
- Purpose: Handle timer 2 rollover interrupts for time tracking.
- Actions:
- Disable interrupts.
- Check if Timer 2 interrupt flag is set.
- If set, increment the upper 16 bits of the FullTime structure.
- Clear the Timer 2 interrupt flag.
- Re-enable interrupts.
Private Function: __ISR(_INPUT_CAPTURE_2_VECTOR, IPL5SOFT) InputHandler
- Purpose: Handle input capture events to measure beacon signal frequency.
- Actions:
- Disable interrupts.
- Read captured times from the IC2 buffer until the buffer is empty.
- Clear the Input Capture 2 interrupt flag.
- Check for timer 2 rollover and update FullTime accordingly.
- Calculate the period of the beacon signal.
- Update LastTime to the current timestamp.
- #ifdef TestHarness3:
- Check for a specific frequency (test mode) and post a test code via SPI if matched.
- If in preparation mode (preparationFlag is true):
- Increment period_right_counter if the period matches the target tree frequency.
- Update DetectedTrees array with detected tree information.
- Post an event (e.g., ES_ONESHOT) to MainServiceTopHSM when all trees are detected.
- If in alignTreeFlag mode:
- Increment period_right_counter if the period matches the target tree frequency.
- Post ES_TREE_FOUND event when the target tree is detected.
- If in alignFireFlag mode:
- Check for opponent trees and handle rotation and alignment accordingly.
- Re-enable interrupts.
Implementation Notes:
- The service uses input capture and timer interrupts to measure the frequency of signals from the tree beacons.
- It handles different states and transitions based on the detected frequencies and posted events.
Pseudo Code for SPIService
Pseudo Code for SPIService
Module: SPIService
Description:
- Manages SPI communication in a Gen2 Events and Services Framework.
Constants:
- SET_SS_DRIV: Slave Select pin for the drivetrain.
- SET_SS_PER: Slave Select pin for the peripheral.
- ACORN_IN_BAY: Byte code indicating acorn is in the bay.
- DIST_WALL_CLOSE: Byte code indicating proximity to a wall.
Variables:
- MyPriority: Priority level of the SPIService module.
Function: InitSPIService(Priority)
- Set MyPriority to Priority.
- Initialize SPI:
- Disable analog function on pins (ANSELAbits and ANSELBbits).
- Set pin modes (TRISAbits and TRISBbits).
- Initialize SPI as the leader using SPI_Leader_Init().
Function: PostSPIService(ThisEvent)
- Post an event to the SPIService's queue.
Function: RunSPIService(ThisEvent)
- Handle events based on their type:
- ES_INIT: Print "init" for debugging.
- ES_BUTTON1: Send BUTTON_PRESS byte via SPI and set the slave select for the peripheral low.
- ES_NEW_KEY:
- Handle various key presses ('a', 's', 'd', etc.) to send corresponding SPI messages.
- Set the slave select for the drivetrain low after each message is sent.
Function: SPI_Leader_Init()
- Basic configuration for SPI1.
- Disable SPI1 for setup.
- Set SPI1 as the leader and configure bit time and data transfer settings.
- Map output and input pins for SPI communication.
- Initialize leader interrupts for SPI.
- Clear overflow status and enable SPI1.
Function: SPISetup_InitLeaderInterrupts()
- Disable interrupts and set the multi-vector interrupt mode.
- Configure and enable RX interrupt for SPI1.
- Enable global interrupts.
Interrupt Service Routine: SPILeaderRecvHandler()
- Disable interrupts.
- Set slave select pins high.
- Handle received data from SPI1:
- If data is STOP_DRIV_TO_LEADER, post a stop event.
- If data is DIST_WALL_CLOSE, post a forward half event.
- Handle other specific data codes as needed.
- Clear the SPI1 RX interrupt flag.
- Re-enable interrupts.
Pseudo Code for DriveTrainService
Pseudo Code for DriveTrainService
Module: DriveTrainService
Description: Manages motor control for the drivetrain within a Gen2 Events and Services framework. It handles motor initialization and control, and can send feedback events to other modules.
Constants:
- Definitions for PWM period and percentage calculations.
- Time definitions such as ONE_SEC, TWO_SEC, etc.
- Pin definitions for motor control.
Module Variables:
- MyPriority: Priority of the service.
- Various variables for motor operation control like curr_cmd, curr_duty, etc.
Function: InitDriveTrainService(Priority)
Takes in: Priority (uint8_t)
Returns: bool (true if successful, false otherwise)
- Set up motor control hardware configurations (pin modes, timer, output compare).
- Post ES_INIT event to service.
- Return initialization status.
Function: PostDriveTrainService(ThisEvent)
Takes in: ThisEvent (ES_Event_t)
Returns: bool (true if successful, false otherwise)
- Posts ThisEvent to the service queue using MyPriority.
Function: RunDriveTrainService(ThisEvent)
Takes in: ThisEvent (ES_Event_t)
Returns: ES_Event (ES_NO_EVENT if no error, ES_ERROR otherwise)
- Process events based on ThisEvent.EventType:
- ES_INIT:
- Initialize motor control pins to inactive state.
- Set PWM duty cycle for both wheels to 0 (stop motors).
- ES_NEW_CMD:
- Handle various drive commands like STOP, REVERSE, ROTATE, and FORWARD.
- Depending on the command, set motor direction, adjust PWM duty cycle, or initiate rotation.
- ES_TIMEOUT:
- Handle motor stop after a specified duration. Used in conjunction with movement commands.
- ES_STOP:
- Stop both motors by setting PWM duty cycle to 0.
- ES_REVERSE, ES_FINDING, ES_ROTATE_DONE, ES_ROTATE_FULL, ES_ROTATE45, ES_ROTATE90, ES_FORWARD_FULL, ES_FORWARD_HALF:
- Specific cases for each command, involving direction control and PWM adjustments.
- For rotation commands, manage rotation duration and angle through PWM adjustments and timing.
- ES_ADJUST_LEFT, ES_ADJUST_RIGHT:
- Minor directional adjustments using differential PWM on wheels.
- ES_NEW_KEY:
- Special case for testing purposes, controlled by keyboard input for manual adjustments.
- Adjust duty cycle and direction based on key inputs.
- ES_TEST:
- Test mode to operate motors at a specific duty cycle for a predefined time.
- Return ReturnEvent indicating status of processing.
Private Function: HW_Timer3Init
- Purpose: Initializes Timer 3 for PWM operation.
- Actions:
- Disable Timer 3.
- Configure Timer 3 for internal clock source and non-gated mode.
- Set prescale value for Timer 3 to achieve the desired PWM frequency.
- Clear the Timer 3 register and set the PWM period.
- Enable Timer 3.
Private Function: OutputCompareInit
- Purpose: Initializes Output Compare modules for PWM to control motors.
- Actions:
- Disable Output Compare 2 (OC2) and Output Compare 3 (OC3).
- Set OC2 and OC3 to use Timer 3.
- Configure OC2 and OC3 for PWM mode without fault input.
- Map OC2 to RB5 and OC3 to RPA3 (or other designated pins).
- Set initial duty cycle for OC2 and OC3 to 50% of the PWM period.
- Enable OC2 and OC3.
Private Function: PWM_Operate_Wheel1(dutyCycle)
- Takes in: dutyCycle (int, range 0-100)
- Purpose: Operates Wheel 1 with the given duty cycle.
- Actions:
- Calculate the register value for OC2 based on the dutyCycle percentage.
- Update OC2RS to adjust the duty cycle of the PWM signal controlling Wheel 1.
Private Function: PWM_Operate_Wheel2(dutyCycle)
- Takes in: dutyCycle (int, range 0-100)
- Purpose: Operates Wheel 2 with the given duty cycle.
- Actions:
- Calculate the register value for OC3 based on the dutyCycle percentage.
- Update OC3RS to adjust the duty cycle of the PWM signal controlling Wheel 2.
Implementation Notes:
- The service uses hardware-level controls to manage the motors for driving actions like moving forward, reversing, rotating, etc.
- It reacts to a variety of commands and adjusts the drivetrain accordingly.
Pseudo Code for EncoderService
Pseudo Code for EncoderService
Module: EncoderService
Description: Manages the encoder readings for wheels within a Gen2 Events and Services framework. It includes functions for initializing encoders, handling encoder signals, and calculating wheel speeds.
Constants:
- PWM Period and duty cycle calculations.
- Time definitions such as ONE_SEC, TWO_SEC, etc.
- Pin definitions for encoders on each wheel.
- Gearbox ratio for drivetrain.
Module Variables:
- MyPriority: Priority of the service.
- Variables to count rising edges and track periods for each wheel.
Function: InitEncoderService(Priority)
- Parameters: Priority (uint8_t) - The priority of this service.
- Returns: bool - True if successful, false otherwise.
- Description:
- Sets up hardware configurations for encoder readings (pin modes, timer interrupts).
- Initializes Input Capture for both wheels.
- Posts an ES_INIT event to the service.
- Returns the status of initialization.
Function: PostEncoderService(ThisEvent)
- Parameters: ThisEvent (ES_Event_t) - The event to post to the queue.
- Returns: bool - True if the enqueue operation was successful, false otherwise.
- Description:
- Posts ThisEvent to the service queue using MyPriority.
Function: RunEncoderService(ThisEvent)
- Parameters: ThisEvent (ES_Event_t) - The event to process.
- Returns: ES_Event - ES_NO_EVENT if no error, ES_ERROR otherwise.
- Description:
- Processes events such as ES_INIT, ES_UPDATE_PERIOD_WHEEL_1, and ES_UPDATE_PERIOD_WHEEL_2.
- Updates internal variables based on the events.
- No motor control is done in this service, only data collection and processing.
Private Functions:
GetNumEdges_Wheel1 and GetNumEdges_Wheel2
- Returns: uint32_t - The number of rising edges detected for each wheel.
- Description: Returns the count of rising edges detected by the encoder for each wheel.
Wheel1_ICInit and Wheel2_ICInit
- Description: Initializes Input Capture for each wheel.
- Configures IC modules for capturing rising edges on encoder signals.
- Sets up appropriate interrupt priorities and clears flags.
IC2Handler and IC3Handler (ISR functions for Input Capture)
- Description: Interrupt Service Routines for handling encoder signals.
- Reads captured values from IC buffers.
- Updates timing and edge count for each wheel.
Timer2ISR and Timer3ISR
- Description: Timer Interrupt Service Routines for rollover handling.
- Ensures accurate timing by adjusting for timer rollovers.
Timer2_InterruptInit and Timer3_InterruptInit
- Description: Initializes Timer 2 and Timer 3 for encoder services.
- Configures timer settings for rollover and interrupt handling.
Implementation Notes:
- The EncoderService is dedicated to processing encoder signals from the wheels, providing essential data for speed and distance measurements.
- It does not control motors directly but serves as a data provider for other services that require wheel speed information.
Pseudo Code for BladeNPushService
Pseudo Code for BladeNPushService
Module: BladeNPushService
Description:
Manages blade and pusher functionalities in a Gen2 Event and Service Framework.
Constants:
Define PWM_Period as 49999 (for 50 Hz).
Define TICS_PER_MS as 2500 (clock rate assumption: 20MHz PBClk / 8).
Define servo motion range constants:
FULL_CW = 0.7 * TICS_PER_MS
FULL_CCW = 2.5 * TICS_PER_MS
MID_POINT_LEFT = 1.3 * TICS_PER_MS
MID_POINT_RIGHT = 1.65 * TICS_PER_MS
HX_L = 1.4 * TICS_PER_MS
RIGHT_ANGLE = 1.05 * TICS_PER_MS
LEFT_ANGLE = 1.95 * TICS_PER_MS
RIGHT_ANGLE_60 = 1.2 * TICS_PER_MS
LEFT_ANGLE_60 = 1.7 * TICS_PER_MS
CLOSE_ANGLE = 2.00 * TICS_PER_MS
CLOSE_ANGLE_LEFT = 1.00 * TICS_PER_MS
Define TICKS_PER_STEP as (FULL_CCW - FULL_CW) / 100
Define PUSH_MOTOR as LATAbits.LATA3
Variables:
MyPriority: uint8_t
CurrentPosition: uint16_t
CurrentDC: int8_t
cwLimit: uint16_t
ccwLimit: uint16_t
timeStep: uint16_t
isMoving: bool
lastAcornSensor1, lastAcornSensor2, lastTreeSensor, lastLimitSwitch: int
Init Function (InitBladeNPushService):
Set MyPriority to the provided Priority value.
Initialize servo control pins and hardware timers.
Configure PWM for servo control using Timer 3 and Output Compare.
Set initial positions for servos (closed door position).
Configure and initialize the push motor control.
Post the ES_INIT event to start the service.
Post Function (PostBladeNPushService):
Post events to the BladeNPush service queue.
Run Function (RunBladeNPushService):
If ThisEvent.EventType is ES_NEW_KEY: // only for testing
If ThisEvent.EventParam is 'p':
Post ES_PULL_IN event.
Else If ThisEvent.EventParam is 'd':
Post ES_CLOSE_DOOR event.
Else If ThisEvent.EventParam is 'o':
Post ES_OPEN_DOOR event.
Else If ThisEvent.EventParam is 'h':
Post ES_PUSH event.
Else If ThisEvent.EventParam is 'n':
Post ES_TRY event.
Else If ThisEvent.EventParam is 'm':
Post ES_TRY_NOW event.
Else If ThisEvent.EventType is ES_NEW_CMD:
If ThisEvent.EventParam is PULL_IN:
Post ES_PULL_IN event.
Else If ThisEvent.EventParam is CLOSE_DOOR:
Post ES_CLOSE_DOOR event.
Else If ThisEvent.EventParam is OPEN_DOOR:
Post ES_OPEN_DOOR event.
Else If ThisEvent.EventParam is PUSH:
Post ES_PUSH event.
Else If ThisEvent.EventType is ES_CLOSE_DOOR:
Operate servos to close the door.
Post ES_DONE_CLOSE event.
Else If ThisEvent.EventType is ES_OPEN_DOOR:
Operate servos to open the door.
Post ES_DONE_OPEN event.
Else If ThisEvent.EventType is ES_PULL_IN:
Operate servos to pull in.
Post ES_CLOSE_DOOR event.
Else If ThisEvent.EventType is ES_PUSH:
Operate servos to prepare for pushing.
Start a timer for pushing.
Else If ThisEvent.EventType is ES_TIMEOUT:
If ThisEvent.EventParam is Pushing_Timer:
Stop the push motor.
Post ES_DONE event.
Else If ThisEvent.EventParam is Open_Door_Timer:
Start pushing.
Else If ThisEvent.EventType is ES_TRY:
Operate the plate servo.
Else If ThisEvent.EventType is ES_TRY_NOW:
Operate the plate servo.
Else If ThisEvent.EventType is ES_BUTTON1:
Activate the push motor.
Private Functions:
HW_Timer3Init: Initializes Timer 3 for PWM operation.
OutputCompare: Initializes Output Compare for PWM control of servos.
PWM_Operate_Right, PWM_Operate_Left, PWM_Operate_Plate: Control individual servos' positions.
Overall Functionality:
The service integrates servo control for blade movements and a push motor to manage physical actions.
It listens to specific events and commands, controlling the hardware to execute these actions.
Pseudo Code for AcornDetection
Pseudo Code for AcornDetection
Module: AcornDetectionService
Description:
Implements acorn detection functionality using an emitter and a sensor under the Gen2 Events and Services Framework.
Constants: None
Variables:
MyPriority: uint8_t
totalCount: countTime (union containing FullTime and BySection[2])
LastTime: uint32_t
NewPeriod: uint16_t
count: int
consecutive: bool
start_Detecting: bool
Init Function (InitAcornDetectionService):
Set MyPriority to the provided Priority value.
Disable global interrupts.
Initialize Timer 2 for input capture.
Initialize output compare for sensor emitter.
Configure input and output pins.
Enable Timer 2 and output compare for sensor emitter.
Enable global interrupts.
Print initialization message.
Post the ES_INIT event to start the service.
Post Function (PostAcornDetectionService):
Post events to the AcornDetection service queue.
Run Function (RunAcornDetectionService):
If ThisEvent.EventType is ES_NEW_CMD:
If ThisEvent.EventParam is EMIT:
Post ES_EMIT event.
Else If ThisEvent.EventType is ES_EMIT:
Set start_Detecting to true.
Private Functions:
HW_Timer2Init:
- Disable global interrupts
- Configure Timer 2:
- Set prescaler to 4
- Set timer mode to continue in idle mode
- Select timer 2 for input capture
- Enable interrupt on every capture event
- Set interrupt priority to 5
- Map input capture to pin RB9
- Clear Timer 2 interrupt flag
- Enable Timer 2 interrupt
- Enable global interrupts
OutputCompare_Emitter:
- Configure output compare for sensor emitter:
- Disable Timer 3
- Disable output compare 5
- Select Timer 3
- Set PWM mode without fault
- Map output to pin RB13
- Set duty cycle to 50%
- Enable output compare 5
InputCapture_Sensor:
- Configure input capture for sensor:
- Disable input capture 2
- Set multivector interrupt mode
- Configure 16-bit capture mode
- Select timer 2 for input capture
- Set input capture mode to capture on every rising edge
- Set interrupt priority to 6
- Map input capture to pin RB9
- Clear input capture 2 interrupt flag
- Enable input capture 2 interrupt
TimerHandlerISR:
- Disable global interrupts
- Increment rollover count
- Clear Timer 2 interrupt flag
- Enable global interrupts
InputCaptureISR:
- Read captured time value
- Clear input capture 2 interrupt flag
- If Timer 2 interrupt flag is set and captured time is valid:
- Increment rollover count
- Clear Timer 2 interrupt flag
- Calculate new period
- If new period is within the acorn detection range and detection is enabled:
- Increment count
- If count reaches the threshold:
- Post ES_FOUND event
- Reset count
- Disable detection
- Else:
- Reset count
Overall Functionality:
The service initializes hardware components for emitter and sensor control.
It listens for EMIT commands to start the detection process.
Upon EMIT command, it enables detection and waits for sensor readings.
When a valid sensor reading is detected, it counts consecutive readings to confirm acorn presence.
If three consecutive valid readings are detected, it posts an ES_FOUND event.
Pseudo Code for InprogressLEDService
Pseudo Code for InprogressLEDService
Module: InprogressLEDService
Description:
- This module controls the in-progress LED based on received events, primarily ES_BUTTON1 and ES_TIMEOUT.
Functions:
1. InitInProgressLEDService(uint8_t Priority):
- Description: Initializes the service and sets up the necessary configurations.
- Parameters: Priority - the priority of this service
- Returns: True if initialization is successful, otherwise false
2. PostInProgressLEDService(ES_Event_t ThisEvent):
- Description: Posts an event to the InprogressLEDService queue.
- Parameters: ThisEvent - the event to post to the queue
- Returns: True if the enqueue operation is successful, otherwise false
3. RunInProgressLEDService(ES_Event_t ThisEvent):
- Description: Processes the received event and controls the in-progress LED accordingly.
- Parameters: ThisEvent - the event to process
- Returns: ES_Event_t - ES_NO_EVENT if no error, ES_ERROR otherwise
Private Functions:
- None
Module Variables:
- MyPriority: uint8_t variable to store the priority of this service
- WholeTimeout: uint32_t variable to track the timeout duration
- INPROGRESS_LED: LATBbits variable to control the in-progress LED
Pseudo Code for UltrasonicSensorService
Pseudo Code for UltrasonicSensorService
Module: UltrasonicSensorService
Includes:
- Include ES_Configure.h: Header file for ES_Configure module containing macro definitions for configuration.
- Include ES_Framework.h: Header file for ES_Framework module containing framework functions and data structures.
- Include UltrasonicSensorService.h: Header file for UltrasonicSensorService module.
- Include HexCode.h: Header file containing hex code definitions.
- Include SPIServiceFSM.h: Header file for SPIServiceFSM module.
- Include dbprintf.h: Header file for dbprintf module for debugging.
- Include sys/attribs.h: Header file for system attributes.
Defines:
- Define NUM_SECTIONS as 2: Constant defining the number of sections.
Variables:
- MyPriority: uint8_t: Priority level of the service.
- LastEchoState: boolean: State of the ultrasonic sensor's echo pin during the last check.
- TimeAtRise: uint16_t: Timestamp when the echo pin rises.
- readyToUse: boolean: Flag indicating whether the ultrasonic sensor is ready for use.
- period_us: uint16_t: Duration of a period in microseconds.
- pulse_high: uint16_t: Duration of the high pulse in microseconds.
- last_time_us: uint32_t: Timestamp of the last recorded time in microseconds.
- rise_incident: uint32_t: Timestamp of the last rising edge detected.
- fall_incident: uint32_t: Timestamp of the last falling edge detected.
Functions:
1. InitUltrasonicService(uint8_t Priority):
- Set MyPriority to Priority: Initialize service priority level.
- Initialize hardware and timers: Configure hardware and timers for ultrasonic sensing.
- Post ES_INIT event to service queue: Initialize the service by posting an initialization event to the event queue.
2. PostUltrasonicService(ES_Event_t ThisEvent):
- Post ThisEvent to the Ultrasonic service's event queue: Add the provided event to the event queue of the Ultrasonic service.
3. RunUltrasonicService(ES_Event_t ThisEvent):
- If ThisEvent is ES_TIMEOUT:
- If ThisEvent's EventParam is ULTRASONIC_TIMER:
- Send ultrasonic pulse: Trigger the ultrasonic sensor to send a pulse.
- Initialize timer for next pulse: Set up the timer for the next pulse emission.
4. UltrasonicSendPulse(void):
- Set PulsePin high: Activate the pulse pin to send a pulse.
- Wait for ~10us: Pause for approximately 10 microseconds.
- Set PulsePin low: Deactivate the pulse pin.
5. CheckUltrasonic(void):
- Read current state of EchoPin: Read the current state of the ultrasonic sensor's echo pin.
- If current state is different from LastEchoState:
- If current state is high:
- Record current time as TimeAtRise: Capture the timestamp when the echo pin rises.
- If current state is low:
- Calculate time difference as TimeDelta: Compute the duration of the echo pulse.
- If TimeDelta is within valid range:
- Post ES_TREE event: Notify the system that an object (tree) has been detected.
- Update LastEchoState: Update the last recorded state of the echo pin.
6. Ultrasonic_ICInit():
- Configure input capture for ultrasonic sensor: Set up input capture for detecting rising and falling edges of the ultrasonic sensor's echo pin.
7. UltrasonicICHandler(void):
- Read captured value from IC buffer: Read the captured value from the input capture buffer.
- If value is valid:
- Update full_curr_time with captured value: Update the timestamp with the captured value.
- If rising edge detected:
- Record timestamp as rise_incident: Save the timestamp of the rising edge.
- If falling edge detected:
- Record timestamp as fall_incident: Save the timestamp of the falling edge.
- Calculate pulse_high duration: Compute the duration of the high pulse.
- Post event if pulse duration is valid: Notify the system if the pulse duration is within a valid range.
Event Checkers
Event Checkers
Pseudo Code for TapeService
Pseudo Code for TapeService
Module: TapeService
Description:
- This module handles tape sensor readings and alignment control.
State Variables:
- MyPriority: uint8_t // Priority of the service
- TargetTree: struct TreeInfo_t // Information about the target tree
- TreeFlag: bool // Flag indicating the presence of a tree
- currentTapeSensor[NUM_AD_CHANNELS]: array // Current tape sensor readings
- LastTapeSensors[NUM_AD_CHANNELS]: array // Previous tape sensor readings
- alignFlag: bool // Flag indicating if alignment is active
- leaderState: MasterState_t // State of the main service top-level HSM
- directionFlag: bool // Flag indicating the direction of alignment (forward or reverse)
Initialization:
- Set MyPriority to the provided priority value
- Initialize sensor pins as inputs
- Configure ADC for auto-scan on specified channels
- Read initial ADC values
- Post ES_INIT event to service
State Transition Functions:
- InitTapeService(uint8_t Priority):
- Set MyPriority to Priority
- Initialize sensor pins as inputs
- Configure ADC for auto-scan on specified channels
- Read initial ADC values
- Post ES_INIT event to service
- Return true if initialization succeeds, false otherwise
- PostTapeService(ES_Event_t ThisEvent):
- Post ThisEvent to service queue
- Return true if successful, false otherwise
- RunTapeService(ES_Event_t ThisEvent):
- Handle various event types:
- ES_FORWARD_FULL, ES_FORWARD_HALF, ES_REVERSE, ES_STOP:
- Set alignFlag based on event type
- ES_NEW_KEY:
- Check event parameter:
- ' ': Start forward full alignment test
- ',': Start forward half alignment test
- '.': Start reverse alignment test
- '1': Print current tape sensor values
- ES_TIMEOUT:
- If timer event, stop alignment
- Return ES_NO_EVENT
Private Functions:
- Check4TapeSensor()
Description: Checks tape sensor values and adjusts vehicle alignment based on current state and alignment flag.
- Initialize local variables: counter, returnVal, adjustRightCounter, adjustLeftCounter
- Read current tape sensor values and store in currentTapeSensor array.
- Query state of main service top-level HSM and store in leaderState.
- If alignFlag is set:
- If leaderState is TREE:
- Query state of tree service HSM and store in treeState.
- If treeState is FORWARD:
- Check middle tape sensor value:
- If exceeds threshold, adjust alignment and increment adjustment counters.
- If treeState is GRABING:
- Check middle tape sensor value and adjust alignment accordingly.
- If leaderState is not TREE:
- Adjust alignment based on directionFlag and tape sensor values, incrementing adjustment counters.
- If alignFlag is not set, clear it.
- Store current tape sensor values as LastTapeSensors array.
- Return value of returnVal indicating whether alignment adjustment was performed.
Pseudo Code for ButtonCheckers
Pseudo Code for ButtonCheckers
Module: ButtonCheckers
Description:
- This module checks the status of buttons and posts corresponding events based on button state changes.
Functions:
1. Check4OnOffButton():
- Description: Checks the on/off button status and posts ES_BUTTON1 event when the button is pressed.
- Parameters: None
- Returns: True if an event is posted, otherwise false
- Local Variables:
- ReturnVal: boolean variable to indicate if an event is posted
- bottonFlag: static boolean variable to track button press state
- CurrentButtonState: uint8_t variable to store the current state of the on/off button
- lastOnOffStatus: static boolean variable to store the last state of the on/off button
- Execution:
- Read the current state of the on/off button.
- If the current state is different from the last state:
- If the current state is high (pressed) and the button flag is low:
- Post ES_BUTTON1 event indicating the on/off button is pressed.
- Set the button flag to indicate button press.
- Update the last button state.
- Return the value of ReturnVal.
2. Check4SuddenDeathButton():
- Description: Checks the sudden death button status and posts ES_BUTTON2 event when the button is pressed.
- Parameters: None
- Returns: True if an event is posted, otherwise false
- Local Variables:
- ReturnVal: boolean variable to indicate if an event is posted
- CurrentButtonState: uint8_t variable to store the current state of the sudden death button
- lastSuddenDeathStatus: static boolean variable to store the last state of the sudden death button
- Execution:
- Read the current state of the sudden death button.
- If the current state is different from the last state:
- If the current state is high (pressed):
- Post ES_BUTTON2 event indicating the sudden death button is pressed.
- Update the last button state.
- Return the value of ReturnVal.
Pseudo Code for LimitSwitchChecker for Tree Detection
Pseudo Code for LimitSwitchChecker for Tree Detection
Module: LimitSwitchCheckers
Description:
- This module checks the status of a limit switch and responds to specific events related to the limit switch status.
Functions:
1. InitLimitSwitchService(uint8_t Priority):
- Description: Initializes the LimitSwitchService and sets up the necessary configurations.
- Parameters: Priority - the priority of this service
- Returns: True if initialization is successful, otherwise false
2. PostLimitSwitchService(ES_Event_t ThisEvent):
- Description: Posts an event to the LimitSwitchService's queue.
- Parameters: ThisEvent - the event to post to the queue
- Returns: True if the enqueue operation is successful, otherwise false
3. RunLimitSwitchService(ES_Event_t ThisEvent):
- Description: Processes the received event and takes appropriate actions based on the event type.
- Parameters: ThisEvent - the event to process
- Returns: ES_Event_t - ES_NO_EVENT if no errors, otherwise ES_ERROR
4. CheckLimitSwitch():
- Description: Checks the status of the limit switch and takes actions if a change in status is detected.
- Returns: True if a change in limit switch status is detected, otherwise false
Module Variables:
- MyPriority: uint8_t variable to store the priority of this service
- lastLimitSwitch: int variable to store the previous state of the limit switch
- checkTreeFlag: bool variable to indicate whether to check for the tree state
Pseudo Code for LimitSwitchChecker and Acorn Distance Checker for Pushing System
Pseudo Code for LimitSwitchChecker and Acorn Distance Checker for Pushing System
Module: DistanceSensorService
Description:
- This module checks the status of distance sensors and responds to specific events related to the sensor readings.
Module Variables:
- MyPriority: uint8_t variable to store the priority of this service
- lastAcornSensor1, lastAcornSensor2: int variables to store the previous state of the acorn sensors
- lastLimitSwitch: int variable to store the previous state of the limit switch
Functions: InitDistanceService(uint8_t Priority):
- Description: Initializes the DistanceSensorService and sets up the necessary configurations.
- Parameters: Priority - the priority of this service
- Returns: True if initialization is successful, otherwise false
Function: CheckAcornInBay():
- Declare local variables:
- CurrentAcornDistance1: int
- CurrentAcornDistance2: int
- Read the current values of the left and right acorn distance sensors.
- If either CurrentAcornDistance1 or CurrentAcornDistance2 is different from lastAcornSensor1 or lastAcornSensor2 respectively:
- If either sensor detects an acorn in the bay:
- Print "Acorn in bay" message.
- Create an event indicating an acorn is detected in the bay.
- EventType: ES_ACORN_IN
- Post the event to the SPI Service FSM.
- Update lastAcornSensor1 and lastAcornSensor2 with the current sensor values.
- Return false.
Function: CheckLimitSwitch():
- Declare local variables:
- CurrentLimitSwitch: int
- currentTime: uint16_t
- Read the current value of the limit switch.
- If CurrentLimitSwitch is different from lastLimitSwitch:
- Get the current time.
- If the time difference between the current time and lastTime is greater than or equal to 50 milliseconds:
- If the limit switch is triggered:
- Set PUSH_MOTOR to 0.
- Update lastTime with the current time.
- Update lastLimitSwitch with the current switch value.
- Return false.
Page updated
Report abuse