Software Specification

Andrew Sack, Afshan Chandani, Ryan Brandt

Conventions
Events
States

RobotTopHSM (HSM) - PIC1

Events
ROBOT_INIT_COMPLETE
START_BUTTON_PRESSED
ES_TIMEOUT
States
RobotInitState
RobotInactiveState
RobotActiveState

PlayGameHSM (HSM) - PIC1

Events
GAME_STARTUP_COMPLETE
GAME_CYCLE_COMPLETE
REFILL_BUTTON_PRESSED
START_BUTTON_PRESSED
REHOME_DONE
States
GameStartupState
GameCycleState
GameRefillState
GameRehomeState

StartupHSM (HSM) - PIC1

Events
GAME_STARTUP_COMPLETE
STARTUP_STEP_COMPLETE
MOTORS_STOPPED
DRIVE_GOAL_REACHED
BUMP_OCCURRED
States
StartupInitState
FindBeaconState
DetermineTeamState
RotateToSideState
 DriveToWallState
MoveFromWallState
RotateToForwardState

CycleShootHSM
Events
ES_TIMEOUT
States
CycleShootEntry
CycleShootTension
CycleShootFire
CycleShootRelease
CycleShootSecure

RehomeHSM (HSM) - PIC1

Events
ES_TIMEOUT
DRIVE_GOAL_REACHED
BUMP_OCCURRED
States
RehomeInitState
RehomeRotateToSideState
RehomeDriveToWallState
RehomeMoveFromWallState
RehomeRotateToForwardState

CycleHSM (HSM) - PIC1

Events
BEACON_FOUND
RELOAD_COMPLETE
SHOOT_COMPLETE
DRIVE_GOAL_REACHED
ES_TIMEOUT
BEACON_ACKNOWLEDGED
BUMP_OCCURRED
MOTORS_STOPPED
States
CycleDriveForwardState
CycleAimState
CycleReloadState
CycleShootState
CycleUndoRotationState
CycleDriveBackState
 CycleStoppingState

Find_Beacon (Flat State Machine) - PIC1

Events
GIVE_UP
BEACON_FOUND
FIND_BEACON
States
BeaconWaitingState
BeaconSearchingState
Pseudocode
bool InitFind_Beacon(uint8_t Priority)
bool PostFind_Beacon(ES_Event_t ThisEvent)
ES_Event_t RunFind_Beacon(ES_Event_t ThisEvent)
Find_BeaconState_t QueryFind_Beacon(void)
TeamIdentity_t QueryTeamIdentity(void)
void __ISR(_TIMER_2_VECTOR, IPL6SOFT) Timer2IntHandler(void)
void __ISR(_INPUT_CAPTURE_4_VECTOR, IPL7SOFT)
MeasureTimingIntHandler(void)
static bool SetupTimer2(void)
static bool SetupIC4(void)
static void EnableIC4Interrupts(void)
static void DisableIC4Interrupts(void)

LaunchService (Service) - PIC1

Events
FLAG_UP
FLAG_DOWN
RELOAD_OUT
RELOAD_IN
LATCH_ENGAGE
LATCH_RELEASE
TENSION_ENGAGE
TENSION_RELEASE
SERVO_RESET
Pseudocode
bool InitLaunchService(uint8_t Priority)
bool PostLaunchService(ES_Event_t ThisEvent)
ES_Event_t RunLaunchService(ES_Event_t ThisEvent)

StartButton (FSM) - PIC1

Events
START_BUTTON_CHANGE
 States
StartButtonHigh
StartButtonLow
Pseudocode
bool InitStartButton(uint8_t Priority)
bool PostStartButton(ES_Event_t ThisEvent)
ES_Event_t RunStartButton(ES_Event_t ThisEvent)
bool CheckStartButtonEvents(void)

ReloadButton (FSM) - PIC1

Events
RELOAD_BUTTON_CHANGE
States
ReloadButtonHigh
ReloadButtonLow
Pseudocode
bool InitReloadButton(uint8_t Priority)
bool PostReloadButton(ES_Event_t ThisEvent)
ES_Event_t RunReloadButton(ES_Event_t ThisEvent)
bool CheckReloadButtonEvents(void)

SPILeaderSM (Service) - PIC1

Events
SEND_SPI_COMMAND
SPI_RESPONSE_RECEIVED
SPI_TASK_COMPLETE
SPI_TASK_FAILED
SPI_RESET

SPIFollowerSM (FSM) - PIC2

Events
SPI_COMMAND_RECEIVED
SPI_TASK_COMPLETE
SPI_TASK_FAILED
SPI_RESET
States
SPIFollowerInitState
SPIFollowerReceiveState
Pseudocode
bool InitSPIFollowerSM(uint8_t Priority)
bool PostSPIFollowerSM(ES_Event_t ThisEvent)
ES_Event_t RunSPIFollowerSM(ES_Event_t ThisEvent)
SPIFollowerSMState_t QuerySPIFollowerSM(void)
 bool InitializeSPI(void)
bool CheckSPIRBF(void)
void DecodeSPICommand(SPI_MOSI_Command_t SPICommand)

DriveTrain (FSM) - PIC2

Events
DRIVE_STOP
DRIVE_DISTANCE
DRIVE_UNTIL_BUMP
DRIVE_BEACON_SWEEP
DRIVE_UNDO_ROTATE
DRIVE_GOAL_REACHED
States
DriveInitState
DriveStoppedState
DriveDistanceState
DriveUntilBumpState
DriveClockwiseSweepState
DriveOverRotateState
DriveBeaconWaitState
DriveUndoRotateState
Pseudocode
bool InitDriveTrain(uint8_t Priority)
bool PostDriveTrain(ES_Event_t ThisEvent)
ES_Event_t RunDriveTrain(ES_Event_t ThisEvent)
DriveTrainState_t QueryDriveTrain(void)

MotorControl (Driver) - PIC2

Pseudocode
bool InitMotorControlDriver(void);
void MotorControl_SetMotorDutyCycle(MotorControl_Motor_t WhichMotor,
MotorControl_Direction_t WhichDirection, uint16_t DutyCycle);
void MotorControl_StopMotors(void);
void MotorControl_EnableClosedLoop(void);
void MotorControl_DisableClosedLoop(void);
void MotorControl_SetMotorSpeed(MotorControl_Motor_t WhichMotor,
MotorControl_Direction_t WhichDirection, uint16_t Speed);
void MotorControl_ResetTickCount(MotorControl_Motor_t WhichMotor);
void MotorControl_SetTickGoal(MotorControl_Motor_t WhichMotor, uint32_t NumTicks);
Encoder_t MotorControl_GetEncoder(MotorControl_Motor_t WhichMotor);
ControlState_t MotorControl_GetControlState(MotorControl_Motor_t WhichMotor);
void MotorControl_DriveStraight(MotorControl_Direction_t WhichDirection, uint16_t
Speed, uint16_t DistanceCM);
 void MotorControl_DriveTurn(MotorControl_Turn_t WhichTurn, uint16_t Speed, uint16_t
AngleDeg);
static void InitPWMTimer(void)
static void InitLeftMotor(void)
static void InitRightMotor(void)
static void InitInputCapture(void)
static void InitLeftEncoder(void)
static void InitRightEncoder(void)
static void InitControlLaw(void)
void __ISR(_TIMER_2_VECTOR, IPL6SOFT) Timer2Handler(void)
void __ISR(_INPUT_CAPTURE_1_VECTOR, IPL7SOFT) LeftEncoderHandler(void)
void __ISR(_INPUT_CAPTURE_2_VECTOR, IPL7SOFT) RightEncoderHandler(void)
void __ISR(_TIMER_4_VECTOR, IPL4SOFT) ControlLawHandler(void)
void UpdateControlLaw(ControlState_t *ThisControl, Encoder_t *ThisEncoder)

BumperService (FSM) - PIC2

Events
DRIVE_STOP
DRIVE_UNTIL_BUMP
BUMP_FOUND
States
BumperIdleState
BumperActiveState
Pseudocode
bool InitBumperService(uint8_t Priority)
bool PostBumperService(ES_Event_t ThisEvent)
ES_Event_t RunBumperService(ES_Event_t ThisEvent)
BumperState_t QueryBumperService(void)
bool Check4Bump(void)

Conventions

Events
All events should be in UPPER_CASE_SNAKE_CASE and be clear which service they apply to.
Ideally, the service name (or part of it) should be included in the event name unless event
meaning is unambiguous without it.

Framework events (ES_ENTRY, ES_NEW_KEY, etc) should be used as written in the template
and no other events should begin with ES
Examples:
DRIVE_MOVE_FORWARD - DRIVE is short for drive train
START_BUTTON_PRESSED - No service name, but it is clear that this event occurs when the
physical start button is pressed
CYCLE_COMPLETE - CYCLE refers to CYCLE state machine

States
States should be in CamelCase and unique to their service, preferably including the service
name (or part of it) and the word State, especially if there are similarly named events. There
cannot be multiple enums with the same state name or there will be an error (Do not use
InitPState. Rename it).

Example:
RobotInitState - State in RobotTopHSM

RobotTopHSM (HSM) - PIC1

The top level module for the HSM that handles all game logic

Events

ROBOT_INIT_COMPLETE
Event to trigger transition to RobotInactiveState after hardware initialization complete

START_BUTTON_PRESSED
Posted by Start Button Event Checker when it is pressed

ES_TIMEOUT
Timeouts occur 1) when the start button is pressed to allow the user to move their finger out of
the way before movement starts and 2) to allow the servos to move back to initial positions in a
sequence after a game has ended (since the game might end in the middle of a shooting cycle
and we need to move these servos in a particular order)

GAME_TIMEOUT
This game timeout occurs after 2 minutes 18 seconds have elapsed in a game
States

RobotInitState
Services and Hardware are in the process of being reset to their initial status:
Initialize all hardware
Untension spring and
reset reloader (hold state) and
lower game status flag
Start 1 second timer (just to
give the user enough time to
get out of the way)

RobotInactiveState
Initialization complete. All lower level state machines suspended and nothing is moving

RobotActiveState
Main state for when robot is in an active game

PlayGameHSM (HSM) - PIC1

HSM that handles robot logic when in an active game

Events

GAME_STARTUP_COMPLETE
Posted when all startup motions are finished and cycling is ready to begin

GAME_CYCLE_COMPLETE
Robot returned to reload zone and ready to be refilled

REFILL_BUTTON_PRESSED
Refill button pressed. Tells robot to begin new cycle

START_BUTTON_PRESSED
Start button pressed. Tells robot to begin new rehoming cycle

REHOME_DONE
Rehoming cycle is done and we return back to the GameRefillState
States

GameStartupState
Robot is performing initial find beacon and motion to move to cycling position

GameCycleState
Robot is in a shooting cycle

GameRefillState
Robot is idle in reload zone waiting for human to load balls and press reload button

GameRehomeState
Run RehomeHSM

StartupHSM (HSM) - PIC1

HSM that handles robot logic while performing game startup sequence

Events

GAME_STARTUP_COMPLETE
Posted when all startup motions are finished and cycling is ready to begin

STARTUP_STEP_COMPLETE
Tells robot to move to next step of startup sequence

MOTORS_STOPPED
Tells robot to move to next step of startup sequence

DRIVE_GOAL_REACHED
Tells robot to move to next step of startup sequence

BUMP_OCCURRED
Tells robot to move to next step of startup sequence
States

StartupInitState
Enable Game Status indicator and start 2:18 timer

FindBeaconState
Robot rotating until it sees beacon

DetermineTeamState
Robot measuring beacon frequency and setting team indicator

RotateToSideState
Robot performing 1st CCW rotation to move to 5pt starting position

DriveToWallState
Robot driving straight until it reaches side wall to move to 5pt starting position

MoveFromWallState
Robot moving forward slightly (3 cm) to prevent wall collision while upcoming rotation

RotateToForwardState
Robot performing 2nd CW rotation to face the 5pt starting position

CycleShootHSM
HSM that handles ordering and control of servo commands to perform a shot

Events

ES_TIMEOUT
This is a timeout for each step. The steps must be executed sequentially so this allows for the
state machine to progress without implementing blocking code
States

CycleShootEntry
This is the entry state. We just ensure the latch servo is closed here so that tensioning will be
successful

CycleShootTension
Tension the springs

CycleShootFire
Open the latch to fire

CycleShootRelease
Release tension on the springs to allow the catapult arm to fall back into resting position

CycleShootSecure
Close the latch servo to secure the catapult arm

RehomeHSM (HSM) - PIC1

HSM that handles a rehoming cycle, which repositions the bot next to the leftmost wall to
counteract drift over multiple cycles.

Events

ES_TIMEOUT
After the start button is pressed to initiate a rehoming procedure, a one second timer is set to
allow the user to move their finger out of the way before the bot starts moving. This is the
timeout event for that one second timer, allowing the state machine to progress.

DRIVE_GOAL_REACHED
This event gets posted when the bot is finished rotating a certain amount (in this case, 90
degrees)
BUMP_OCCURRED
This event gets posted when the bot, after having driven backwards, either hits the wall with its
bumper sensor (limit switch) or a 4 second timeout has occurred (just in case it hits the wall off
center and would otherwise get stuck).

States

RehomeInitState
Waiting for 1 second timeout to occur before movement starts

RehomeRotateToSideState
Rotating 90 degrees clockwise

RehomeDriveToWallState
Drive backwards until the bump sensor (or timeout) is activated (a bump occurs)

RehomeMoveFromWallState
Move forward 3 cm to prevent touching the wall while rotating

RehomeRotateToForwardState
Rotate 90 degrees counterclockwise to face the front again

CycleHSM (HSM) - PIC1

HSM that handles robot logic while performing a shooting cycle

Events

BEACON_FOUND
Posted when the BeaconSM has detected the beacon

RELOAD_COMPLETE
Posted when reloading is complete
SHOOT_COMPLETE
Posted when a shot is completed

DRIVE_GOAL_REACHED
Posted when 1) the 45 cm forward distance has been traversed when the bot is initially moving
forward into the 5 point scoring region, 2) if the beacon was not found after a full 360 degree
sweep, and 3) when undoing an alignment rotation is completed after shooting

ES_TIMEOUT
This timeout corresponds to a short delay between detecting the beacon and telling the motors
to stop. This allows us to aim closer to the center of the goal instead of at the edge

BEACON_ACKNOWLEDGED
This event is an acknowledgement from the Drive PIC that it has stored the rotation amount
corresponding to the beacon location. After shooting is completed, it will anticipate undoing this
rotation to return to a forward facing position so that we can back up into the reload zone again

BUMP_OCCURRED
This event occurs when the bumper sensor (limit switch) is triggered, which occurs when we
drive backwards to return to the reload zone

MOTORS_STOPPED
This is an acknowledgement from the Drive PIC that the motors have stopped, which occurs
after the bot has reached the back wall (and hence is in the reload zone)

States

CycleDriveForwardState
Robot driving forward until it is fully in 5pt zone

CycleAimState
Robot rotating until it sees beacon to fine tune aiming

CycleReloadState
Loading ball:
Reload Servo Forward (start timer)
(upon timeout) Reload Servo Backward
CycleShootState
Run CycleShootHSM

CycleUndoRotationState
Send undo rotation command to Drive PIC

CycleDriveBackState
Robot reversing until it enters reload zone

CycleStoppingState
Send stop command to Drive PIC; change event to GAME_CYCLE_COMPLETE when stop is
acknowledged

Find_Beacon (Flat State Machine) - PIC1

This state machine searches for the beacon, determines the team identity, and informs the
TopRobotHSM of when it is aligned with the beacon during fine-tuning for shot aim

Events

GIVE_UP
Tells the Find_Beacon state machine to give up (this would allow an external state machine or
service to force it to stop even if it hasn’t detected a beacon yet, but ended up being unused)

BEACON_FOUND
Posted by the input capture interrupt service routine in the event that the beacon was found

FIND_BEACON
Posted by an external state machine or service to initiate searching for the beacon

States

BeaconWaitingState
Doing nothing
BeaconSearchingState
Actively searching for the beacon (interrupts are enabled, input capture is on)

Pseudocode
/****************************************************************************
Function
InitFind_Beacon

Parameters
uint8_t : the priorty of this service

Returns
bool, false if error in initialization, true otherwise

Description
Saves away the priority, sets up the initial transition and does any
other required initialization for this state machine
Notes

Author
Ryan Brandt
****************************************************************************/

bool InitFind_Beacon(uint8_t Priority)

Set current state to BeaconWaitingState

Do lots of setup:
Setup the timer 2 for input capture 1: SetupTimer2()

Setup the input capture 1 module: SetupIC4()

Set up and enable interrupts using the following steps:

Ensure multi-vector mode is enabled (INTCONbits.MVEC = 1)
Ensure global interrupts are on (__builtin_enable_interrupts())

Turn on timer 2 to get the ball rolling (T2CONbits.ON = 1)

Turn on input capture 4 (IC4CONbits.ON = 1)

return true
/****************************************************************************
Function
PostFind_Beacon

Parameters
EF_Event_t ThisEvent , the event to post to the queue

Returns
boolean False if the Enqueue operation failed, True otherwise

Description
Posts an event to this state machine's queue
Notes

Author
J. Edward Carryer, 10/23/11, 19:25
****************************************************************************/

bool PostFind_Beacon(ES_Event_t ThisEvent)

return ES_PostToService(MyPriority, ThisEvent);

/****************************************************************************
Function
RunFind_Beacon

Parameters
ES_Event_t : the event to process

Returns
ES_Event_t, ES_NO_EVENT if no error ES_ERROR otherwise

Description
Implements the state machine for finding the beacon
Notes
uses nested switch/case to implement the machine.
Author
Ryan Brandt
****************************************************************************/

ES_Event_t RunFind_Beacon(ES_Event_t ThisEvent)

If current state is BeaconWaitingState

 If ThisEvent has type FIND_BEACON:
Set Found flag equal to 0 (beacon hasn’t been found yet)

If ThisEvent has parameter DetermineTeam (done at the beginning of the game)

Set SearchMode equal to DetermineTeam
Output "Determining team identity” to terminal
Set TeamIdentity to Unknown
else If ThisEvent has parameter FindKnownFrequency
Set SearchMode equal to FindKnownFrequency;

if TeamIdentity is Red
Output “Searching for Team Red's Beacon” to terminal
else if TeamIdentity is Blue
Output “Searching for Team Blue's Beacon” to terminal
else
Output “Cannot search yet - need to determine team identity first” to terminal
break

Output "Finding beacon - start rotation" to terminal

Set FirstMeasurementFlag equal to 1 (take two measurements to get first period)

Enable IC4 interrupts

Set CurrentState equal to BeaconSearchingState

break

If current state is BeaconSearchingState

If ThisEvent has type BEACON_FOUND:
Post BEACON_FOUND event to RobotTopHSM

Send “Beacon Found” success message to terminal

Disable IC4 interrupts

Set FirstMeasurementFlag equal to 1 (for next time - redundant)
Set Found equal to 0 (for next time - redundant)

Set current state to BeaconWaitingState

break

If ThisEvent has type GIVE_UP:

Send “Did not find beacon” failure message to terminal

Disable IC4 interrupts

 Set FirstMeasurementFlag equal to 1 (for next time - redundant)
Set Found equal to 0 (for next time - redundant)

Set current state to BeaconWaitingState

break
break

return ES_NO_EVENT (so the framework is happy)

/****************************************************************************
Function
QueryFind_Beacon

Parameters
None

Returns
Find_BeaconState_t The current state of the Template state machine

Description
returns the current state of the Template state machine
Notes

Author
J. Edward Carryer, 10/23/11, 19:21
****************************************************************************/

Find_BeaconState_t QueryFind_Beacon(void)
return CurrentState

/****************************************************************************
Function
QueryTeamIdentity

Parameters
None

Returns
TeamIdentity_t TeamIdentity

Description
returns the current team identity
Notes

Author
Ryan Brandt
****************************************************************************/

TeamIdentity_t QueryTeamIdentity(void)
return TeamIdentity

void __ISR(_TIMER_2_VECTOR, IPL6SOFT) Timer2IntHandler(void)

Disable global interrupts (__builtin_disable_interrupts() or EnterCritical())
If Timer 2’s rollover interrupt is active (IFS0bits.T2IF is 1)
Increment RolloverCounter
Clear the rollover interrupt (IFS0CLR = _IFS0_T2IF_MASK)
Enable global interrupts again (__builtin_enable_interrupts() or ExitCritical())

void __ISR(_INPUT_CAPTURE_4_VECTOR, IPL7SOFT)

MeasureTimingIntHandler(void)
Read input capture 4 buffer into local variable uint32_t CapturedTime
Clear the pending capture interrupt (IFS0CLR = _IFS0_IC4IF_MASK)

If Timer 2?s rollover interrupt is active (IFS0bits.T2IF is 1) and the CapturedTime is after
rollover (<0x8000):
Increment RolloverCounter
Clear the rollover interrupt (IFS0CLR = _IFS0_T2IF_MASK)
Set CapturedTime += (RolloverCounter << 16) (aka lower 16 bits are from the buffer
and upper 16 bits are from the rollover buffer)

If FirstMeasurementFlag is true, this is the first measurement, so collect it, disable the
flag, and get out:
Save CapturedTime into LastRiseTime
Disable the flag (set FirstMeasurementFlag to false)
Else, this is not the first measurement, so we can measure a pulse period and report a
duration:
Calculate the last pulsePeriod as CapturedTime - LastRiseTime (pulsePeriod is a
volatile uint32_t module level variable)

if SearchMode is DetermineTeam
If pulsePeriod is in an acceptable range for the red team; aka:
 if (((pulsePeriod < REDTEAM_PULSEMAX) && (pulsePeriod >
REDTEAM_PULSEMIN)) && (!Found))

Set TeamIdentity to Red

Set PulseMin equal to REDTEAM_PULSEMIN
Set PulseMax equal to REDTEAM_PULSEMAX

Post EncoderPulse event to service

Set Found to true

If pulsePeriod is in an acceptable range for the blue team; aka:

else if (((pulsePeriod < BLUETEAM_PULSEMAX) && (pulsePeriod >
BLUETEAM_PULSEMIN)) && (!Found))

Set TeamIdentity to Blue

Set PulseMin equal to BLUETEAM_PULSEMIN
Set PulseMax equal to BLUETEAM_PULSEMAX
Post EncoderPulse event to service
Set Found to true

else if SearchMode is FindKnownFrequency

If pulsePeriod is in an acceptable range; aka:
if (((pulsePeriod < PulseMax) && (pulsePeriod > PulseMin)) && (!Found)) {
Post EncoderPulse event to this state machine
Output pulse period
Set Found to true

Save CapturedTime into LastRiseTime

static bool SetupTimer2(void)

Setup timer 2 for use in input capture for beacon detection:

Set up timer 2 using the following steps:

Disable the timer (T2CONbits.ON = 0)
Select the internal PBCLK source (T2CONbits.TCS = 0)
Disable gated time accumulation (T2CONbits.TGATE = 0)
Set the input clock prescale to 4 (T2CONbits.TCKPS = 0b010)
Set the period register to the max value for 16 bits (PR2 = 0xFFFF)
Clear the timer (TMR2 = 0)
Clear the T2IF interrupt flag bit (IFS0CLR = _IFS0_T2IF_MASK)
Set the timer's interrupt priority level to 6 (IPC2bits.T2IP = 6)
Set the local interrupt enable for the timer (IEC0SET = _IEC0_T2IE_MASK)
 return true

static bool SetupIC4(void)

Setup input capture 4 module for beacon detection:

Configure pin RB4 as a digital input (PortSetup_ConfigureDigitalInputs(_Port_B,

_Pin_4)) for input capture

Disable the Input Capture module (IC4CONbits.ON = 0)

Map RPB4 to the input capture 4 input (IC4R = 0b0010)
Use a 16-bit timer capture (IC4CONbits.C32 = 0)
Select timer 2 for the input capture (IC4CONbits.ICTMR = 1)
Make interrupts occur on every capture event (IC4CONbits.ICI = 0b00)
Set up input capture mode to simple capture event mode every rising edge
(IC4CONbits.ICM = 0b011)
Clear the input capture interrupt flag bit (IFS0CLR = _IFS0_IC4IF_MASK)
Set the input capture?s interrupt priority level to 7 (IPC4bits.IC4IP = 7)

DisableIC4Interrupts()

return true

static void EnableIC4Interrupts(void)

Set the local interrupt enable for the input capture module (IEC0SET =
_IEC0_IC4IE_MASK)
Clear possible pending interrupt flag (IFS0CLR = _IFS0_IC4IF_MASK)
Turn IC4 module on (IC4CONbits.ON = 1)

static void DisableIC4Interrupts(void)

Clear the local interrupt enable for the input capture module (IEC0CLR =
_IEC0_IC4IE_MASK)
Clear possible pending interrupt flag (IFS0CLR = _IFS0_IC4IF_MASK)
Turn IC4 module off (IC4CONbits.ON = 0)
}
LaunchService (Service) - PIC1
A service that sends commands to the servos

Events

FLAG_UP
Sets the game indicator flag to the up position

FLAG_DOWN
Sets the game indicator flag to the down position

RELOAD_OUT
Sets the reloader to the out position

RELOAD_IN
Sets the reloader to the in position

LATCH_ENGAGE
Sets the catapult latch to the engaged position

LATCH_RELEASE
Sets the catapult latch to the released position

TENSION_ENGAGE
Sets the catapult tensioner to the engaged position

TENSION_RELEASE
Sets the catapult tensioner to the released position

SERVO_RESET
Resets servo positions
Pseudocode

/****************************************************************************
Function
InitLaunchService

Parameters
uint8_t : the priorty of this service

Returns
bool, false if error in initialization, true otherwise

Description
Saves away the priority, sets up the initial transition and does any
other required initialization for this state machine
Notes

Author
Afshan Chandani
****************************************************************************/

bool InitLaunchService(uint8_t Priority)

Configure Servo ports

Configure PWM Timer interrupts
Configure 4 Output Compares
Enable interrupts

/****************************************************************************
Function
PostLaunchService

Parameters
ES_Event_t ThisEvent , the event to post to the queue

Returns
boolean False if the Enqueue operation failed, True otherwise

Description
Posts an event to this state machine's queue
Notes
 Author
Afshan Chandani
****************************************************************************/

bool PostLaunchService(ES_Event_t ThisEvent)

return ES_PostToService(MyPriority, ThisEvent)

/****************************************************************************
Function
RunLaunchService

Parameters
ES_Event_t : the event to process

Returns
ES_Event_t, ES_NO_EVENT if no error ES_ERROR otherwise

Description

Notes
uses nested switch/case to implement the machine.
Author
Afshan Chandani
****************************************************************************/

ES_Event_t RunLaunchService(ES_Event_t ThisEvent)

Switch on Event

In case of each event, set the corresponding output compare module to the duty cycle in the
define section

OC1 - Reloader
OC3 - Tensioner
OC4 - Flag
OC5 - Latch

StartButton (FSM) - PIC1

A service that handles the start button and implements debouncing
Events

START_BUTTON_CHANGE
The start button has changed states

States

StartButtonHigh
The button is in a high state

StartButtonLow
The button is in a low state

Pseudocode

/****************************************************************************
Function
InitStartButton

Parameters
uint8_t : the priorty of this service

Returns
bool, false if error in initialization, true otherwise

Description
Saves away the priority, sets up the initial transition and does any
other required initialization for this state machine
Notes

Author
Afshan Chandani
****************************************************************************/

bool InitStartButton(uint8_t Priority)

Configure Digital Input

Set LastButtonState to the current pin value
Set CurrentButtonState to LastButtonState
Set CurrentState to StartButtonHigh

/****************************************************************************
Function
PostStartButton

Parameters
ES_Event_t ThisEvent , the event to post to the queue

Returns
boolean False if the Enqueue operation failed, True otherwise

Description
Posts an event to this state machine's queue
Notes

Author
Afshan Chandani
****************************************************************************/

bool PostStartButton(ES_Event_t ThisEvent)

return ES_PostToService(MyPriority, ThisEvent)

/****************************************************************************
Function
RunStartButton

Parameters
ES_Event_t : the event to process

Returns
ES_Event_t, ES_NO_EVENT if no error ES_ERROR otherwise

Description

Notes
uses nested switch/case to implement the machine.
Author
Afshan Chandani
****************************************************************************/
ES_Event_t RunStartButton(ES_Event_t ThisEvent)

Switch on CurrentState
case StartButtonHigh:
if EventType is START_BUTTON_CHANGE
Start StartButtonTimer
if EventType is ES_TIMEOUT - debounce is satisfied
if CurrentButtonState is low
CurrentState = StartButtonLow
Post START_BUTTON_PRESSED to Top HSM

case StartButtonLow:
if EventType is START_BUTTON_CHANGE
Start StartButtonTimer
if EventType is ES_TIMEOUT
if CurrentButtonState is high
CurrentState = StartButtonHigh

/****************************************************************************
Function
CheckStartButtonEvents

Parameters
None

Returns
True if start button has changed states

Description
Check if the start button has changed states
Notes

Author
Afshan Chandani
****************************************************************************/

bool CheckStartButtonEvents(void)

CurrentButtonState = Port value

if CurrentButtonState is not LastButtonState
Post START_BUTTON_CHANGE event
LastButtonState = CurrentButtonState
return true
ReloadButton (FSM) - PIC1
A service that handles the Reload button and implements debouncing

Events

RELOAD_BUTTON_CHANGE
The Reload button has changed states

States

ReloadButtonHigh
The button is in a high state

ReloadButtonLow
The button is in a low state

Pseudocode

/****************************************************************************
Function
InitReloadButton

Parameters
uint8_t : the priorty of this service

Returns
bool, false if error in initialization, true otherwise

Description
Saves away the priority, sets up the initial transition and does any
other required initialization for this state machine
Notes

Author
Afshan Chandani
****************************************************************************/
bool InitReloadButton(uint8_t Priority)

Configure Digital Input

Set LastButtonState to the current pin value
Set CurrentButtonState to LastButtonState
Set CurrentState to ReloadButtonHigh

/****************************************************************************
Function
PostReloadButton

Parameters
ES_Event_t ThisEvent , the event to post to the queue

Returns
boolean False if the Enqueue operation failed, True otherwise

Description
Posts an event to this state machine's queue
Notes

Author
Afshan Chandani
****************************************************************************/

bool PostReloadButton(ES_Event_t ThisEvent)

return ES_PostToService(MyPriority, ThisEvent)

/****************************************************************************
Function
RunReloadButton

Parameters
ES_Event_t : the event to process

Returns
ES_Event_t, ES_NO_EVENT if no error ES_ERROR otherwise

Description
 Notes
uses nested switch/case to implement the machine.
Author
Afshan Chandani
****************************************************************************/

ES_Event_t RunReloadButton(ES_Event_t ThisEvent)

Switch on CurrentState
case ReloadButtonHigh:
if EventType is RELOAD_BUTTON_CHANGE
Reload ReloadButtonTimer
if EventType is ES_TIMEOUT - debounce is satisfied
if CurrentButtonState is low
CurrentState = ReloadButtonLow
Post Reload_BUTTON_PRESSED to Top HSM

case ReloadButtonLow:
if EventType is RELOAD_BUTTON_CHANGE
Reload ReloadButtonTimer
if EventType is ES_TIMEOUT
if CurrentButtonState is high
CurrentState = ReloadButtonHigh

/****************************************************************************
Function
CheckReloadButtonEvents

Parameters
None

Returns
True if Reload button has changed states

Description
Check if the Reload button has changed states
Notes

Author
Afshan Chandani
****************************************************************************/
bool CheckReloadButtonEvents(void)

CurrentButtonState = Port value

if CurrentButtonState is not LastButtonState
Post RELOAD_BUTTON_CHANGE event
LastButtonState = CurrentButtonState
return true
SPILeaderSM (Service) - PIC1
A service that sends SPI commands to PIC2 and receives responses.

Events

SEND_SPI_COMMAND
Posted by other services to SPI Leader with a 16 bit param of what drive code to send to PIC2

SPI_RESPONSE_RECEIVED
Posted by SPI Event Checker to the SPI Leader to indicate that PIC2 has sent back data

SPI_TASK_COMPLETE
Posted by SPI Leader to HSM to indicate the current drive task has completed execution

SPI_TASK_FAILED
Posted by SPI Leader to HSM to indicate the current drive task has failed execution

SPI_RESET
Resets SPI Leader

SPIFollowerSM (FSM) - PIC2

A service that receives SPI commands from PIC1 and sends responses.

Events

SPI_COMMAND_RECEIVED
Posted by SPI Event Checker to the SPI Follower to indicate that PIC1 has sent data

SPI_TASK_COMPLETE
Posted by SM to SPI Follower to indicate the current drive task has completed execution

SPI_TASK_FAILED
Posted by SM to SPI Follower to indicate the current drive task has failed execution
SPI_RESET
Resets SPI Follower

States

SPIFollowerInitState
SPI is initialized

SPIFollowerReceiveState
SPI Follower waits for a command to be passed into it from PIC1, and then posts the correct
event to the SM

Pseudocode

/****************************************************************************
Function
InitSPIFollowerSM

Parameters
uint8_t : the priorty of this service

Returns
bool, false if error in initialization, true otherwise

Description
Saves away the priority, sets up the initial transition and does any
other required initialization for this state machine
Notes

Author
Andrew Sack
****************************************************************************/

bool InitSPIFollowerSM(uint8_t Priority)

Set CurrentState to SPIFollowerInitState

Init SendData to 0xFFFF
Post ES_INIT to this service
/****************************************************************************
Function
PostSPIFollowerSM

Parameters
ES_Event_t ThisEvent , the event to post to the queue

Returns
boolean False if the Enqueue operation failed, True otherwise

Description
Posts an event to this state machine's queue
Notes

Author
Andrew Sack
****************************************************************************/

bool PostSPIFollowerSM(ES_Event_t ThisEvent)

return ES_PostToService(MyPriority, ThisEvent)

/****************************************************************************
Function
RunSPIFollowerSM

Parameters
ES_Event_t : the event to process

Returns
ES_Event_t, ES_NO_EVENT if no error ES_ERROR otherwise

Description

Notes
uses nested switch/case to implement the machine.
Author
Andrew Sack
****************************************************************************/
ES_Event_t RunSPIFollowerSM(ES_Event_t ThisEvent)

Switch on CurrentState
case SPIFollowerInitState:
if EventType is ES_INIT
Call InitializeSPI
If it returns true
Set CurrentState to SPIFollowerReceiveState
Else return ES_ERROR

case SPIFollowerReceiveState:
If EventType is SPI_COMMAND_RECEIVED
call DecodeSPICommand() for the event

else if EventType is STOP_ACKNOWLEDGED

Write SPI_STOP_ACKNOWLEDGED to SPICommand struct name field
Write to SendData module var
else if EventType is DRIVE_GOAL_REACHED
Write SPI_DRIVE_GOAL_REACHED to SPICommand struct name field
Write to SendData module var
else if EventType is BUMP_FOUND
Write SPI_BUMP_SUCCESS to SPICommand struct name field
Write to SendData module var
else if EventType is TAPE_FOUND
Write SPI_TAPE_SUCCESS to SPICommand struct name field
Write to SendData module var
else if EventType is BEACON_ACKNOWLEDGED
Write SPI_BEACON_ACKNOWLEDGED to SPICommand struct name field
Write to SendData module var

/****************************************************************************
Function
QuerySPIFollowerSM

Parameters
None

Returns
SPIFollowerSMState_t The current state of the SPIFollowerSM state machine

Description
returns the current state of the Drive Train state machine
Notes
 Author
Andrew Sack
****************************************************************************/

SPIFollowerSMState_t QuerySPIFollowerSM(void)

return CurrentState

/****************************************************************************
Function
InitializeSPI

Parameters
None

Returns
True if successful, else false

Description
Initializes SPI module as follower
Notes

Author
Andrew Sack
****************************************************************************/

bool InitializeSPI(void)

//Use SPI HAL to configure SPI 1

BasicConfig SPI1
Set SPI1 as Follower
Set BitTime for SPI1 to 1000 ns (1 MHz)

Set SSEN to 0
Set SPIROV to 0

Configure RA0 as Digital Input

Set SS1R to 0b0000
Map SD Output to RPA1
Configure RB5 as Digital input
Set SDI1R to 0b0001
Configure RB14 as Digital Input
//Use SPI HAL to configure SPI 1
SetClockIdleState to idle hi
SetActiveEdge to 2nd edge
SetXferWidth to 16 bits
SetEnhancedBuffer to false
Enable SPI 1
Wait until SS1 Rises

return True if config is success. Else, false

/****************************************************************************
Function
CheckSPIRBF

Parameters
None

Returns
True if something read from buffer

Description
Check if something is in SPI buffer and if so, post an event
Notes

Author
Andrew Sack
****************************************************************************/

bool CheckSPIRBF(void)

if SPI1 SPIBRF is true

Write SendData to SPI Buff
Write SPI_STILL_WORKING to SendData
Post SPI_COMMAND_RECEIVED to this service with value of SPI1Buf as param
return true

/****************************************************************************
* Function
* DecodeSPICommand
*
* Parameters
* SPI_MOSI_Command_t SPICommand - Command struct to be decoded
* Return
 * void
* Description
* Takes 16bit val cast to a struct, decodes it and posts correct event to
* Framework
****************************************************************************/

void DecodeSPICommand(SPI_MOSI_Command_t SPICommand)

switch on SPICommand.Name
case SPI_POLL:
Do nothing

case SPI_STOP:
Post DRIVE_STOP event to DriveTrain and BumperService

case SPI_DRIVE_DISTANCE:
Post DRIVE_DISTANCE event to DriveTrain service

case SPI_DRIVE_UNTIL_BUMP:
Post DRIVE_UNITL_BUMP event to DriveTrain and BumperService

case SPI_DO_TAPE_ALIGN:
Post DRIVE_TAPE_ALIGN event to DriveTrain service

case SPI_DO_BEACON_SWEEP:
Post DRIVE_BEACON_SWEEP event to DriveTrain service

case SPI_BEACON_FOUND:
Post BEACON_FOUND event to DriveTrain service

case SPI_UNDO_ROTATE:
Post DRIVE_UNDO_ROTATE event to DriveTrain service

DriveTrain (FSM) - PIC2

The drive train module takes events from other services and translates them into MotorControl
function calls.It is responsible for all motion on the robot.
Events

DRIVE_STOP
In any state, the DriveTrain will stop the motors and move to DriveStoppedState

DRIVE_DISTANCE
Do a drive of the specified type for specified distance

DRIVE_UNTIL_BUMP
Drive backward until BUMP_FOUND event received

DRIVE_BEACON_SWEEP
Start performing beacon sweep

DRIVE_UNDO_ROTATE
Rotate back to stored orientation

DRIVE_GOAL_REACHED
Posted by DriveTrain when a distance goal is reached

States

DriveInitState
Initializing hardware and registers

DriveStoppedState
Initialization complete. Robot is not moving

DriveDistanceState
Robot is moving for a specified distance or indefinitely

DriveUntilBumpState
Robot is driving straight backward and waiting for a BUMP_FOUND event
DriveClockwiseSweepState
Robot turning clockwise and waiting for either DRIVE_GOAL_REACHED or BEACON_FOUND
event

DriveOverRotateState
Robot continuing turning by set amount after seeing beacon and waiting for
DRIVE_GOAL_REACHED event

DriveBeaconWaitState
Robot not moving and waiting for DRIVE_UNDO_ROTATE event

DriveUndoRotateState
Robot rotating for specified distance back to original angle

Pseudocode
/****************************************************************************
Function
InitDriveTrain

Parameters
uint8_t : the priority of this service

Returns
bool, false if error in initialization, true otherwise

Description
Saves away the priority, sets up the initial transition and does any
other required initialization for this state machine
Notes

Author
Andrew Sack
****************************************************************************/

bool InitDriveTrain(uint8_t Priority)

Set CurrentState to DriveInitState

Call InitMotorControlDriver()
Set SweepAmount to 0
Post ES_INIT event to this service
/****************************************************************************
Function
PostDriveTrain

Parameters
ES_Event_t ThisEvent , the event to post to the queue

Returns
boolean False if the Enqueue operation failed, True otherwise

Description
Posts an event to this state machine's queue
Notes

Author
Andrew Sack
****************************************************************************/

bool PostDriveTrain(ES_Event_t ThisEvent)

return ES_PostToService(MyPriority, ThisEvent)

/****************************************************************************
Function
RunDriveTrain

Parameters
ES_Event_t : the event to process

Returns
ES_Event_t, ES_NO_EVENT if no error ES_ERROR otherwise

Description

Notes
uses nested switch/case to implement the machine.
Author
Andrew Sack
****************************************************************************/
ES_Event_t RunDriveTrain(ES_Event_t ThisEvent)

if ThisEvent is DRIVE_STOP (Run in all states)

Stop Motors
Set CurrentState to DriveStoppedState
Post STOP_ACKNOWLEDGED to SPI Service

Cast EventParam to SPI_MOSI_Command_t called SPICommand

Switch on CurrentState
case DriveInitState:
If ES_INIT
Stop Motors
Set CurrentState to DriveStoppedState

case DriveStoppedState
Switch on ThisEvent.EventType
case DRIVE_DISTANCE:
if SPICommand.DriveType is Translation
call MotorControl_DriveStraight with params from SPICommand
else if SPICommand.DriveType is Rotation
call MotorControl_DriveTurn with params from SPICommand
Set CurrentState to DriveDistanceState

case DRIVE_UNTIL_BUMP:
call MotorControl_DriveStraight to drive backwards at medium speed indefinitely
Set CurrentState to DriveUntilBumpState

case DRIVE_BEACON_SWEEP:
call MotorControl_DriveTurn to turn CW by SWEEP_CW_ANGLE at low speed
Set CurrentState to DriveClockwiseSweepState

case DriveDistanceState:
If EventType is DRIVE_GOAL_REACHED
Stop Motors
Set CurrentState to DriveStoppedState
Post DRIVE_GOAL_REACHED to SPI

case DriveUntilBumpState:
If EventType is BUMP_FOUND
Stop Motors
Set CurrentState to DriveStoppedState

case DriveClockwiseSweepState:
 If EventType is BEACON_FOUND
Save Current Left Encoder TickCount to SweepAmount
Call DriveTurn Clockwise at low speed by Overrotate amount
Set CurrentState to DriveOverRotateState
else if EventType is DRIVE_GOAL_REACHED
Stop Motors
Post DRIVE_GOAL_REACHED to SPI Service
Set CurrentState to DriveStoppedState

case DriveOverRotateState:
if EventType is DRIVE_GOAL_REACHED
Stop Motors
Post BEACON_ACKNOWLEDGED to SPI Service
Set CurrentState to DriveBeaconWaitState

case DriveBeaconWaitState:
if EventType is DRIVE_UNDO_ROTATE
if SweepAmount > 0 (clockwise)
Calculate Angle from SweepAmount
Undo Rotation by angle in ccw direction
else
Calculate Angle from SweepAmount
Undo Rotation by angle in cw direction
Set CurrentState to DriveUndoRotateState

case DriveUndoRotateState:
if EventType is DRIVE_GOAL_REACHED
Stop Motors
Post DRIVE_GOAL_REACHED to SPI Service
Set CurrentState to DriveStoppedState

/****************************************************************************
Function
QueryDriveTrain

Parameters
None

Returns
DriveTrainState_t The current state of the DriveTrain state machine

Description
returns the current state of the Drive Train state machine
Notes
 Author
Andrew Sack
****************************************************************************/

DriveTrainState_t QueryDriveTrain(void)

return CurrentState

MotorControl (Driver) - PIC2

The motor control module abstracts away all low-level setup and control of motors and encoders
and is utilized by DriveTrain Service for high-level closed loop control

Pseudocode
// Initialization function for Driver. Configures all pins, timers, interrupts

bool InitMotorControlDriver(void);

disable global interrupts

enable interrupt multi-vector configuration
Call all init helper functions to init PWMTimer, LeftMotor, RightMotor, InputCapture
LeftEncoder, RightEncoder, and ControlLaw
enable global interrupts

Enable OC1, OC2, IC1, IC2, Timer2, Timer3, Timer4

Init all module variables to 0

/****************************************************************************
* Function
* MotorControl_SetMotorDutyCycle
*
* Parameters
* MotorControl_Motor_t WhichMotor - Left or Right Motor
* MotorControl_Direction_t WhichDirection - Direction to move motor in
* Forward is relative to robot base (CW for right motor, CCW for left)
* uint16_t DutyCycle - (valid range 0-1000 inclusive) PWM duty cycle to set
* Return
* void
 * Description
* Set specified motor to move at set PWM duty cycle in specified direction
* Only works with CLosed Loop Control Disabled
****************************************************************************/

void MotorControl_SetMotorDutyCycle(MotorControl_Motor_t WhichMotor,

MotorControl_Direction_t WhichDirection, uint16_t DutyCycle);

if Motor is Left Motor

Set Left DIRB pin LAT to specified direction
if Direction is backwards
Invert Duty Cycle
Set Left OCRS to DutyCycle * scaling factor

else if Motor is Right Motor

Set Right DIRB pin LAT to specified direction
if Direction is backwards
Invert Duty Cycle
Set Right OCRS to DutyCycle * scaling factor

/****************************************************************************
* Function
* MotorControl_StopMotors
*
* Parameters
* void
* Return
* void
* Description
* Stop both motors, and cancel any tick goal
****************************************************************************/

void MotorControl_StopMotors(void);

Call SetMotorDutyCycle for both left and right motor with duty cycle of 0
Set Left and Right TargetRPM to 0
Set Left and Right TargetTickCount to 0
Clear all DriveGoal Active and Reached flags

/****************************************************************************
* Function
* MotorControl_EnableClosedLoop
*
 * Parameters
* void
* Return
* void
* Description
* Enables Control Law timer interrupt, causing motors to follow CL control
* SetMotorDutyCycle will not work properly when enabled
****************************************************************************/

void MotorControl_EnableClosedLoop(void);

enable timer 4 to start control law isr running

/****************************************************************************
* Function
* MotorControl_DisableClosedLoop
*
* Parameters
* void
* Return
* void
* Description
* Disables Control Law timer interrupt. Motors use direct Duty Cycle
* SetMotorDutyCycle will not work properly when enabled
****************************************************************************/

void MotorControl_DisableClosedLoop(void);

Turn off timer 4 to stop control law running

Reset all PID error terms to 0 for both left and right control structs

/****************************************************************************
* Function
* MotorControl_SetMotorSpeed
*
* Parameters
* MotorControl_Motor_t WhichMotor - Left or Right Motor
* MotorControl_Direction_t WhichDirection - Direction to move motor in
* Forward is relative to robot base (CW for right motor, CCW for left)
* uint16_t Speed - target speed to move at in units of 0.1 RPM (5 RPM = 50)
* Max speed is approx 170 RPM
* Return
* void
 * Description
* Set specified motor to move at set PWM duty cycle in specified direction
* Only works when CloseLoop is enabled
****************************************************************************/

void MotorControl_SetMotorSpeed(MotorControl_Motor_t WhichMotor,

MotorControl_Direction_t WhichDirection, uint16_t Speed);

EnableClosedLoop()

if motor is Left Motor

Set Left TargetDirection to specified direction
Set Left TargetRPM to Speed/10
else if motor is RightMotor
Set Right TargetDirection to specified direction
Set Right TargetRPM to Speed/10

/****************************************************************************
* Function
* MotorControl_ResetTickCount
*
* Parameters
* MotorControl_Motor_t WhichMotor - Left or Right Motor
* Return
* void
* Description
* Sets current tick count for specified motor to zero
****************************************************************************/

void MotorControl_ResetTickCount(MotorControl_Motor_t WhichMotor);

if motor is LeftMotor
Set Left TickCount to 0
else if motor is RightMotor
Set Right TickCount to 0

/****************************************************************************
* Function
* MotorControl_SetTickGoal
*
* Parameters
* MotorControl_Motor_t WhichMotor - Left or Right Motor
* uint32_t NumTicks - Amount of ticks encoder will count to before stopping
 * Return
* void
* Description
* Sets tick goal for specified motor to NumTicks
****************************************************************************/

void MotorControl_SetTickGoal(MotorControl_Motor_t WhichMotor,

uint32_t NumTicks);

if motor is LeftMotor
Set Left TargetTickCount to NumTicks
Set Left DriveGoalReached to false
if NumTicks nonzero set LeftDriveGoalActive to true, else false
else if motor is RightMotor
Set Right TargetTickCount to NumTicks
Set Right DriveGoalReached to false
if NumTicks nonzero set RightDriveGoalActive to true, else false

/****************************************************************************
* Function
* MotorControl_GetEncoder
*
* Parameters
* MotorControl_Motor_t WhichMotor - Left or Right Motor
* Return
* Encoder_t struct for specified motor
* Description
* Get function to return Encoder Struct for specified Motor
****************************************************************************/

Encoder_t MotorControl_GetEncoder(MotorControl_Motor_t WhichMotor);

return Encoder Struct for specified motor

/****************************************************************************
* Function
* MotorControl_GetControlState
*
* Parameters
* MotorControl_Motor_t WhichMotor - Left or Right Motor
* Return
* ControlState_t struct for specified motor
* Description
* Get function to return ControlState Struct for specified Motor
****************************************************************************/

ControlState_t MotorControl_GetControlState(MotorControl_Motor_t
WhichMotor);
return ControlState Struct for specified motor
/****************************************************************************
* Function
* MotorControl_DriveStraight
*
* Parameters
* MotorControl_Direction_t WhichDirection - Direction to drive in
* (forward or backward)
* uint16_t Speed - target speed to move at in units of 0.1 RPM (5 RPM = 50)
* Max speed is approx 170 RPM
* uint16_t DistanceCM - Ground distance to travel in centimeters
* if set to 0, will drive indefinitely
* Return
* void
* Description
* Drive whole drive train straight forward or backward at set speed for
* specified distance
****************************************************************************/

void MotorControl_DriveStraight(MotorControl_Direction_t WhichDirection,

uint16_t Speed, uint16_t DistanceCM);

Call ResetTickCount for both Left and Right Motor

Convert DistanceCM to NumTicks

Call SetTickGoal for NumTicks for both Left and Right Motor
Call SetMotorSpeed for specified Speed and Direction for both Left and Right Motor

/****************************************************************************
* Function
* MotorControl_DriveTurn
*
* Parameters
* MotorControl_Turn_t WhichTurn - Direction to turn (clock or counterclock)
* uint16_t Speed - target speed to move at in units of 0.1 RPM (5 RPM = 50)
* Max speed is approx 170 RPM
* uint16_t AngleDeg - Angle in degrees to rotate base by
 * if set to 0, will drive indefinitely
* Return
* void
* Description
* Turn whole drive train on the spot by Angle in specified direction and speed
****************************************************************************/

void MotorControl_DriveTurn(MotorControl_Turn_t WhichTurn, uint16_t

Speed, uint16_t AngleDeg);

Call ResetTickCount for both Left and Right Motor

Convert Angle to NumTicks

Call SetTickGoal for NumTicks for both Left and Right Motor
If turn is Clockwise
SetMotorSpeed for Left Motor FORWARD at specified Speed
SetMotorSpeed for Right Motor BACKWARD at specified Speed
else if turn is CounterClockwise
SetMotorSpeed for Left Motor BACKWARD at specified Speed
SetMotorSpeed for Right Motor FORWARD at specified Speed

/*
* InitPWMTimer
* Helper Function for InitMotorControl
* Handles configuration of Timer for PWM
*/

static void InitPWMTimer(void)

//Timer 3 configuration (PWM OC)

turn timer off
disable stop in idle
Set prescaler to 4
Set to 16-bit mode (Using T3 does this inherently)
Use synchronous internal clock
Set period to required PWM period
Clear timer

/*
* InitLeftMotor
* Helper Function for InitMotorControl
* Handles configuration of ports and OC for Left Motor
*/
static void InitLeftMotor(void)

Configure DIRB pin as digital output

Set DIRB Lo to ensure motor isn't running
Turn OC1 off
Disable SIDL
Use 16 bit mode
Use timer 3
Set to PWM mode, fault disabled
Write initial duty cycle of 0
Configure DIRA Pin for Output Compare 1

/*
* InitRightMotor
* Helper Function for InitMotorControl
* Handles configuration of ports and OC for Right Motor
*/

static void InitRightMotor(void)

Configure DIRB pin as digital output

Set DIRB Lo to ensure motor isn't running
Turn OC2 off
Disable SIDL
Use 16 bit mode
Use timer 3
Set to PWM mode, fault disabled
Write initial duty cycle of 0
Configure DIRA Pin for Output Compare 2

/*
* InitInputCapture
* Helper Function for InitMotorControl
* Handles configuration interrupts, and timers for input capture
*/

static void InitInputCapture(void)

Clear Interrupt flags

Set priority to 6
Enable Interrupt

// Timer 2 (Encoder input capture)

turn timer off
disable stop in idle
Set prescaler to 4
Set to 16-bit mode
Use syncronous internal clock
Set period to max
Clear timer to 0

/*
* InitLeftEncoder
* Helper Function for InitMotorControl
* Handles configuration of IC and ports for left encoder
*/

static void InitLeftEncoder(void)

Clear Interrupt flags for IC1 and IC1 Error

Set interrupt priority to 7
enable interrupt

//Input Capture 1 configuration (Encoder)

Turn IC1 Off
disable SIDL
Configure Mode Edge Detect mode to every edge (rising and falling)
Use 16 bit timer
Use timer 2
Interrupt every event

Clear IC1 buffer by reading IC1BUF

//Encoder Port Setup

Configure pins for Channel A & B as digital inputs
Map IC1 to RPA4

/*
* InitRightEncoder
* Helper Function for InitMotorControl
* Handles configuration of IC and ports for Right encoder
*/
static void InitRightEncoder(void)

Clear Interrupt flags for IC2 and IC2 Error

Set interrupt priority to 7
enable interrupt

//Input Capture 2 configuration (Encoder)

Turn IC2 Off
disable SIDL
Configure Mode Edge Detect mode to every edge (rising and falling)
Use 16 bit timer
Use timer 2
Interrupt every event

Clear IC2 buffer by reading IC2BUF

//Encoder Port Setup

Configure pins for Channel A & B as digital inputs
Map IC2 to RB10

/*
* InitControlLaw
* Helper Function for InitMotorControl
* Handles configuration of interrupts and timer for control law
*/

static void InitControlLaw(void)

Clear Interrupt flags for Timer 4

Set priority for Timer 4 to 4
enable interrupt

// Timer 4 (Control Law timer)

turn timer 4 off
disable stop in idle
Set prescaler to 4
Set to 16-bit mode
Use syncronous internal clock
Set period to 24999 for time of 5 ms
Clear timer 4 to 0
/****************************************************************************
Function
Timer2Handler

Parameters
None

Returns
void
Description
Interrupt Handler for Encoder timer rollover
Notes

Author
* Andrew Sack
****************************************************************************/

void __ISR(_TIMER_2_VECTOR, IPL6SOFT) Timer2Handler(void)

Disable interrupts globally (creates protected region in case IC happens)

If T2IF is pending (there is a small chance that the IC has fired and handled it already)
Increment the rollover counter
Clear the rollover interrupt (timer interrupt flag)

For Left Encoder

If Current Time - Last Time > threshold to be considered to moving
Set CurrentRPM to 0

For Right Encoder

If Current Time - Last Time > threshold to be considered to moving
Set CurrentRPM to 0

Enable interrupts (OK, since we know interrupts were enabled to get here)

/****************************************************************************
Function
LeftEncoderHandler

Parameters
None
Returns
void
Description
Interrupt Handler for LeftEncoder
Notes

Author
* Andrew Sack
****************************************************************************/

void __ISR(_INPUT_CAPTURE_1_VECTOR, IPL7SOFT)

LeftEncoderHandler(void)

disable global interrupts

Read ICxBUFinto a module variable
Clear the input capture interrupt flag
If T2IF is pending and CapturedTime is after rollover (<0x8000)
Increment the rollover counter
Clear the rollover interrupt (timer interrupt flag)

Copy timer value to Encoder struct and calculate speed

Increment encoder TickCount

Calculate RPM as float. Only keep if below max value to filter out spikes

If Trigger was a rising edge

save value on ch B pin as Direction
else
save inverse of val on ch B pin as Direction

// Distance handling
if TargetTickCount is set (not 0)
if TickCount is within margin of TargetTickCount
Set Motor TargetRPM to 0
Set DriveGoalReached to true
Reset TargetTickCount
enable global interrupts

/****************************************************************************
Function
RightEncoderHandler
Parameters
None

Returns
void
Description
Interrupt Handler for RightEncoder
Notes

Author
* Andrew Sack
****************************************************************************/

void __ISR(_INPUT_CAPTURE_2_VECTOR, IPL7SOFT)

RightEncoderHandler(void)

disable global interrupts

Read ICxBUFinto a module variable
Clear the input capture interrupt flag
If T2IF is pending and CapturedTime is after rollover (<0x8000)
Increment the rollover counter
Clear the rollover interrupt (timer interrupt flag)

Copy timer value to Encoder struct and calculate speed

Increment encoder TickCount

Calculate RPM as float. Only keep if below max value to filter out spikes

If Trigger was a rising edge

save value on ch B pin as Direction
else
save inverse of val on ch B pin as Direction

// Distance handling
if TargetTickCount is set (not 0)
if TickCount is within margin of TargetTickCount
Set Motor TargetRPM to 0
Set DriveGoalReached to true
Reset TargetTickCount
enable global interrupts

/****************************************************************************
Function
ControlLawHandler

Parameters
None

Returns
void
Description
Interrupt Handler for Control Law Timer
Notes

Author
* Andrew Sack
****************************************************************************/

void __ISR(_TIMER_4_VECTOR, IPL4SOFT) ControlLawHandler(void)

Clear the timer 4 interrupt flag

// Left Motor Control Law

Call UpdateControlLaw for LeftControl and LeftEncoder
Call SetMotorDutyCycle to set LeftMotor values to those in LeftControl struct

// Right Motor Control Law

Call UpdateControlLaw for RightControl and RightEncoder
Call SetMotorDutyCycle to set RightMotor values to those in RightControl struct

//Check Drive Goal status for event posting

if either are Left or Right Drive Goal are active
If either are active and not reached, Set DriveGoalReached to false. Else, true

if DriveGoalReached
Post DRIVE_GOAL_REACHED event to DriveTrain Service if DriveGoalReached
Clear all DriveGoalActive and DriveGoalReached flags

/****************************************************************************
Function
UpdateControlLaw

Parameters
ControlState_t *ThisControl - Pointer to Control struct for desired motor
Encoder_t *Encoder - Pointer to Encoder struct for desired motor
Returns
void
Description
Helper function for control law
Notes

Author
* Andrew Sack
****************************************************************************/

void UpdateControlLaw(ControlState_t *ThisControl, Encoder_t

*ThisEncoder)

if a position goal is set (TargetTickCount not 0)

Scale target velocity proportionally based on distance to goal
offset the 0rpm point to beyond the goal to prevent issues with very low velocity motion
ActualTargetRPM = Kp_Postion * (TargetTickCount-TickCount) + PositionOffset

Bound ActualTargetRPM between 0 and TargetRPM

else, no position target

Set ActualTargetRPM to TargetRPM

// Calculate Requested Duty Cycle using PID control

RPMError = ActualTargetRPM - CurrentRPM
SumError += RPMError
RequestedDutyCycle = (pGain * (RPMError+(iGain * SumError)+(dGain*
(RPMError-LastError))));

if RequestedDutyCycle outside of bounds of 0 and Max Duty Cycle

Set RequestedDutyCycle to bound
Remove current RPMError from SumError to prevent windup
LastError = RPMError

BumperService (FSM) - PIC2

The Bumper service manages the event checker for presses of limit switch mounted on rear
bumper of robot
Events

DRIVE_STOP
Will disable Bumper if active

DRIVE_UNTIL_BUMP
Will enable bumper event checker

BUMP_FOUND
Posted by event checker when press detected

States

BumperIdleState
Bumper event checker disabled

BumperActiveState
Bumper event checker is active and looking for bumper press

Pseudocode
/****************************************************************************
Function
InitBumperService

Parameters
uint8_t : the priorty of this service

Returns
bool, false if error in initialization, true otherwise

Description
Saves away the priority, sets up the initial transition and does any
other required initialization for this state machine
Notes

Author
Andrew Sack
****************************************************************************/
bool InitBumperService(uint8_t Priority)

MyPriority = Priority
CurrentState = BumperIdleState
Set EventCheckerActive to false
Configure Pin for bumper as digital input
Enable pull up on bumper pin

/****************************************************************************
Function
PostBumperService

Parameters
ES_Event_t ThisEvent , the event to post to the queue

Returns
boolean False if the Enqueue operation failed, True otherwise

Description
Posts an event to this state machine's queue
Notes

Author
Andrew Sack
****************************************************************************/

bool PostBumperService(ES_Event_t ThisEvent)

return ES_PostToService(MyPriority, ThisEvent)

/****************************************************************************
Function
RunBumperService

Parameters
ES_Event_t : the event to process

Returns
ES_Event_t, ES_NO_EVENT if no error ES_ERROR otherwise

Description
 add your description here
Notes
uses nested switch/case to implement the machine.
Author
Andrew Sack
****************************************************************************/

ES_Event_t RunBumperService(ES_Event_t ThisEvent)

switch on CurrentState
case BumperIdleState:
if EventType is DRIVE_UNTIL_BUMP
EventCheckerActive = true
Init Timer for timeout
Set CurrentState to BumperActiveState

case BumperActiveState:
if EventType is BUMP_FOUND
Post BUMP_FOUND to DriveTrain and SPI
EventCheckerActive = false
Stop Timer
Set CurrentState to BumperIdleState
else if EventType is DRIVE_STOP
EventCheckerActive = false
Stop Timer
Set CurrentState to BumperIdleState
else if EventType is ES_TIMEOUT
Post BUMP_FOUND to DriveTrain and SPI
EventCheckerActive = false
Set CurrentState to BumperIdleState

/****************************************************************************
Function
QueryBumperService
Parameters
None

Returns
BumperState_t The current state of the Bumper state machine

Description
returns the current state of the Bumper state machine
Notes
 Author
Andrew Sack
****************************************************************************/

BumperState_t QueryBumperService(void)

return CurrentState

//Include the event checking function for detecting bumper press

bool Check4Bump(void)
if EventCheckerActive
if bumper pin value is lo (pressed)
post BUMP_FOUND to this service
Set EventCheckerActive to false
return true
return false