Added helper functions

Lightdiff and go_to_tree get the robot to go to the tree