InitMotorSM:

Call InitPWM
Call InitEncoderIC
Call InitPeriodicTimers
Move our state into Stopped
Set target RPM to stopped and command RPM to default speed

RunMotorSM:
If the event is StopDriving
Call StopMotor
Change state to Stopped
End if

If this event is ES_TIMEOUT from the left encoder timer

Set left period to maximum
Set left RPM to stopped
End if

If this event is ES_TIMEOUT from the right encoder timer

Set right period to maximum
Set right RPM to stopped
End if

Switch on current state:

Stopped:
If this event is DriveForward
Call Drive with Forward parameter
Change state to DrivingForward
Else if this event is DriveBackward
Call Drive with Backward parameter
Change state to DrivingBackward
Else if this event is TurnLeft
Call Rotate with CCW parameter
Change state to TurningLeft
Else if this event is TunRight
Call Rotate with CW parameter
Change state to TurningRight
End if
DrivingForward:
If this event is FrontOof
Call StopMotor
Clear the odometer
Set the drive odometer for backing up
Set the speed for backing up
Start backing up
Change state to UnFrontOofing
End if
DrivingBackward:
If this event is RearOof
Call StopMotor
 Clear the odometer
Set the drive odometer for driving forward
Set the speed for driving forward
Start driving forward
Change state to UnRearOofing
End if
TurningLeft:
Do nothing
TurningRight:
Do nothing
UnFrontOofing:
If this event is EncoderCountReached
Call StopMotor
Change state to Stopped
Post UnFrontOofComplete to NavigationSM
End if

UnRearOofing
If this event is EncoderCountReached
Call StopMotor
Change state to Stopped
Post UnRearOofComplete to NavigationSM
End if

QueryMotorSM:
Return state of MotorSM

SetTargetRPM:
Assign CommandSpeedRPM to target RPM

SetDriveOdometer:
Set measuring flag to true
Calculate encoder pulses from distance parameter

SetRotateOdometer:
Set measuring flag to true
Calculate encoder pulses from angle parameter

ClearOdometer:
Set measuring flag to false
Clears encoder counts
Clears target encoder counts

LeftEncoder_ISR: (ISR)
Clear the source of the interrupt
Grab the captured value
Calculate the period
If the measuring flag is true
Increment encoder count
If encoder count matches target encoder count
 If we are UnFrontOofing
Post EncoderCountReached to self
Else
Post StopDriving to self
Post EncoderCountReached to NavigationSM and
MasterSM
End if
Clear the odometer
End if
End if
Update the last capture time
Start encoder timeout timer

RightEncoder_ISR: (ISR)
Clear the source of the interrupt
Grab the captured value
Calculate the period
Update the last capture time
Start encoder timeout timer

LeftPeriodic_ISR:
Clear the source of the interrupt
If target RPM is set to 0
Set duty cycle to 0
End if
Else conduct PID control
Calculate left RPM from left encoder
Calculate error from target RPM and left RPM
Calculate integral term
Calculate position term
Calculate differential term and update last error
Calculate new duty cycle from terms
If duty cycle is greater than threshold
Increment a stall counter
If the stall counter exceeds a stall threshold
If we are moving a miner
Post FrontOof to self
End if
End if
Else
Reset the stall counter
End if
If duty cycle is less than 0 or greater than 100
Bind the duty cycle between 0 and 100
Subtract away integral term
End if
Set duty cycle
End if

RightPeriodic_ISR:
Clear the source of the interrupt
If target RPM is set to 0
Set duty cycle to 0
End if
Else conduct PID control
Calculate left RPM from left encoder
Calculate error from target RPM and left RPM
Calculate integral term
Calculate position term
Calculate differential term and update last error
Calculate new duty cycle from terms
If duty cycle is greater than threshold
Increment a stall counter
If the stall counter exceeds a stall threshold
If we are moving a miner
Post FrontOof to self
End if
End if
Else
Reset the stall counter
End if
If duty cycle is less than 0 or greater than 100
Bind the duty cycle between 0 and 100
Subtract away integral term
End if
Set duty cycle
End if

StopMotor:
Set target RPM to 0
Write digital channels to LO
Clear polarity bits

Rotate:
Set target RPM to command speed RPM
If direction is CW
Write LO to left wheel digital
Clear polarity bit for left wheel
Write HI to right wheel digital
Write polarity bit for right wheel
Else
Write HI to left wheel digital
Write polarity bit for left wheel
Write LO to right wheel digital
Clear polarity bit for right wheel
End if

Drive:
Set target RPM to command speed RPM
If direction is Forward
 Write LO to left wheel digital
Clear polarity bit for left wheel
Write LO to right wheel digital
Clear polarity bit for right wheel
Else
Write HI to left wheel digital
Write polarity bit for left wheel
Write HI to right wheel digital
Write polarity bit for right wheel
End if

InitEncoderIC:
Start by enabling the clock to wide timer 0
Enable the clock to port C
Wait for the peripheral to be ready for wide timer 0
Wait for the peripheral to be ready for port C
Make sure that timer A is disabled
Make sure that timer B is disabled
Set it up in 32bit wide mode
Initialize the interval load register to 0xffffffff
Set up timer A/B in capture mode, edge time, and up-counting
Set event to rising edge
Select alternate function for PC4 and PC5
Map PC4 and PC5 to wide timer 0 A/B
Enabled pins 4 & 5 on Port C for digital I/O
Make pins 4 & 5 inputs
Enable local capture interrupt for Timer A/B
Enable timer A/B interrupt in NVIC
Lower the priority of the NVIC interrupts
Make sure interrupts are enabled globally
Start Timer A and enable stalling in debugger
Start Timer B and enable stalling in debugger

InitPeriodicTimers:
Start by enabling the clock to the timer (Wide Timer 4)
Kill a few cycles to let the clock get going
Make sure that timer (Timer A/B) are disabled before configuring
Set it up in 32bit wide mode
Set up timer A/B in periodic mode
Set timeout in A/B to periodic timer period
Enabled local timeout interrupt in timer A/B
Enable timer A/B in the NVIC interrupt
Lower the priority of the NVIC interrupts
Make sure interrupts are enabled globally
Start Timer A and enable stalling in debugger
Start Timer B and enable stalling in debugger