Edit swap function to take u32 and fix motors so they are not sensors :(
Merge branch 'drive_mode' of https://github.com/Vandborg/SW505E14-Software into drive_mode