More motor code stuff... The reverse problem is fixed. More speed calculations and whatnot