Milestone 3: POC

Work on further robot control features
- Multiple controllers for the robot?
- Docking (should be builtin, but can look into this)
Improve robot interface, tidy things up (code and stying)
documentation