Fixed priorities for peripheral functions so that short, critical code (I2C, UART RX) cannot be preempted by low priority processes (pid, UART TX). Fixed issue where UART RX killed encoders, and address error on high UART RX.
Adding LED indicator to interrupt
Uses #defined constant offset for encoders
Fixed a HUGE error where the encoder would change the angle values in between I2C byte reads, causing the bytes to be inconsistent. The transaction must use a repeated start condition between writing the register value and reading to avoid this.
Interrupt priority
Merge branch 'abuchan/imageproc2.5-integration'
Updated setpoints for current robot. Should really be in robot specific h file
Pre-switch
setpoint update
Update setpoints
Configure PWM 3L as servo output
Merge commit 'affc7f05004a12cb6b8cfc03c7ed69c3f1f742ce' into imageproc2.5-integration

Conflicts:
	tih.c