Changed to Servo library (not tested)
Tested Servo, corrected servo angle and sonic delay
refactoring