// Sim https://wokwi.com/projects/362739239461471233
// for https://forum.arduino.cc/t/controlling-a-linear-actuator-using-a-pid-controller/1117777/28
const int encoderPin = A0; // analog input pin connected to encoder signal wire
const float calibValues[] = {0, 50, 100, 150, 200, 250, 300, 350, 400, 450, 500, 550, 600, 650, 700, 750, 800, 850}; // calibration values (analog values at 0%, 50%, 100% extension)
const float calibDist[] = {0, 500.6, 1000.6, 1500.6, 2000.6, 2500.6, 3000.6, 3500.6, 4000.6, 4500.6, 5000.6, 5500.6, 6000.6, 6500.6, 7000.6, 7500.6, 8000.6, 850 * 0.6}; // calibration distances (corresponding distances at 0%, 50%, 100% extension)

void setup()
{
  Serial.begin(9600); // initialize serial communication
}

void loop() {
  static uint32_t lastZeroTime = 0;

  int rawValue = analogRead(encoderPin); // read analog input from encoder
  //float distance = interpolate(rawValue, calibValues, calibDist, 18); // interpolate distance from calibration data
  float distance = (rawValue + 0.5) * 850 / 1024;

  float target = 5.74 / 60 / 1000 * (millis() - lastZeroTime);

  float error = target - distance;

  Serial.print("Displacement:");
  Serial.print(distance, 1); // output distance measurement to serial monitor with 0.1mm accuracy
  Serial.print("mm, Target:");
  Serial.print(target, 1); // output distance measurement to serial monitor with 0.1mm accuracy
  Serial.print("mm error:");
  Serial.print(error, 1); // output distance measurement to serial monitor with 0.1mm accuracy
  Serial.print("mm\n");

  delay(100);// wait for 100ms

  if (target > 150 ) {
    lastZeroTime = millis();
  }
}

float interpolate(float x, float xData[], float yData[], int dataSize) {
  // linear interpolation function
  for (int i = 0; i < dataSize - 1; i++) {
    if (x >= xData[i] && x <= xData[i + 1]) {
      return yData[i] + (yData[i + 1] - yData[i]) / (xData[i + 1] - xData[i]) * (x - xData[i]);
    }
  }
  return 0; // return 0 if x is outside of calibration range
}