FSM with obstacle avoidance as well as wall following working
FSM done now commenting