Almost complete. Main bug is that the ID's switch often. Next step is to clean it up
Merge branch 'master' of https://github.com/ncsurobotics/seawolf5