Problems using PID to control DC Motor?