int pins[14]={A0,A1,A2,A3,A4,A5,A6,A7,A8,A9,A10,A11,A12,A13};
bool sensor[14];
void setup() {
  Serial.begin(115200);
  for(byte i=0;i<14;i++){
    pinMode(pins[i], INPUT);
  }

}

int P;
int I;
int D;
float Kp = 0.02;
float Ki = 0.00001;
float Kd = 0.7;

int lastError;
void loop() {
  get_sensor();
  int pos = get_pos();
  int error = 6500 - pos;

  P = error;
  I = error + I;
  D = error - lastError;
  lastError = error;

  int speedChange = P*Kp + I*Ki + D*Kd;
  int speed_kanan = 100 + speedChange;
  int speed_kiri = 100 - speedChange;

  
  for(byte i=0;i<14;i++){
    Serial.print(" ");
    Serial.print(sensor[i]);
  }
  Serial.print(" ");
  Serial.print(speedChange);
  Serial.print(" ");
  Serial.print(pos);
  Serial.println();
  delay(100);
}
void get_sensor(){
  for(byte i=0;i<14;i++){
    sensor[i] = analogRead(pins[13-i]) > 512;
  }
}
int get_pos(){
  int sum1 =sensor[0]*0+sensor[1]*1000+sensor[2]*2000+sensor[3]*3000+sensor[4]*4000+sensor[5]*5000+sensor[6]*6000+sensor[7]*7000+sensor[8]*8000+sensor[9]*9000+sensor[10]*10000+sensor[11]*11000+sensor[12]*12000+sensor[13]*13000;
  int sum2 = sensor[0]+sensor[1]+sensor[2]+sensor[3]+sensor[4]+sensor[5]+sensor[6]+sensor[7]+sensor[8]+sensor[9]+sensor[10]+sensor[11]+sensor[12]+sensor[13];
  if(sum2 == 0){
    return -1;
  }
  if(sum2 != 0){
    return sum1/sum2;
  }
}