#include <Encoder.h> // library for using optical encoders
#include <PID_v2.h> // library for PID control

// pin definitions for optical encoders
#define encoder1A 2
#define encoder1B 3
#define encoder2A 4
#define encoder2B 5

// initialize encoder objects
Encoder encoder1(encoder1A, encoder1B);
Encoder encoder2(encoder2A, encoder2B);

// wheel diameter in mm
#define wheelDiameter 176

// distance to travel in mm
#define distance 6000

// motor control pins
#define motor1PWM 6
#define motor1Dir 7
#define motor2PWM 8
#define motor2Dir 9

// PID control variables
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint, 2, 5, 0.1, REVERSE);

void setup() {
  // set up motor control pins as outputs
  pinMode(motor1PWM, OUTPUT);
  pinMode(motor1Dir, OUTPUT);
  pinMode(motor2PWM, OUTPUT);
  pinMode(motor2Dir, OUTPUT);

  // set the direction of the motors
  digitalWrite(motor1Dir, HIGH);
  digitalWrite(motor2Dir, HIGH);

  // set the PID controller's parameters
  myPID.SetMode(AUTOMATIC);
  myPID.SetSampleTime(50);

  // set the desired distance as the setpoint
  Setpoint = distance;
}

void loop() {

  // initialize variables for distance traveled and last encoder counts
  long encoder1Count = 0;
  long encoder2Count = 0;
  long lastEncoder1Count = 0;
  long lastEncoder2Count = 0;
  long distanceTraveled = 0;

  // loop until distance traveled is greater than or equal to distance
  while (distanceTraveled < distance) {
    // read current encoder counts
    encoder1Count = encoder1.read();
    encoder2Count = encoder2.read();

    // calculate distance traveled based on change in encoder counts
    distanceTraveled += (encoder1Count - lastEncoder1Count + encoder2Count - lastEncoder2Count) * (wheelDiameter * PI / 8);

    // save current encoder counts for next iteration
    lastEncoder1Count = encoder1Count;
    lastEncoder2Count = encoder2Count;

    // set the input for the PID controller to the current distance traveled
    Input = distanceTraveled;

    // calculate the output from the PID controller
    myPID.Compute();

    // set the motor speeds based on the PID output
    analogWrite(motor1PWM, Output);
    analogWrite(motor2PWM, Output);
  }

  // stop motors
  analogWrite(motor1PWM, 0);
  analogWrite(motor2PWM, 0);

}