#include <ESP32Servo.h>

// Create Servo motor objects
Servo baseMotor;
Servo motor1;
Servo motor2;
Servo motor3;
Servo gripMotor;

// Pin assignment for each motor
int basePin = 14;
int motor1Pin = 27;
int motor2Pin = 26;
int motor3Pin = 25;
int gripPin = 32;
int irPin = 2;

int basePos = 0;  // variable to hold base position

void setup() {
  // Servo initialization
  baseMotor.attach(basePin);
  motor1.attach(motor1Pin);
  motor2.attach(motor2Pin);
  motor3.attach(motor3Pin);
  gripMotor.attach(gripPin);

  // IR sensor initialization
  pinMode(irPin, INPUT_PULLUP);

  // Set initial positions
  baseMotor.write(0);
  motor1.write(90);
  motor2.write(90);
  motor3.write(90);
  gripMotor.write(140);
}

void loop() {
  // Scan with base from 0 to 160 degrees
  for (basePos = 0; basePos <= 160; basePos++) {
    baseMotor.write(basePos);
    delay(15);

    // If an object is detected, perform the pick and place sequence
    if (digitalRead(irPin) == LOW) {
      pickAndPlace();
    }
  }

  // Scan with base back from 160 to 0 degrees
  for (basePos = 160; basePos >= 0; basePos--) {
    baseMotor.write(basePos);
    delay(15);
    // If an object is detected, perform the pick and place sequence
    if (digitalRead(irPin) == LOW) {
      pickAndPlace();
    }
  }
}
void smoothMove(Servo &servo, int startPos, int endPos, int steps, int delayTime) {
  int stepIncrement = (endPos - startPos) / steps;  // Calculate steps required to go to the end position

  // Decide whether to increment or decrement
  if (endPos > startPos) {  // If new angle is greater than previous angle Increment angle
    for (int i = 0; i < steps; i++) {
      startPos += stepIncrement;  // stepIncrement will be positive in this case.
      servo.write(startPos);
      delay(delayTime);
    }
  } else if (endPos < startPos) {  // If new angle is less than previous angle Increment angle
    for (int i = 0; i < steps; i++) {
      startPos += stepIncrement;  // stepIncrement will be negative in this case
      servo.write(startPos);
      delay(delayTime);
    }
  }

  // Make sure the servo reaches the exact end position
  servo.write(endPos);
}
void pickAndPlace() {
  // parameters for smooting function are: startPos, endPos, steps, delayTime
  // Open the grip smoothly
  smoothMove(gripMotor, 90, 0, 10, 50);  // startPos, endPos, steps, delayTime // startPos, endPos, steps, delayTime

  // Get down to pick the object smoothly
  smoothMove(motor1, 90, 120, 10, 50);  // startPos, endPos, steps, delayTime
  smoothMove(motor2, 90, 60, 10, 50);   // startPos, endPos, steps, delayTime
  smoothMove(motor3, 90, 60, 10, 50);   // startPos, endPos, steps, delayTime

  // Close the grip to hold the object smoothly
  smoothMove(gripMotor, 0, 90, 10, 50);  // startPos, endPos, steps, delayTime

  // Lift the object smoothly
  smoothMove(motor1, 120, 90, 10, 50);  // startPos, endPos, steps, delayTime
  smoothMove(motor2, 60, 90, 10, 50);   // startPos, endPos, steps, delayTime
  smoothMove(motor3, 60, 90, 10, 50);   // startPos, endPos, steps, delayTime

  // Rotate base to 180 degrees smoothly
  smoothMove(baseMotor, basePos, 180, 20, 50);  // startPos, endPos, steps, delayTime

  // Get down to release the object smoothly
  smoothMove(motor1, 90, 120, 10, 50);  // startPos, endPos, steps, delayTime
  smoothMove(motor2, 90, 60, 10, 50);   // startPos, endPos, steps, delayTime
  smoothMove(motor3, 90, 60, 10, 50);   // startPos, endPos, steps, delayTime

  // Open the grip smoothly
  smoothMove(gripMotor, 90, 0, 10, 50);  // startPos, endPos, steps, delayTime

  // Lift the arm back up smoothly
  smoothMove(motor1, 120, 90, 10, 50);  // startPos, endPos, steps, delayTime
  smoothMove(motor2, 60, 90, 10, 50);   // startPos, endPos, steps, delayTime
  smoothMove(motor3, 60, 90, 10, 50);   // startPos, endPos, steps, delayTime

  // Close the grip smoothly
  smoothMove(gripMotor, 0, 90, 10, 50);  // startPos, endPos, steps, delayTime
}