Colosensor.c all functions add, robot localizer tools added for increment motion