#include <Servo.h>

// Define servos
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;  // New servo

// Define button pins
const int buttonPins[8] = {2, 3, 4, 5, 6, 7, 8, 9};

// Servo angles
int angles[4] = {90, 90, 90, 90}; // Start with the servos at 90 degrees

// Define grip positions (adjust these values based on your specific setup)
const int gripPosition = 30;  // Angle for gripping
const int releasePosition = 90; // Angle for releasing

// Variable to keep track of grip state
bool isGripping = false;

void setup() {
  // Attach servos
  servo1.attach(10);
  servo2.attach(11);
  servo3.attach(12);
  servo4.attach(13);  // Attach new servo to pin 13

  // Set button pins as input
  for (int i = 0; i < 8; i++) {
    pinMode(buttonPins[i], INPUT_PULLUP);
  }
}

void loop() {
  // Check each button and control the corresponding servo
  if (!digitalRead(buttonPins[0])) { // Button 1 pressed
    angles[0] = constrain(angles[0] + 1, 0, 180); // Increase angle of servo 1
  }
  if (!digitalRead(buttonPins[1])) { // Button 2 pressed
    angles[0] = constrain(angles[0] - 1, 0, 180); // Decrease angle of servo 1
  }
  if (!digitalRead(buttonPins[2])) { // Button 3 pressed
    angles[1] = constrain(angles[1] + 1, 0, 180); // Increase angle of servo 2
  }
  if (!digitalRead(buttonPins[3])) { // Button 4 pressed
    angles[1] = constrain(angles[1] - 1, 0, 180); // Decrease angle of servo 2
  }
  if (!digitalRead(buttonPins[4])) { // Button 5 pressed
    angles[2] = constrain(angles[2] + 1, 0, 180); // Increase angle of servo 3
  }
  if (!digitalRead(buttonPins[5])) { // Button 6 pressed
    angles[2] = constrain(angles[2] - 1, 0, 180); // Decrease angle of servo 3
  }
  if (!digitalRead(buttonPins[6])) { // Button 7 pressed (Grip)
    isGripping = true;  // Set gripping state to true
  }
  if (!digitalRead(buttonPins[7])) { // Button 8 pressed (Release)
    isGripping = false; // Set gripping state to false
  }

  // Update grip state based on isGripping
  if (isGripping) {
    angles[3] = gripPosition; // Move servo 4 to grip position
  } else {
    angles[3] = releasePosition; // Move servo 4 to release position
  }

  // Update servo positions
  servo1.write(angles[0]);
  servo2.write(angles[1]);
  servo3.write(angles[2]);
  servo4.write(angles[3]);  // Update the new servo position

  delay(20); // Small delay for smooth operation
}