SPIService

InitSPIService:
Disable interrupts
Configure SPI1 using SPI HAL
Set in follower mode
Clear receive buffer
Disable enhanced buffer mode
Clear SPI1 interrupt flag
Set priority
Set SPI1 interrupt enable bit
Clear SPIROV bits
Map A1 as SDI
Map A0 as SS1
Disable framed mode
Set CKP to 1
Set CKE to 0
Set up 8 bit communication
Set BRG to 999
Turn on SPI1
Disable Timer 5 for Control Law
Choose internal clock as source
Disable gate
Set pre-scale
Set period for 5 ms
Enable local timeout interrupt
Set priority
Enable interrupts
Start timer 5

Control Law ISR:

Clear source of interrupt
Post ES_NEW_DUTY event to motor services

ReceiverHandler ISR:
Disable interrupts
If received flag is set
Read buffer to Received
Post ES_COMMAND event to all with parameter of command Received
Clear source of interrupt
End if
Enable interrupts
Left Motor Service

InitLeftMotorService:
Disable timer 3 for PWM
Choose internal clock as source
Disable gate
Set pre-scale
Set timeout period
Set multivector mode
Make sure interrupts are enabled globally
Turn off OC4 for PWM
Set PWM pin to output
Map pin to OC4
Set IN2 pin to output
Set OC4 to timer 3
Set OC4 to PWM mode with no fault detect
Set OC4RS
Set OC4R
Turn on OC4
Disable Timer 2 for encoders
Choose internal clock as source
Disable gate
Set pre-scale
Set timeout period
Enable local timeout interrupt
Set priority
Disable IC1 for left encoder
Set encoder pin as input
Map pin to IC1
Continue to operate in idle mode
Interrupt on every capture event
Set to 16 bit capture
Set Timer 2 as source for IC1
Simple capture rising edge
Enable interrupts
Set priority
Enable IC1
Start timers

RunLeftMotorService:
Switch for event type
Case for ES_NEW_DUTY
Run control law
Calculate new PulseWidth
Set OC4RS to PulseWidth
End case
 Case for ES_COMMAND
Reset SumError to 0
Reset RPM to 0
Switch for event parameter
Case for stop (0x00)
Set targetRPM to 0
Set OC4RS to 0 to make IN1 low
Set IN2 low
Set direction to forward
End stop case

Case for forward (0x01) and CW (0x03)

Set targetRPM to full speed
Set OC4RS to half speed forward to get started
Set IN2 pin low
Set direction to forward
End forward and CW case

Case for backward (0x02) and CCW (0x04)

Set targetRPM to full speed
Set OC4RS to 0 to get started backwards
Set IC2 pin high
Set direction to backwards
End case
End switch
End case
End switch

LeftEncoderISR:
Read IC1BUF into ThisPulse
Set PulseLength as difference between LastPulse and ThisPulse
If PulseLength is acceptable
Calculate current RPM
If RPM is acceptable
Set RPM to calculated value
End if
Set ThisPulse as LastPulse
End if
Clear source of interrupt

ControlLaw:
Initialize RPMError as static float
Initialize RequestedDuty as static float
Calculate RPMError (make sure the arithmetic is floating point)
If direction is backwards
 Set RPMError as negative
End if
Add error to SumError
Calculate RequestedDuty
If RequestedDuty is over 100
Set RequestedDuty to 100
Take error away from sum
Else if RequestedDuty is less than 0
Set RequestedDuty to 0
Take error away from sum
End if
Set DC to RequestedDuty

LauncherService

InitLauncherService:
Disable OC1 for Flywheel
Set PWM pin to output
Map pin to OC1
Set OC1 to Timer 3
Set to PWM mode with no fault detect
Set OC1RS and OC1R as 0
Turn on OC1
Disable OC5 for Feeder
Set PWM pin to output
Map pin to OC5
Set OC5 to Timer 3
Set to PWM mode with no fault detect
Set OC5RS and OC5R as 0
Turn on OC5

RunLauncherService:
Switch for Event Type
Case for ES_COMMAND
Switch for Event Parameter
Case for start flywheel (0x05)
Set OC1RS to full duty cycle to get flywheel going
Wait for a short time
Set OC1RS to tuned duty cycle
End start flywheel case

Case for stop flywheel (0x06)

Set OC1RS to 0
End stop flywheel case
 Case for feed ball (0x07)
Set OC5RS to tuned duty cycle
Set FEEDER_TIMER for time to rotate 90 degrees
End feed ball case
End switch
End ES_COMMAND case

ES_TIMEOUT case (for FEEDER_TIMER)

Turn off feeder motor
End ES_TIMEOUT case
End switch