Bachelor project submission code

This is the code that we managed to get to for our bachelor project.
Everything should work, though it has only been tested on a single testbed, and has never been tested in water.

If you find any bugs or have ideas feel free to submit them.

Assets 2