PID working

Everything SHOULD be working together