Fixed up PID to make the library cleaner. Also got Howard's old code to
somewhat work with the motor oscillating quickly.