#define TX 13
#define RX1 2
#define RX2 4
#define RX3 5
#define RX4 18
#define RX5 19
#define RX6 21
#define RX7 22
#define RX8 23
int a[8];
int position;
int leftSpeed=180, rightSpeed=180;

//PID parameter
const int setPoint=3500;
int P, I, D, error, prev_error;
float Kp=0.08f, Ki=0.0002f, Kd=1.0f;

int PID_value;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(TX, OUTPUT);
  pinMode(RX1, INPUT_PULLUP);
  pinMode(RX2, INPUT_PULLUP);
  pinMode(RX3, INPUT_PULLUP);
  pinMode(RX4, INPUT_PULLUP);
  pinMode(RX5, INPUT_PULLUP);
  pinMode(RX6, INPUT_PULLUP);
  pinMode(RX7, INPUT_PULLUP);
  pinMode(RX8, INPUT_PULLUP);
  digitalWrite(TX, HIGH);
}

void loop() {
  position = readLine();
  error=position - setPoint;
  P = error; //replace your position value at set point
  I = I + error;
  D = P - prev_error;
  prev_error = error;
  
  PID_value = P * Kp + I * Ki + D *Kd;

   const int maxSpeed = 180;  //setting the maxSpeedimum speed of motor

    if(PID_value>maxSpeed)
     PID_value = maxSpeed;

    if(PID_value < -maxSpeed)
      PID_value = (-1*maxSpeed);
      
    if(PID_value < 0)
    {
      leftSpeed=maxSpeed-PID_value;
      rightSpeed=maxSpeed;
    }else{
      rightSpeed=maxSpeed+PID_value;
      leftSpeed=maxSpeed;
    }
  //Serial.println(PID_value);
  Serial.println(leftSpeed);
  delay(10);
}
int readLine(){
  //read sensor value
  a[0] = digitalRead(RX1);
  a[1] = digitalRead(RX2);
  a[2] = digitalRead(RX3);
  a[3] = digitalRead(RX4);
  a[4] = digitalRead(RX5);
  a[5] = digitalRead(RX6);
  a[6] = digitalRead(RX7);
  a[7] = digitalRead(RX8);

  int val;
  val = (7000*a[0] + 6000*a[1] + 5000*a[2] + 4000*a[3] + 3000*a[4] + 2000*a[5] + 1000*a[6] + 0*a[7])/(a[0] + a[1] + a[2] + a[3] + a[4] + a[5] + a[6] +a[7]);
  return val;
}

//display OLED