/*
  Forum: https://forum.arduino.cc/t/help-with-pid-line-follower-robot-code/1228016/8
  Wokwi: https://wokwi.com/projects/390702904999743489

  This sketch calculates and prints input and results of the PID algorithm
  as realized in the function linefollow()

  It shows that whatever data are coming from the sensors 
  the robot should always go left if the right motor speed is calculated 
  as x = rsp/1.49

*/

int P, D, I, previousError, PIDvalue, error;
int lsp, rsp, x;
int lfspeed = 234;

float Kp = 0.02;  // Adjust this value based on experimentation
float Kd = 0.001;  // Adjust this value based on experimentation
float Ki = 0.00001; // Adjust this value based on experimentation

void setup() {
  Serial.begin(115200);
  Serial.println("Using the divider 1.49 to calculate right motor speed");
  Serial.println("SensorL\tSensorR\trsp\tlsp\tx");
  for (int i = 0; i <= 1000;i += 100){
     linefollow(i,1000-i, true);
  }
  Serial.println();
  Serial.println("Using rsp as right motor speed");
  Serial.println("SensorL\tSensorR\trsp\tlsp\tx");
  for (int i = 0; i <= 1000;i += 100){
     linefollow(i,1000-i, false);
  }
}

void loop() {
}

void linefollow(int analogValueSensorLeft, int analogValueSensorRight, boolean useDivider) {
  int error = analogValueSensorLeft - analogValueSensorRight;
  P = error;
  I = I + error;
  D = error - previousError;

  PIDvalue = (Kp * P) + (Ki * I) + (Kd * D);
  previousError = error;

  lsp = lfspeed - PIDvalue;
  rsp = lfspeed + PIDvalue;

  if (lsp > 255) {
    lsp = 255;
  }
  if (lsp < 0) {
    lsp = 0;
  }
  if (rsp > 255) {
    rsp = 255;
  }
  if (rsp < 0) {
    rsp = 0;
  }
  Serial.print(analogValueSensorLeft);
  Serial.print('\t');
  Serial.print(analogValueSensorRight);
  Serial.print('\t');
  Serial.print(rsp);
  Serial.print('\t');
  Serial.print(lsp);
  Serial.print('\t');
  if (useDivider) {
     x = rsp/1.49;
  } else {
    x = rsp;
  }
  Serial.print(x);
  Serial.print('\t');
  // x and lsp used to control the speed of x -> right motor, lsp = left motor
  // analogWrite(engineR, x);
  // analogWrite(engineL, lsp);
  byte mode = 0;
  if (lsp > x) { mode = 1;}  // left motor quicker than right one 
  if (lsp < x) { mode = 2;}  // left motor slower than right one
  switch(mode){
    case 0:
        Serial.println("Same speed - Robot should move straight forward");
      break;
    case 1:
        Serial.println("Left quicker - Robot turns to the right");
      break;
    case 2:
        Serial.println("Right quicker - Robot turns to the left");
      break;
  }
  
}