PID control completed, Timer stuff added