PID Better testing and values
Added stuff for fine tuning the PID values