Fixed configs

Everything works now PERFECTION
Updated PID loop (not done yet)