Separated big PID.c, fixed a bug, more comments