Updated Sketch to include servo code - needs to be tested
Sketch with servo working