Update CollisionController with Servo

Servo Movement to return the safe distance.
Added Comments in the header file.