Various SCMT fixes. Constant speed, LCM and diagnostic display now working.