#include <Servo.h>

// Create Servo objects for 4 servos
Servo servo1, servo2, servo3, servo4;
bool hasRun = false; // Flag to ensure the program runs only once

void moveServo(Servo &servo, int startAngle, int endAngle, int delayTime) {
  int step = (startAngle < endAngle) ? 1 : -1;
  for (int angle = startAngle; angle != endAngle + step; angle += step) {
    servo.write(angle);
    delay(delayTime);
  }
}

void setup() {
  Serial.begin(9600); // Start serial communication for debugging

  servo1.attach(3);
  servo2.attach(5);
  servo3.attach(6);
  servo4.attach(9);

  servo1.write(0);
  servo2.write(0);
  servo3.write(0);
  servo4.write(0);
  
  Serial.println("Setup complete.");
}

void loop() {
  if (!hasRun) {
    delay(2000);
    Serial.println("Moving servo2 down");
    moveServo(servo2, 0, 40, 30);
    delay(1000);
    
    Serial.println("Clamping servo4");
    moveServo(servo4, 0, 35, 30);
    delay(1000);

    Serial.println("Moving servo2 up");
    moveServo(servo2, 40, 0, 30);
    delay(1000);

    Serial.println("Rotating servo1");
    moveServo(servo1, 0, 90, 30);
    delay(1000);

    Serial.println("Moving servo2 down again");
    moveServo(servo2, 0, 40, 30);
    delay(1000);

    Serial.println("Releasing clamp");
    moveServo(servo4, 35, 0, 30);
    delay(1000);

    Serial.println("Moving servo2 up");
    moveServo(servo2, 40, 0, 30);
    
    hasRun = true; // Set flag to prevent loop from running again
  }
}