Uses PID Type C calculations. Sort of works. Output gain jumps around like crazy right now.