Fix graph so it represents the new changes
add reverse functionality
Merge branch 'drive_mode' of https://github.com/Vandborg/SW505E14-Software into drive_mode