#include <Servo.h>

// Define servo objects
Servo servoWaist;
Servo servoShoulder;
Servo servoElbow;

// Define servo pin numbers
const int servoWaistPin = 9; // Change as per your setup
const int servoShoulderPin = 10; // Change as per your setup
const int servoElbowPin = 11; // Change as per your setup

// Define button pins for vacuum control
const int vacuumOnButtonPin = 2; // Change as per your setup
const int vacuumOffButtonPin = 3; // Change as per your setup

// Define solenoid valve pin for vacuum
const int solenoidValvePin = 4; // Change as per your setup

// Target positions for pick and place
int pickPosition[3] = {90, 45, 90}; // Waist, Shoulder, Elbow positions for picking
int placePosition[3] = {180, 90, 45}; // Waist, Shoulder, Elbow positions for placing

void setup() {
  // Attach the servo motors to their respective pins
  servoWaist.attach(servoWaistPin);
  servoShoulder.attach(servoShoulderPin);
  servoElbow.attach(servoElbowPin);

  // Initialize the button pins as input
  pinMode(vacuumOnButtonPin, INPUT);
  pinMode(vacuumOffButtonPin, INPUT);

  // Initialize the solenoid valve pin as output
  pinMode(solenoidValvePin, OUTPUT);

  // Ensure the vacuum is off at the start
  digitalWrite(solenoidValvePin, LOW);
}

void loop() {
  // Check if the vacuum activation button is pressed
  if (digitalRead(vacuumOnButtonPin) == HIGH) {
    digitalWrite(solenoidValvePin, HIGH); // Activate the vacuum
    smoothMoveToPosition(pickPosition); // Move to the pick position smoothly
  }

  // Check if the vacuum deactivation button is pressed
  if (digitalRead(vacuumOffButtonPin) == HIGH) {
    smoothMoveToPosition(placePosition); // Move to the place position smoothly
    digitalWrite(solenoidValvePin, LOW); // Deactivate the vacuum
  }

  delay(100); // Add a small delay to prevent button bouncing
}

void smoothMoveToPosition(int positions[]) {
  smoothMove(servoWaist, servoWaist.read(), positions[0]);
  smoothMove(servoShoulder, servoShoulder.read(), positions[1]);
  smoothMove(servoElbow, servoElbow.read(), positions[2]);
}

void smoothMove(Servo &servo, int startPos, int endPos) {
  int stepDelay = 20; // Delay between steps to control speed
  if (startPos < endPos) {
    for (int pos = startPos; pos <= endPos; pos++) {
      servo.write(pos);
      delay(stepDelay);
    }
  } else {
    for (int pos = startPos; pos >= endPos; pos--) {
      servo.write(pos);
      delay(stepDelay);
    }
  }
}