Added servo code & added digitalWrite fxns