Code clean up and Robot Driving 2 methods start