Is there a reason not to use some library example code? I could imagine that you need servo library as well.