cleanup and started to add servo control