Fix line follow and reduce cycletime of update color reg