Working on bug fixing with path finding and robot communication