Updating with asynch ams I2C code. Note delay required before ams init.
Fixed pid timer rate to be 5kHz, scheduled async encoder read on 4 cycle, pid update on 5th to get 1kHz update rate. Working on moving radio process code out of interrupts.
All pertinent peripherals seem to work for treadmill streaming test: encoder, mpu, uart, radio, motors, commands, pid. Confirmed with loopback test uart -> cmd -> read pid.p_state -> radio response packet. Nothing is used in interrupts anymore, need to decide if that will be used