Encoder bugfix (Still not proven working)
Some merge. Also initial software motor code (unfinished).