Added algorithm for driving our firetruck to a destination.