Revised encoder debug, added drive encoder support