Initial pull request

There are still a lot of things missing. The code to move the robot is not there yet for instance.