PID and encoders working