Base of obstacle avoidance is written. needs to be tested with the real sensor