Basic implementation of PID+small fixes