Did some Compass improvements

changed motor setups, Fixed some of the compass code, so it works ~ 3/4
of the time