initial commit of collision avoidance script
Fixed servo wiring and direction issue