FollowerService.

c
Data private to module
Constants: LOST_LED, LEFT_MOTOR_DIRECTION, RIGHT_MOTOR_DIRECTION,
BLUE_RGB, RED_RGB, GREEN_RGB, INTERACT_LED, FRONT_LIMIT_SWITCH,
SERVO_LOW, SERVO_HIGH, leftAdjust

Variables: MyPriority, PWM Period Ticks, direction, tapeFollowSlow,

tapeFollowFast, servoState, inGamePlay, forward

InitFollowerService
Set MyPriority to parameter passed in
Set direction and testDir to zero
Set tapeFollowSlow and tapeFollowFast to target duty cycles for tape
following (60% and 61% respectively)
Set forward to true
Disable global interrupts
Call configure SPI function
Call configure PWM function
Call configure SERVO PWM function
Enable global interrupts
Initialize the following pins as digital outputs: left motor non-pwm
pin, right motor non-pwm pin, interact LED, RBG LED (red, green, and
blue pins), lost LED.
Turn off all LEDs
Set both motor duty cycles to zero
Set inGamePlay to zero
Set servoState to zero
Post ES_INIT event
Return true if ES_INIT successfully posted, false otherwise

PostFollowerService
Post the parameter event to this service.
Return true if event successfully posted, false otherwise

RunFollowerService
Select one of the following blocks of code depending on the event
passed as a parameter:
If ThisEvent is ES_INIT, no actions needed
If ThisEvent is COMMAND_RECEIVED:
Execute the actions corresponding to the command in the event
parameter, as outlined
in Table 1 below
If ThisEvent is ES_TIMEOUT:
Change the servo state to the opposite of what it is.
Restart ServoTimer for half a second
If ThisEvent is LIMIT_SWITCH_HIT:
Set tapeFollowSlow to 99% duty cycle
Set tapeFollowFast to 100% duty cycle
Return true
ConfigSPI(void)
Disable global interrupts
Disable analog function on all pins
Call BasicConfig to configure SPI1 (disables SPI1)
Set SPI1 to follower
Map RA2 as SDO
Set DISSDO to zero
Map RB5 to SDI
Set DISSDI to zero
Map RB3 to SS input
Clear receive buffer
Disable enhanced buffer
Set active edge to second edge
Initialize CKP
Set transfer width to 16 bit
Set baud rate to 917.4 Hz
Clear SPI receive bits
Set interrupt priority to 7
Enable SPI interrupts
Clear SPIROV bit
Set FRMPOL bit to zero
Enable SPI operation

Void ConfigPWM
(all timer configurations in this function are for timer 3)
Turn off timer 3
Disable gated time mode
Clear TCS control bit
Set pre-scale to 4
Set SIDL bit to zero
Clear TMR3 register
Load PR3 so that the drive frequency is 5000 Hz
Clear TMR3 interrupt flag

(beginning of output compare setup for PWM capability)

(unless otherwise specified, all of the following setup is applied to
both OC1 and OC2)
Disable output compare 1 and 2
Set SIDL bits to zero
Use 16-bit timer source
Set OCTSEL bit to 1 to use timer 3 as clock source
Set OC1RS and OC2RS to initial value of zero
Set OC1R and OC2R to zero
Set output compare mode to PWM mode with fault pin disabled
Set RPB4 to OC1
Set RPA1 to OC2
Turn on OC modules
Enable timer 3

__SPI1_ISR
Read SPI1 buffer into local variable command
Post COMMAND_RECEIVED event to FollowerService with parameter command
Clear SPI1 receive bit
RunCommand
Select action based on command passed in as a parameter (per the table
below). If the command is not in the table below, print “Error,
unrecognized command”

Command Description Action

0x0001 Stop motors, turn off Set left motor duty cycle to
indicators, exit gameplay zero
Set right motor duty cycle to
zero
Turn off RGB LED
Turn off interact LED
Set inGamePlay to zero

0x0002 Start gameplay Set inGamePlay to one

Start ServoTimer for 0.5
seconds

0x0004 Stop motors Set left motor duty cycle to

zero
Set right motor duty cycle to
zero

0x0005 Turn counter clockwise Set right motor duty cycle to

27% forwards
Set left motor duty cycle to
27% backwards

0x0006 Turn clockwise Set right motor duty cycle to

27% backwards
Set left motor duty cycle to
27% forwards

0x0009 Go forwards Set left motor duty cycle to

100% forwards
Set right motor duty cycle to
100% forwards

0x000A Go backwards Set left motor duty cycle to

100% backwards
Set right motor duty cycle to
100% backwards

0x0013 Go forwards at half speed Set left motor duty cycle to

50% forwards
Set right motor duty cycle to
50% forwards

0x0014 Go backwards at half speed Set left motor duty cycle to

50% backwards
Set right motor duty cycle to
50% backwards
0x0017 Go forwards at 30% DC Set left motor duty cycle to
30% forwards
Set right motor duty cycle to
30% forwards

0x0018 Go reverse at 30% DC Set left motor duty cycle to

30% backwards
Set right motor duty cycle to
30% backwards

0x000D Turn on interact LED Set INTERACT_LED to 1

0x0010 Set RGB LED to red Set RED_RGB to 1

Set GREEN_RGB to 0
Set BLUE_RGB to 0

0x0011 Set RGB LED to green Set RED_RGB to 0

Set GREEN_RGB to 1
Set BLUE_RGB to 0

0x0012 Set RGB LED to blue Set RED_RGB to 0

Set GREEN_RGB to 0
Set BLUE_RGB to 1

0x000E Turn off interact LED Set INTERACT_LED to 0

0x0019 Go forward at 27% DC Set left motor duty cycle to

27% forwards
Set right motor duty cycle to
27% forwards

0x0020 Go backwards at 27% DC Set left motor duty cycle to

27% backwards
Set right motor duty cycle to
27% backwards

0x0028 Go forward in repo Set right motor duty cycle to

40% forwards
Set left motor duty cycle to
40% forwards

0x0030 Turn CCW in repo Set right motor duty cycle to

35% forwards
Set left motor duty cycle to
35% backwards

0x0029 Turn CW in repo Set right motor duty cycle to

35% backwards
Set left motor duty cycle to
35% forwards

0x0027 Tape following: slow left Set left motor duty cycle of
motor forwards tapeFollowSlow forwards
Set right motor duty cycle of
tapeFollowFast forwards
0x0023 Tape following: slow left Set left motor duty cycle of
motor reverse tapeFollowSlow reverse
Set right motor duty cycle of
tapeFollowFast reverse

0x0021 Tape following: slow right Set left motor duty cycle of
motor forwards tapeFollowFast forwards
Set right motor duty cycle of
tapeFollowSlow forwards
Set forwards to true

0x0025 Tape following: slow right Set left motor duty cycle of
motor backwards tapeFollowFast backwards
Set right motor duty cycle of
tapeFollowSlow backwards

0x0031 Tape follow forwards Set left motor duty cycle of

tapeFollowFast forwards
Set right motor duty cycle of
tapeFollowFast forwards

0x0032 Tape follow reverse Set left motor duty cycle of

tapeFollowFast reverse
Set right motor duty cycle of
tapeFollowFast reverse

0x0033 Change tape following Set tapeFollowSlow to 60% DC

direction to reverse Set tapeFollowFast to 61% DC
Set right DC to zero
Set left DC to zero
Set forward to false

0x0034 Set servo high Set OC3RS to SERVO_HIGH

0x0035 Set servo low Set OC3RS to SERVO_LOW

0x0037 Turn off lost LED Set LOST_LED to zero

0x0036 Turn on lost LED Set LOST_LED to 1

0x1000 Test 10% DC Set right motor to 10% duty

cycle in testDirection
Set left motor to 10% duty
cycle in testDirection

0x2000 Test 20% DC Set right motor to 20% duty

cycle in testDirection
Set left motor to 20% duty
cycle in testDirection

0x3000 Test 30% DC Set right motor to 30% duty

cycle in testDirection
Set left motor to 30% duty
cycle in testDirection
0x4000 Test 40% DC Set right motor to 40% duty
cycle in testDirection
Set left motor to 40% duty
cycle in testDirection

0x5000 Test 50% DC Set right motor to 50% duty

cycle in testDirection
Set left motor to 50% duty
cycle in testDirection

0x6000 Test 60% DC Set right motor to 60% duty

cycle in testDirection
Set left motor to 60% duty
cycle in testDirection

0x7000 Test 70% DC Set right motor to 70% duty

cycle in testDirection
Set left motor to 70% duty
cycle in testDirection

0x8000 Test 80% DC Set right motor to 80% duty

cycle in testDirection
Set left motor to 80% duty
cycle in testDirection

0x9000 Test 90% DC Set right motor to 90% duty

cycle in testDirection
Set left motor to 90% duty
cycle in testDirection

0x1100 Test 100% DC Set right motor to 100% duty

cycle in testDirection
Set left motor to 100% duty
cycle in testDirection

ConfigServoPWM
(all timer initialization steps below are for timer 2)
Turn off timer
Disable gated time mode
Clear TCS control bit and select clock source as internal clock
Set pre-scale to 64
Set SIDL bit to zero
Load and clear timer register
Set PR2 to 20ms period
Clear T2IS interrupt flag

(beginning of setup for output compare for PWM)

(all setup steps below are applied to OC3)
Disable output compare modules
Set SIDL bits to zero
Use 16-bit timer source
Set timer2 as clock course with OCTSEL bit
Initialize OC3RS to SERVO_LOW
Initialize OC3R to SERVO_LOW
Set output compare mode to PWM mode with fault pin disabled
Set RPB10 to servo
Turn on output compare module
Enable timer 2

Timer2RollOver
Disable global interrupts
If T2IF is pending: increment rollover counter and clear rollover
interrupt
Enable global interrupts

SetLeftDC
If DC parameter is less than zero, set DC to zero
If DC parameter is more than 100, set DC to 100
If direction is zero: set OC2RS to PWMPeriodTicks * DC / 100
If direction is one: set OC2RS to PWMPeriodTicks * (100 - DC) / 100
If direction is not zero or one: print error message for invalid
direction

SetRightDC
If DC parameter is less than zero, set DC to zero
If DC parameter is more than 100, set DC to 100
If direction is zero: set OC1RS to PWMPeriodTicks * DC / 100
If direction is one: set OC1RS to PWMPeriodTicks * (100 - DC) / 100
If direction is not zero or one: print error message for invalid
direction