made a few variables global, added a few methods, now the robot is capable of changing goal position, it just need somebody (aka the kinect obstacle avoidance) when to change from one side to the other