Fix a lot of things

Fixed motor direction consistency issues
Rewrote most of control code
tl;dr Fix everything.