Command Module HSM

MasterMachine

InitMasterSM:
Configure pins A2, A3, and A4 as digital inputs and enable pull downs
Configure pin B4 as digital input and enable pull down
Configure SPI1 using SPI HAL
Set SPI module as master
Set pin A0 as SS output
Set pin A1 as SD output
Set idle state for clock high (CKP = 1)
Set CKE equal to 0
Setup 8 bit communication
Set BRG register to 999
Disable enhanced buffer mode
Clear SPI buffer
Enable SPI

Set up PWM for game flag servo

Disable OC4 
Set up B13 as digital output
Select Timer 2 for OC4
Set up PWM mode with no fault detect 
Set initial duty cycle to cw limit
Turn on OC4

Set up PWM for team flag servo

Disable OC2 
Set up B11 as digital output
Select Timer 2 for OC4
Set up PWM mode with no fault detect 
Set initial duty cycle to midpoint position
Turn on OC2

Start Master SM

RunMasterSM:
Set MakeTransition to false
Set NextState to CurrentState

Switch for CurrentState:

Case NotPlaying:
 Call during function for NotPlaying state 
If CurrentEvent.EventType is not ES_NO_EVENT:
Switch for event type:
Case START_BUTTON:
Start Game Timer
Set game flag position to midpoint
Send CW command to motors
Send LOOK_FOR_BEACON event to beacon service
Set NextState to Playing
Mark that we are making a transition
Break
Break
Case Playing:
Call during function for PlayingState
If CurrentEvent.EventType is not ES_NO_EVENT:
Switch for event type:
Case ES_TIMEOUT:
If timer is GAME_TIMER:
Send stop command to motors
Send stop command to flywheel
Put game flag down
Set NextState to NotPlaying
Mark that we are making a transition
Break
Break
If MakeTransition is true:
Set CurrentEvent type to ES_EXIT
Call RunMasterSM
Set CurrentState to NextState
Execute entry function for new state
Return CurrentEvent

StartMasterSM:
Set CurrentState to NotPlaying
Call RunMasterSM with CurrentEvent

CheckStartButton:
Set ReturnVal to false
Read current state of button pin
If CurrentButtonState is not equal to LastButtonState:
Set ReturnVal to true
If button state is 1 (pressed down):
Post START_BUTTON event to MasterSM
Set LastButtonState to CurrentButtonState
Return ReturnVal
CheckLeftBumper:
Set ReturnVal to false
Read current state of left bumper pin
If CurrentLeftBumper is not equal to LastLeftBumper:
If left bumper is pressed and right bumper has been pressed:
Reset bumper values
Post BACK_BUMPER event to Master SM
Set ReturnVal to true
Else
Set Left bumper value
Set LastLeftBumper to CurrentLeftBumper
Return ReturnVal

CheckRightBumper:
Set ReturnVal to false
Read current state of right bumper pin
If CurrentRightBumper is not equal to LastRightBumper:
If right bumper is pressed and left bumper has been pressed:
Reset bumper values
Post BACK_BUMPER event to Master SM
Set ReturnVal to true
Else
Set Right bumper value
Set LastRightBumper to CurrentRightBumper
Return ReturnVal

DuringPlaying function:
Set ReturnEvent to Event
If EventType is ES_ENTRY:
Call StartPlayingSM
If EventType is ES_EXIT:
Call RunPlayingSM
Else:
Do the during function for the Playing SM
Return ReturnEvent

PlayingSM

RunPlayingSM:
Set MakeTransition to false
Set NextState to CurrentState

Switch for CurrentState:

 Case InitialAlign:
Call during function for InitialAlign state
If CurrentEvent.EventType is not ES_NO_EVENT:
Switch for event type:
Case BEACON_ALIGNED:
If strategy is five point:
Set NextState to FivePoint
Set MakeTransition to true
Break
Break
Case FivePoint:
Call during function for FivePoint state
If CurrentEvent.EventType is not ES_NO_EVENT:
No transition to be made
Break
If MakeTransition is true:
Set CurrentEvent type to ES_EXIT
Call RunPlayingSM function
Set CurrentState to NextState
Execute entry function for new state
Return ReturnEvent

StartPlayingSM:
Set CurrentState to InitialAlign
Call RunPlayingSM with CurrentEvent

During5Point function:
Set ReturnEvent to Event
If EventType is ES_ENTRY:
Call StartPlayingSM
If EventType is ES_EXIT:
Call RunPlayingSM
Else:
Do the during function for the Playing SM
Return ReturnEvent

5PointSM

Run5PointSM:
Set MakeTransition to false
Set NextState to CurrentState

Switch for CurrentState:

Case Moving:
If CurrentEvent.EventType is not ES_NO_EVENT:
Switch for event type:
Case ES_TIMEOUT:
If timer is MOVING_TIMER:
If Step is 1:
Set move timer for ninety degree turn CW
Send turn CW command to both motors
Increase step count
If Step is 2:
Set move timer for backward
Send move backward command to motors
Increase step count
If Step is 4:
Set move timer for ninety degree turn CCW
Send turn CCW command to motors
Increase step count
If Step is 5:
Send stop command to motors
Set timer for small forward
Increase step count
Set NextState to Realign
Set MakeTransition to true
If Step is 6:
Set moving timer to small turn time
Send turn CCW command to motors
If Step is 7:
Send move backward command to motors
Send off center turret event to turret service
If Step is 8:
Set moving timer to forward time
Send move forward command to motors
Set NextState to realign
Set MakeTransition to true
If timer is FEEDER_TIMER:
Send stop flywheel command
Break
Case BACK_BUMPER:
If Step is 3:
Set moving timer for small forward time
Send move forward command to motors
Increase step count
Else:
Send stop command to both motors
Set BallsLeft to 3
Set NextState to Reloading
Set MakeTransition to true
Break
Case START_BUTTON:
Send stop command to motors
Set BallsLeft to 3
 Set NextState to Reloading
Set MakeTransition to true
Break
Break
Case Realign:
If CurrentEvent.EventType is not ES_NO_EVENT:
Switch for event type:
Case ES_TIMEOUT:
If timer is MOVING_TIMER:
Send stop command to motors
Send LOOK_FOR_BEACON event to beacon serv
Send turret CW event to turret service
Break
Case BEACON_ALIGNED:
Send stop command to turret service
Send start flywheel command
Start FEEDER_TIMER
Set NextState to Shooting
Set MakeTransition to true
Break
Break
Case Shooting:
If CurrentEvent.EventType is not ES_NO_EVENT:
Switch for event type:
Case ES_TIMEOUT:
If timer is FEEDER_TIMER:
If BallsLeft is greater than 1:
Send feed ball command
Reset feeder timer
Decrement BallsLeft count
Else if BallsLeft is 1 (on last ball):
Send feed ball command
Set last feeder timer
Start moving timer with backward time
Set Step to 6
Set NextState to Moving
Set MakeTransition to true
Break
Break
Case Reloading:
If CurrentEvent.EventType is not ES_NO_EVENT:
Switch for event type:
Case START_BUTTON:
Set step to 8/move forward step
Set moving timer to small forward time
Set NextState to Moving
Set MakeTransition to true
Break
Break
If MakeTransition is true:
Set CurrentEvent type to ES_EXIT
 Call Run5PointSM
Set CurrentState to NextState
Execute entry function for new state
Return ReturnEvent

Start5PointSM:
Set CurrentState to Moving
Set moving timer to move forward time
Send move forward command to motors

TurretService

InitTurretService:
Disable Timer 3 for PWM
Choose internal clock as source
Disable gate
Set prescaler to 1:8
Set period appropriate for 50 Hz
Disable Output Compare 1 for PWM
Set PWM pin as output
Map pin to OC1
Choose Timer 3 as source
Set OC1 in PWM mode with no fault detect
Set OC1RS and OC1R to MID_POINT to start turret in center
Turn on OC1
Start Timer 3
Set CurrentPosition to MID_POINT

RunTurretService:
Switch for Event Type
Case for ES_COMMAND
Switch for Event Parameter (command)
Case for Turn CW (0x08)
Start TURRET_TIMER for timeStep
Set isMoving to true
Set moving direction to CW
End turn CW case

Case for Turn CCW (0x09)

Start TURRET_TIMER for timeStep
Set isMoving to true
 Set moving direction to CCW
End turn CCW case

Case for stop (0x10)

Set isMoving to false
End stop case

Case for move to center (0x11)

Set isMoving to true
Set MovingToPosition to true
Set desired position to MID_POINT
Start TURRET_TIMER for timeStep
If current position is clockwise from desired position
Set moving direction to CCW
Else if current position is counterclockwise from desired position
Set moving direction to CW
Else, we must already be there
Set isMoving to false
Set MovingToPosition to false
End if
End center case
End switch
End ES_COMMAND case

Case for ES_TIMEOUT

If still moving
Move one step in desired direction
Restart TURRET_TIMER with timeStep
If it is moving to a position and it reaches that position
Set isMoving to false
Set MovingToPosition to false
End if
End if
End ES_TIMEOUT case
End switch

IC2Service

RunIC2Service:
Switch for Event Type
Case for LOOK_FOR_BEACON
Set current state to AlignWithBeacon
End look for beacon case

Case for ES_RISING_EDGE

 Increment rising edge counter
Calculate frequency
Add frequency to sum frequency
If rising edge counter reaches 5
Calculate avg freq
Reset frequency sum
Reset rising edge counter
End if
If current state is AlignWithBeacon and avg freq is beacon A
Set team to A
Post BEACON_ALIGNED event with parameter of 1
Set current state to standby
End if
If current state is AlignWithBeacon and avg freq is beacon B
Set team to B
Post BEACON_ALIGNED event with parameter of 0
Set current state to standby
End if
End rising edge case
End switch

ISR_PIC32:

InitializeIC2:
Turn Timer 2 off
Base Timer 2 on PBClk
Disable gate
Set in 16 bit mode
Set 1:16 prescaler
Set period to max time
Turn off IC2 for beacon sensor
Capture every rising edge
Use 16 bit timer
Interrupt on every capture
Use Timer 2
Map input pin to IC2
Set IC2R to 0
Set in multivector mode
Set priority
Clear interrupt flag
Set interrupt bit
Start timer 2
Enable global interrupts
Turn on IC2
IC2Handler:
Read IC2BUF into InterruptTime
Increment interrupt counter
If InterruptTime is greater than previous
Calculate delta
Else
Calculate delta accounting for rollover
End if
If delta is larger than max for uint16
Send overflow event
Else (delta is good)
Send ES_RISING_EDGE event with delta as parameter to IC2Service
End if
Clear source of interrupt