Added algorithm for tool change