TOWRP Slave Motor
Files:
Pseudo-Code
void main(void) INIT Initialize Variables Initialize Ports (PORT A1 for servo 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 10 = 12.5 kHz or 80 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 Velocity function Loop back to main VELOCITY Move rxdata to W Store w in a temp file Move zero velocity into w. Test if velocity is negative (bit 7 of temp file) If yes, save zero velocity to motor width file, return. If no, use AND bit mask to select three velocity bytes, and save in temp file Clear C to avoid shifting it into the file Rotate right through file four times, the last time save to w. Call velocity table Velocity table returns to w the correct velocity value Save velocity to motor width file, return. TIMER2 INT: If motor 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 motor_width to w Move w to PR2 Clear interrupt flag, return