#include <ESP32Servo.h>
// Define servo objects for each finger
Servo finger1;
Servo finger2;
Servo finger3;

// Define FSR sensor pins for each finger
const int fsrPin1 = 25;
const int fsrPin2 = 12;
const int fsrPin3 = 34;

// Define force limits for each finger
const int forceLimit1 = 1000;
const int forceLimit2 = 1000;
const int forceLimit3 = 1000;

void setup() {
  // Start serial communication
  Serial.begin(115200);

  // Attach servos to pins
  finger1.attach(18);
  finger2.attach(19);
  finger3.attach(21);

  // Set initial position of servos
  finger1.write(0);
  finger2.write(0);
  finger3.write(0);

  // Set FSR sensor pins as inputs
  pinMode(fsrPin1, INPUT);
  pinMode(fsrPin2, INPUT);
  pinMode(fsrPin3, INPUT);
  }

void loop() {

  int input = Serial.parseInt(); // Read user input from serial monitor
  if (input == 1) { // If user input is 1, start operation
    Serial.println("Starting operation in 2 seconds...");
    delay(2000); // Wait for 5 seconds before starting operation
    
    // Read force sensor values for each finger
  int fsrValue1 = analogRead(fsrPin1);
  delay(10);
  int fsrValue2 = analogRead(fsrPin2);
  delay(10);
  int fsrValue3 = analogRead(fsrPin3);

    // Print FSR sensor values to serial monitor


    // Move each finger until force limit is reached
    while (fsrValue1 < forceLimit1 || fsrValue2 < forceLimit2 || fsrValue3 < forceLimit3) {
      // Move finger 1
      if (fsrValue1 < forceLimit1) {
        int newPos = finger1.read()+4;
        
        if (newPos > 160) {
          newPos = 160;
        }
        finger1.write(newPos);
        delay(10);
        Serial.print(newPos);
        Serial.print("\t");
        fsrValue1 = analogRead(fsrPin1);
      }

      // Move finger 2
      if (fsrValue2 < forceLimit2) {
        int newPos = finger2.read() + 4;
        if (newPos > 160) {
          newPos = 160;
        }
        finger2.write(newPos);
        delay(10);
        fsrValue2 = analogRead(fsrPin2);
      }

      // Move finger 3
      if (fsrValue3 < forceLimit3) {
        int newPos = finger3.read() + 4;
        if (newPos > 160) {
          newPos = 160;
        }
        finger3.write(newPos);
        fsrValue3 = analogRead(fsrPin3);
      }
  fsrValue1 = analogRead(fsrPin1);
  fsrValue2 = analogRead(fsrPin2);
  fsrValue3 = analogRead(fsrPin3);
  Serial.print(fsrValue1);
  Serial.println("\t");
  delay(100);

    }

    // Stop all fingers when force limit is reached
    Serial.println("Contact made on all three fingers");
   
  }
  else if (input == 2){
     
    
    finger1.write(0);
    finger2.write(0);
    finger3.write(0);
  }

}