To fix this we simply change :
if (DistanceToDestination < 1.0f)
{
position = waypoints.Peek();
waypoints.Dequeue();
}
to :
if (DistanceToDestination < speed)
{
position = waypoints.Peek();
waypoints.Dequeue();
}
Thanks for pointing this out Ryan! This was what I had intended to write but for some reason left the speed hard coded...