Added servo library, wired up servo code