First very stable configuration

Still need to update the joystick and move routine