TOWRP Slave Servo

Files:

Pseudo-Code

void main(void)

INIT
Initialize Clock (internal 8MHz oscillator)
Initialize Variables
Initialize Ports (PORT C5 for motor out, PORT B4 and B6 for SSP data in and SSP clock in)
Initialize SSP (slave mode, clock = SCK pin, slave select disabled)
Initialize Timer2 (prescaler to 16, postscaler to 13 = 12.0 kHz or 83.2 us per tick
Initialize Interrupts (global, peripheral, and timer2)

MAIN
Test if data has been received (is transmit complete flag set?)
If not, loop back to main
Otherwise, dump contents of the SSP buffer into W
Save W in user RAM called rxdata
Call Turn function
Loop back to main

TURNS
Move rxdata to W
Store w in a temp file
Use AND bit mask to select three turn bytes, and save in temp file
If positive rotation, turn CCW
   subtract W from center value
Else (negative rotation, turn CCW)
   add w to center value
Save turn value to servo width file, return.

TIMER2 INT:
If servo port was high, send a low signal
  Set port lo
  Subtract PR2 from period (20 ms) to get low time, save to w
Else, send a high signal
  Set port hi
  Move servo_width to w
Move w to PR2
Clear interrupt flag, return