Code can compile, now needs to be testet on boat, TODO: endstops