Servo controlling function...

Have a look at the Servo library, in particular the "writeMicroseconds" method.

"millis ()" returns the number of millisconds since you reset the Arduino, so after the first five seconds " if(millis() > 5000) {" will always be true. You need to use the difference between the time "now" and the time you decided to start.