#include <Servo.h>
#include <Arduino.h>

Servo servos[5];  
int positions[5] = {90, 90, 90, 90, 90}; 
int speeds[5] = {10, 10, 10, 10, 10};    

void setup() {
  Serial.begin(115200); 
  servos[0].attach(3);
  servos[1].attach(5);
  servos[2].attach(6);
  servos[3].attach(9);
  servos[4].attach(10);

  for (int i = 0; i < 5; i++) {
    servos[i].write(positions[i]);
  }

  Serial.print("Enter: servo number (1-5), position (0-180), speed (ms): ");
  
}


void moveServo(int servoIndex, int targetPos, int stepDelay) {
  int currentPos = positions[servoIndex];

  if (currentPos < targetPos) {
    for (int pos = currentPos; pos <= targetPos; pos++) {
      servos[servoIndex].write(pos);
      delay(stepDelay);
    }
  } else {
    for (int pos = currentPos; pos >= targetPos; pos--) {
      servos[servoIndex].write(pos);
      delay(stepDelay);
    }
  }

  positions[servoIndex] = targetPos; 
}

void loop() {
  if (Serial.available() > 0) {
    int servoNum, pos, speed;
    
    if (Serial.available() >= 5) {  
      servoNum = Serial.parseInt(); 
      pos = Serial.parseInt();      
      speed = Serial.parseInt();    

      if (servoNum >= 1 && servoNum <= 5 && pos >= 0 && pos <= 180 && speed > 0) {
        moveServo(servoNum - 1, pos, speed);
      } else {
        Serial.println("Invalid input. Try again.");
      }
    }
  }
}