Restored original analogWrite. Moved servoWrite into its own function and added an optional frequency parameter.