#include <Encoder.h>  // Include Encoder library

// Define motor control pins
int IN1 = 8;
int IN2 = 9;
int ENABLE1 = 10; // PWM for speed

// Define encoder pins and create Encoder object
Encoder myEncoder(2, 3);  // Channel A = pin 2, Channel B = pin 3

long targetPosition = 1000;   // 1/4 cycle, assuming 1000 pulses for a full cycle
long currentPosition = 0;    // Current position from the encoder

// Define the proportional constant (Kp) to tune the responsiveness
float Kp = 0.5;  // Adjust this value to tune the proportional control

void setup() {
  // Set motor control pins as outputs
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(ENABLE1, OUTPUT);

  // Initialize serial communication for debugging
  Serial.begin(9600);

  // Reset encoder position to 0 at start
  myEncoder.write(0);
}

void loop() {
  // Read the current encoder position
  currentPosition = myEncoder.read();

  // Calculate the error between the target and current position
  long error = targetPosition - currentPosition;

  // Calculate the motor speed using proportional control
  // Speed is proportional to the error (bigger error = higher speed)
  int motorSpeed = abs(error) * Kp;

  // Cap the motor speed between 0 and 255 (PWM range)
  motorSpeed = constrain(motorSpeed, 0, 255);

  // Print positions and speed for debugging
  Serial.print("Current Position: ");
  Serial.print(currentPosition);
  Serial.print(" | Target Position: ");
  Serial.print(targetPosition);
  Serial.print(" | Error: ");
  Serial.print(error);
  Serial.print(" | Motor Speed: ");
  Serial.println(motorSpeed);

  // Control motor direction and speed
  if (error > 5) {
    // Rotate forward
    digitalWrite(IN1, HIGH);  // Forward direction
    digitalWrite(IN2, LOW);
    analogWrite(ENABLE1, motorSpeed);  // Proportional speed
  } else if (error < -5) {
    // Rotate backward
    digitalWrite(IN1, LOW);   // Backward direction
    digitalWrite(IN2, HIGH);
    analogWrite(ENABLE1, motorSpeed);  // Proportional speed
  } else {
    // Stop the motor when within tolerance of target position
    analogWrite(ENABLE1, 0);  // Stop motor by setting speed to 0
  }

  delay(50);  // Small delay for stability (can be adjusted or removed)
}