Make debug() function. Saved ~50 bytes. Implement UART rx handler to alter Kp,Ki,Kd values.