Arduino servo code with elapsed time and servo routine