Corrected bugs and reverted the path planner to use 1.05 times the manhattan distance. Works OK, but the robot turns a lot to reach a detected object.