Arduino Code Help

[Chr5]

Hello, I need to make a slight modification to the following code so that I can control the speed of each servo motor and i can interrupt each input. It works for one servo motor. I have just added the other servo objects, but how do I complete the part at the bottom "int speedControle(int dly, int pos) { // function that needs delay and new position" I can't add all the servo objects here. Any help is appreciated, thanks!

#include <Servo.h>

Servo myservo; // create servo object to control a servo
Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;
int servoStep;
int stepActive;
int timer;
int speedTimer;

void setup()
{
myservo.attach(9); // attaches the servo on pin 9 to the servo object
myservo2.attach(10);
myservo3.attach(11);
myservo4.attach(5);
myservo.attach(6);
Serial.begin(9600); //begins serial communication
speedTimer = millis();
}

void loop()
{
int pos;
if (Serial.available()){
delay(100);
while(Serial.available()>0){
pos=Serial.read(); //reads the value sent from Visual Basic
if(pos=='0'){
servoStep = 1;
Serial.println("0");
}
else if(pos=='1') {
servoStep = 0;
myservo.write(-90); //rotates the servo 90 degrees (right)
myservo2.write(-90);
myservo3.write(100);
myservo4.attach(25);
myservo5.attach(60);
Serial.println("1");
}
else if(pos=='2') {
servoStep = 0;
myservo.write(180); //rotates the servo 180 degrees (Left)
myservo2.write(180);
myservo3.write(60);
myservo4.write(30);
myservo5.write(10);
Serial.println("2");
}
else if(pos=='3') {
servoStep = 0;
myservo.write(-180); //rotates the servo 180 degrees (right)
myservo2.write(-180);
myservo3.write(30);
myservo4.write(30);
myservo5.write(50);
Serial.println("3");
}
}
}
if(servoStep != 0) {
servoControle(servoStep);
}
}
void servoControle(int v) {
switch (v) {
case 1:
if(stepActive == 0) {
timer = millis();
stepActive = 1;
}
if(speedControle(20, 45) == 1) { // calls the function with these parameters delay=20, new position=45
servoStep++; // and cheks if it is done
stepActive = 0;
}
break;
case 2:
if(timer+1000 <= millis()) {
if(stepActive == 0) {
timer = millis();
stepActive = 1;
}
if(speedControle(20, 0) == 1) {
servoStep++;
stepActive = 0;
}
}
break;
case 3:
if(timer+1000 <= millis()) {
if(stepActive == 0) {
timer = millis();
stepActive = 1;
}
if(speedControle(20, 90) == 1) {
servoStep++;
stepActive = 0;
}
}
break;
case 4:
if(timer+2000 <= millis()) {
if(stepActive == 0) {
timer = millis();
stepActive = 1;
}
if(speedControle(20, 135) == 1) {
servoStep++;
stepActive = 0;
}
}
break;
case 5:
if(timer+1000 <= millis()) {
if(stepActive == 0) {
timer = millis();
stepActive = 1;
}
if(speedControle(20, 180) == 1) {
servoStep++;
stepActive = 0;
}
}
break;
case 6:
if(timer+4000 <= millis()) {
if(stepActive == 0) {
timer = millis();
stepActive = 1;
}
if(speedControle(20, 90) == 1) {
servoStep = 0;
stepActive = 0;
}
}
break;
}
}
int speedControle(int dly, int pos) { // function that needs delay and new position
int currentPos = myservo.read(); // reads last position
if(speedTimer+dly <= millis()) { // manages the delay
if(currentPos > pos) { // chek if new position is clockwise og not
myservo.write(currentPos-1); // moves servo one degree
speedTimer = millis();
} else if(currentPos < pos) {
myservo.write(currentPos+1);
speedTimer = millis();
} else if(currentPos == pos) { // cheks if function is completed
return 1;
}
}
}