major refactor, ultasonic sensor is working, better error handling and cleanup