#include <Servo.h>

Servo servo1, servo2;
int pos1 = 0, pos2 = 0;
int speedDelay = 10;  // ค่าหน่วงเวลาตั้งต้น (10 มิลลิวินาที)

void setup() {
    Serial.begin(9600);  // เริ่มต้น Serial Monitor
    servo1.attach(3);  
    servo2.attach(2);
    Serial.println("Enter position (0-180) and speed delay (ms)");
}

void loop() {
    if (Serial.available()) {
        int newPos = Serial.parseInt();  // อ่านค่าตำแหน่งจาก Serial Monitor
        int newSpeed = Serial.parseInt(); // อ่านค่าความเร็วจาก Serial Monitor

        if (newPos >= 0 && newPos <= 180) {
            Serial.print("Moving to: ");
            Serial.println(newPos);

            if (newSpeed > 0) {
                speedDelay = newSpeed;  // ปรับค่าความเร็วหากผู้ใช้ป้อนค่าที่ถูกต้อง
            }

            // เคลื่อนที่จากตำแหน่งปัจจุบันไปยังตำแหน่งใหม่
            if (newPos > pos1) {
                for (; pos1 <= newPos; pos1++) {
                    servo1.write(pos1);
                    servo2.write(pos1);
                    delay(speedDelay);
                }
            } else {
                for (; pos1 >= newPos; pos1--) {
                    servo1.write(pos1);
                    servo2.write(pos1);
                    delay(speedDelay);
                }
            }
        } else {
            Serial.println("Invalid position! Enter value between 0-180.");
        }
    }
}