added final obstacle avoidance algorithm (may need some tweaking. aka not 'final')