Attempting to use the vfh algorithm (provided) for motion planning. Doesn't work yet.