// Include FastPID library for PID control
#include <FastPID.h>
// Include SparkFun_TB6612 library for motor control
#include <SparkFun_TB6612.h>


// Define Finger_Controller class
class Finger_Controller
{
  private:
    // Motor object for motor control
    Motor motor;
    // FastPID object for PID control
    FastPID myPID;
    // Target position for the motor
    int targetpos;
    // Pin for the potentiometer
    int potpin;

  public:
    // Finger_Controller constructor
    Finger_Controller(int PWNA, int AIN1, int AIN2, int pin, int offsetA, int STBY )
      : motor(AIN1, AIN2, PWNA, offsetA, STBY), myPID(2, 0.001, 0.1, 10, 16, true)
    {
      // Set up the pins for the motor
      pinMode(PWNA, OUTPUT);
      pinMode(AIN1, OUTPUT);
      pinMode(AIN2, OUTPUT);
      pinMode(STBY, OUTPUT);

      // Set the output range of the PID controller
      myPID.setOutputRange(-255, 255);
      // Initialize the target position
      targetpos = 512;
      // Initialize the potentiometer pin
      potpin = pin;
    }

    // Method to set the target position
    void set_target(int target)
    {
      targetpos = target;
    }

    // Method to update the motor position
    void update()
    {
      // Read the current position from the potentiometer
      int positionfeed = analogRead(potpin);
      // Get the PID output based on the current and target position
      int16_t output = myPID.step(targetpos, positionfeed);
      // Print debug information to the serial monitor
      Serial.print(targetpos);
      Serial.print(" ");
      Serial.print(positionfeed);
      Serial.print(" ");
      Serial.println(output);

      // Drive the motor based on PID output
      motor.drive(output,50);
      delay(0);
    }
};

// Create four Finger_Controller objects
Finger_Controller finger1(3, 4, 2, A1, 1, 0);
Finger_Controller finger2(5, 6, 7, A2, 1, 0);
Finger_Controller finger3(9, 10, 8 , A3, 1, 1);
Finger_Controller finger4(11, 13, 12, A4, 1, 1);

void setup() {
  // Setup function is intentionally left empty in this case
}

void loop() {
  // Set the target positions for the motors
  finger1.set_target(900);
  finger2.set_target(900);
  finger3.set_target(900);
  finger4.set_target(900);

  // Update the motor positions in the main loop
  finger1.update();
  finger2.update();
  finger3.update();
  finger4.update();
}