Do line 5/july

//File lấy dữ liệu
//Số đc lấy từ trái sang phảiic
#define motor1 10
#define motor2 11
const int trig = 2;     // chân trig của HC-SR04
const int echo = 3;     // chân echo của HC-SR04


const int qtro[7] = {A0,A1,A2,A3,A4,A5,A6};
int ktraqtro[7] = {0,0,0,0,0,0,0};
int locqtro[7] = {0,0,0,0,0,0,0};

int maxspeed = 120;
bool isRun = false;
int timer = 0;
//---------------------------------------------------------------------

void setup() {
   pinMode(trig,OUTPUT);   // chân trig sẽ phát tín hiệu
   pinMode(echo,INPUT);    // chân echo sẽ nhận tín hiệu

  Serial.begin(9600);
  for(int i = 0; i<=6;i++){
   pinMode(qtro[i], INPUT);
  }
}
//--------------------------------------------------------------------

void loop() {
 // ktravatcan();
  //if(analogRead(qtro[1]) > 500 && analogRead(qtro[2]) > 500 && analogRead(qtro[3]) > 500&& analogRead(qtro[4]) > 500){
    //analogWrite(motor1,255);
    //analogWrite(motor2,255);
    //    isRun = false;
  // }else{
  //laydulieu();
    //    }
    //Serial.println("alo");
    //ktravatcan();
    laydulieu();
}
//--------------------------------------------------------------
void ktravatcan(){
  unsigned long duration; // biến đo thời gian
  int distance;           // biến lưu khoảng cách

  digitalWrite(trig,0);   // tắt chân trig
  delayMicroseconds(2);
  digitalWrite(trig,1);   // phát xung từ chân trig
  delayMicroseconds(5);   // xung có độ dài 5 microSeconds
  digitalWrite(trig,0);   // tắt chân trig
  duration = pulseIn(echo,HIGH);
  distance = int(duration/2/29.412);
  if(distance < 50){
     analogWrite(motor1,0);
     analogWrite(motor2,0);
     isRun = false;
  }else{
    laydulieu();
  }
}

//-------------------------------------------------------------------

int s = 60;
int a = 0;
int b = 0;
int pid;
void LinePIDFilter() { //PID control in line tracking mode
  static double kP = 36, kI = 0, kD = 0;
  static double gain = 100;
  static double prev_error = 0, prev_I = 0;
  double p = 0, i = 0, d = 0, pid_value;

  p = xulyloi();
  i = p + prev_I;
  d = p - prev_error;

  pid_value = kP * p + kI * i + kD * d;
  pid = pid_value;
  prev_I = i;
  prev_error = p;
  pid_value *= gain/100;

  float ratio=0;
  float x = 255+pid;
  float y = 255-pid;

  if(x > 255 || y > 255){
      if(x<y){
        ratio = 255/y;
      }else{
        ratio = 255/x;
      }
      x *= ratio;
      y *= ratio;
  }
 /*
  if(!isRun){
    isRun = true;
    timer = millis();
  }

    if(millis() - timer <= 1100){
    maxspeed = map(millis() - timer,0,1000,255,0);
  }

  constrain(maxspeed,0,255);
    */
  //Serial.println(maxspeed);
  //digital();
  if(!isRun){
  if(s <= 10){
        s = 0;
        isRun = true;
   }else{
  s = map(millis(),3000,6000,100,0);
  }
        }

  //GetSensorAnalog();
  //digital();
  a = (int)y;
  b = (int)x;
  constrain(a,0,255);
  constrain(b,0,255);
  //pidinfo(a,b,pid);
  analogWrite(motor1,a);
  analogWrite(motor2,b);
}

//----------Ktra cac kieu-------------------------------------------------------
void GetSensorAnalog() {
  for (int i = 0; i < 7; i++) {
    Serial.print(locqtro[i]);
    Serial.print("  ");
  }

  Serial.println();
}
void digital(){
  for(int i = 0; i<7;i++){
   Serial.print(ktraqtro[i]);
   Serial.print("   ");
  }
   Serial.println();
}

void analog(){
  for(int i = 0; i<7;i++){
   Serial.println(analogRead(qtro[i]));
  }
   Serial.println("=======");
}
void pidinfo(int x,int y,int pid){
  Serial.print("X: ");
  Serial.print(x);
  Serial.print("   ");
  Serial.print("Y: ");
  Serial.print(y);
  Serial.print("   ");
  Serial.print("Pid: ");
  Serial.print(pid);
  Serial.println();
}
//-------------------------------------------------------------------


void laydulieu(){

  //clear ktraqtro
  for(int i = 0; i<7;i++){
   ktraqtro[i] = 0;
  }


  //Lấy max,min
  int max = 0;
  int min = 0;


 locqtro[0] = analogRead(qtro[0])+110;
 locqtro[1] =analogRead(qtro[1])+44;
 locqtro[2] =analogRead(qtro[2])+20;
 locqtro[3] =analogRead(qtro[3])+41;
 locqtro[4] =analogRead(qtro[4])+44;
 locqtro[5] =analogRead(qtro[5])+135;
 locqtro[6] =analogRead(qtro[6])-320;


  for(int i = 0; i<=6;i++){
   if(locqtro[i] > max){
    max = locqtro[i];
   }
   else if(locqtro[i] < min){
    min = locqtro[i];
   }
  }

  //Lấy trb rồi gán digital từng cái
  int trungbinh = (min + max) /2;

  for(int i = 0; i<=6;i++){
   if(locqtro[i] < trungbinh){
    ktraqtro[i] = 1;
   }else{
    ktraqtro[i] = 0;
   };
  }

  //ktraqtro[6]=0;
  //Chuyển động
  LinePIDFilter();

}


//---------------------------------------------------------------------------------


bool Line[7] = {false};
int soluongqtrophiatrenline;
int xulyloi(){
 //Kiểm tra số quang trở phía trên line
    for(int i = 0; i<7;i++){
   Line[i] = false;
  }
    soluongqtrophiatrenline  =0;
 for(int i = 0; i<7;i++){
  if(ktraqtro[i] == 1){
   soluongqtrophiatrenline += 1;
  }
 }
 //Chuyển từ 0001000 sang false,true
 for(int i = 0; i<7;i++){
  if(ktraqtro[i] == 1){
    Line[i] = true;
  }
 }

 //Xác định lỗi
 static int prev_error = 0;
 switch(soluongqtrophiatrenline){
    case 0: {
        if (prev_error == 0) {
          return 0;
        } else if (prev_error == 6 || prev_error == 7) {
          prev_error = 7;
          return 7;
        } else if (prev_error == -6 || prev_error == -7) {
          prev_error = -7;
          return -7;
        }
        else return prev_error;
        break;
      }
    case 1: {
        for (int i = 0; i < 7; i++){
          if (Line[i]) {
            switch (i) {
              case 0: {
                  prev_error = -6;
                  return -6;
                  break;
                }
              case 1: {
                  prev_error = -4;
                  return -4;
                  break;
                }
              case 2: {
                  prev_error = -2;
                  return -2;
                  break;
                }
              case 3: {
                  prev_error = 0;
                  return 0;
                  break;
                }
              case 4: {
                  prev_error = 2;
                  return 2;
                  break;
                }
              case 5: {
                  prev_error = 4;
                  return 4;
                  break;
                }
              case 6: {
                  prev_error = 6;
                  return 6;
                  break;
                }
              default: {
                  return prev_error;
                  break;
                }
            }
            break;
          }
        }
        break;
      }
    case 2: {
        if (Line[0] && Line[1]) {
          prev_error = -5;
          return -5;
        } else if (Line[1] && Line[3]) {
          prev_error = -7;
          return -7;
        } else if (Line[1] && Line[2]) {
          prev_error = -3;
          return -3;
        } else if (Line[2] && Line[3]) {
          prev_error = -1;
          return -1;
        } else if (Line[3] && Line[4]) {
          prev_error = 1;
          return 1;
        } else if (Line[4] && Line[5]) {
          prev_error = 3;
          return 3;
        } else if (Line[5] && Line[3]) {
          prev_error = 7;
          return 7;
        } else if (Line[5] && Line[6]) {
          prev_error = 5;
          return 5;
        } else return prev_error;
        break;
      }
    case 3: {
        if (Line[1] && Line[2] && Line[3]) {
          switch (-1) {
            case 0: {
                prev_error = 0;
                return 0;
              }
            case -1: case 1: {
                prev_error = -4;
                return -4;
              }
          }
        } else if (Line[0] && Line[1] && Line[2]) {
          prev_error = -6;
          return -6;
        } else if (Line[3] && Line[4] && Line[5]) {
          switch (1) {
            case 0: {
                prev_error = 0;
                return 0;
              }
            case 1: case -1: {
                prev_error = 4;
                return 4;
              }
          }
        } else if (Line[4] && Line[5] && Line[6]) {
          prev_error = 6;
          return 6;
        } else if (Line[2] && Line[3] && Line[4]) {
          prev_error = 0;
          return 0;
        } else return prev_error;
        break;
      }
    case 4: {
        if (Line[0] && Line[1] && Line[2] && Line[3]) {
                prev_error = -8;
                return -8;
        }
        else if (Line[6] && Line[5] && Line[4] && Line[3]) {
                prev_error = 8;
                return 8;
        }
        else {
          return prev_error;
        }
        break;
      }
    case 5: {
        if (Line[0] && Line[1] && Line[2] && Line[3] && Line[4]) {
          prev_error = -2;
          return -2;
        } else if (Line[1] && Line[2] && Line[3] && Line[4] && Line[5]) {
          prev_error = 0;
          return 0;
        } else if (Line[2] && Line[3] && Line[4] && Line[5] && Line[6]) {
          prev_error = 2;
          return 2;
        }
      }
    case 6: {
      if (Line[0] && Line[1] && Line[2] && Line[3] && Line[4] && Line[5]) {
        prev_error = -1;
        return -1;
      } else if (Line[1] && Line[2] && Line[3] && Line[4] && Line[5] && Line[6]) {
        prev_error = 1;
        return 1;
      }
    }
    case 7: {
      prev_error = 0;
    }
 }
}