///////////////////////////////////////////////////////////////////////////
//                     CODE XE DÒ LINE 2 BÁNH PID                        //
//          HÃY THAM GIA GROUP XE ĐUA TỰ HÀNH MCR TRÊN FACEBOOK          //
//                 HÃY CHIA SẼ VÀ GHI RÕ NGUỒN KHI THAM KHẢO             //
//                              CẢM ƠN BẠN                               //
//            https://www.facebook.com/groups/xedualaptrinhMCR           //
///////////////////////////////////////////////////////////////////////////
#include <avr/interrupt.h>
#include <EEPROM.h>
#define RDIR 6// chiều quay động cơ phải
#define LDIR 9// chiều quay động cơ trái
#define LPWM 10// tốc độ động cơ trái
#define RPWM 5// tốc độ động cơ phải
#define BUTTON1 7// nút nhấn 1
#define BUTTON2 8// nút nhấn 2
#define BUTTON3 12// nút nhấn 3
#define BUZZER 4// chân điều khiển loa
#define RED 3// Chân RED trên mạch
#define GREEN 2// Chân GREEN trên mạch
#define BLUE 13// Chân BLUE trên mạch
//---------------------------------------------------
int addr = 0;
volatile int lastPos;
volatile unsigned char isCalib = 0;
volatile int servoPwm;
volatile unsigned char sensor;
unsigned int sensorValue[8];
unsigned int sensorPID[8];
unsigned int black_value[8];
unsigned int white_value[8];
unsigned int compare_value[8];
int speed_run_forward;
int cnt = 0;
unsigned char pattern, start;
boolean debug = true;
uint32_t time = 0;
void setup() {


  //---------------PWM------------------------//
  pinMode(LDIR, OUTPUT);
  pinMode(RDIR, OUTPUT);
  pinMode(BUTTON1, INPUT_PULLUP);
  pinMode(BUTTON2, INPUT_PULLUP);
  pinMode(BUTTON3, INPUT_PULLUP);
  pinMode(RED, OUTPUT);
  pinMode(GREEN, INPUT);
  pinMode(BLUE, OUTPUT);
  digitalWrite(RDIR, LOW);
  digitalWrite(LDIR, LOW);
  digitalWrite(RED, 1);
  digitalWrite(GREEN, 1);
  digitalWrite(BLUE, 1);
  
  //-------------------------------------------------//
  //CÀI ĐẶT CÁC THÔNG SỐ BAN ĐẦU
  speed_run_forward = 0; //bạn thay đổi tốc độ ở đây, tốc độ chạy max 255, min 10
  speed_run(0, 0 );// dừng 2 bánh
  pattern = 10;// set trạng thái chạy
  start = 0;// Trước khi chạy học màu lại, đưa cảm biến qua lại trên đường line rồi bấm SW1 để lưu
  readEeprom();
  Serial.begin(9600);
  //-------------------------------------------------//
  for (int i = 2000; i < 3500; i += 500) {
    tone(BUZZER, i , 100);
    delay(100);
  }
  timer_init();
  isCalib = 1;
  pinMode(BUZZER, OUTPUT);
  //-------------------------------------------------//
}
void timer_init() {
  // Timer/Counter 2 initialization
  ASSR = (0 << EXCLK) | (0 << AS2);
  TCCR2A = (0 << COM2A1) | (0 << COM2A0) | (0 << COM2B1) | (0 << COM2B0) | (0 << WGM21) | (0 << WGM20);
  TCCR2B = (0 << WGM22) | (1 << CS22) | (1 << CS21) | (1 << CS20);
  TCNT2 = 0xB2;
  OCR2A = 0x00;
  OCR2B = 0x00;
  // Timer/Counter 2 Interrupt(s) initialization
  TIMSK2 = (0 << OCIE2B) | (0 << OCIE2A) | (1 << TOIE2);
}
ISR (TIMER2_OVF_vect) {
  TCNT2 = 0xB2;
  read_sensor();
  cnt++;
}
void loop()// vòng lặp, không dùng delay() trong này
{
  while (start == 0) // nhấn nút để chạy
  {
    updateLine(); // học màu, nhấn SW1 để lưu cảm biến
    if ((digitalRead(RED) && digitalRead(GREEN)) || !digitalRead(BUTTON2)) { // Nhấn SW2 để chạy
      start = 1; // học màu xong va bắt đầu chạy
      isCalib = 0;
      cnt = 0;
    }
  }

  switch ( pattern ) { // đoạn mã trạng thái máy
    case 10:
      if (cnt >= speed_run_forward) { // KHởi động mềm
        pattern = 11;
        cnt = 0;
        break;
      }
      runforwardline(cnt);
      break;
    case 11:
      runforwardline(speed_run_forward);
      break;
    case 100:
      runforwardline(0);
      break;
    default:
      pattern = 11;
      break;
  }
    if (debug && millis() - time >= 250) {
    time = millis();
    consolelog();
  }
}
//--------------------------------------------------------------------------//
void updateLine() {
  //Serial.println(sensor, BIN);
  for (int i = 0; i < 8; i++) {
    if (black_value[i] == 0) black_value[i] = 1100;
    if (sensorValue[i] < black_value[i]) black_value[i] = sensorValue[i];
    if (sensorValue[i] > white_value[i]) white_value[i] = sensorValue[i];
    compare_value[i] = (black_value[i] + white_value[i]) / 2;
  }
  if (digitalRead(BUTTON1) == 0) {
    for (int i = 0; i < 8; i++)
    {
      // compare_value[i] = (black_value[i] + white_value[i]) / 2;
      EEPROM.write(addr + i, compare_value[i] / 4);
    }
    digitalWrite(BUZZER, 1);
    delay(100);
    digitalWrite(BUZZER, 0);
  }
  switch (millis() / 100 % 3) {
    case 0:
      digitalWrite(RED, 1);
      digitalWrite(GREEN, 1);
      digitalWrite(BLUE, 0);
      break;
    case 1:
      digitalWrite(RED, 0);
      digitalWrite(GREEN, 1);
      digitalWrite(BLUE, 1);
      break;
    case 2:
      digitalWrite(RED, 1);
      digitalWrite(GREEN, 0);
      digitalWrite(BLUE, 1);
      break;
    default: break;
  }
}
void readEeprom() {
  for (int i = 0; i < 8; i++)
  {
    compare_value[i] = EEPROM.read(i) * 4;
  }
}
//-------------------------------------------------------------------//
void runforwardline (int tocdo)// hàm chạy bám line
{
  handleAndSpeed(servoPwm, tocdo);
}
//---------------------------------------------------------------------------//
void handleAndSpeed(int angle, int speed1)
{
  int speedLeft;
  int speedRight;
  speedLeft = speed1 + angle;
  speedRight = speed1 - angle;
  speed_run(speedLeft, speedRight);
}
//---------------------------------------------------------------------------//
void speed_run( int speedDC_left, int speedDC_right)// hàm truyền vào tốc độ động cơ trái + phải
{

  if (speedDC_left < 0)
  {
    analogWrite(LPWM, 255 + speedDC_left);
    digitalWrite(LDIR, HIGH);
  }
  else if (speedDC_left >= 0)
  {
    speedDC_left = speedDC_left;
    analogWrite(LPWM, speedDC_left);
    digitalWrite(LDIR, LOW);

  }
  if (speedDC_right < 0)
  {
    analogWrite(RPWM, 255 + speedDC_right);
    digitalWrite(RDIR, HIGH);
  }
  else if (speedDC_right >= 0)
  {
    speedDC_right = speedDC_right;
    analogWrite(RPWM, speedDC_right);
    digitalWrite(RDIR, LOW);
  }

}
//-----------------------------------------------------------//
void read_sensor() // hàm đọc cảm biến
{
  unsigned char temp = 0;
  unsigned int sum = 0;
  unsigned long avg = 0;
  int i, iP, iD;
  int kp;
  int kd;
  int iRet;
  sensorValue[0] = analogRead(A0);
  sensorValue[1] = analogRead(A1);
  sensorValue[2] = analogRead(A2);
  sensorValue[3] = analogRead(A3);
  sensorValue[4] = analogRead(A4);
  sensorValue[5] = analogRead(A5);
  sensorValue[6] = analogRead(A6);
  sensorValue[7] = analogRead(A7);
  for (int j = 0; j < 8; j++)
  {
    if (  isCalib == 0) {
      if (sensorValue[j] < black_value[j])
        sensorValue[j] = black_value[j];
      if (sensorValue[j] > white_value[j])
        sensorValue[j] = white_value[j];
      sensorPID[j] = map(sensorValue[j], black_value[j], white_value[j], 0, 1000);
    }
    temp = temp << 1;
    if (sensorValue[j] > compare_value[j])
    {
      temp |= 0x01;
    }
    else
    {
      temp &= 0xfe;
    }
    sensor = temp;
  }
  for (int j = 0; j < 8; j++)
  {
    avg += (long)(sensorPID[j]) * ((j) * 1000);
    sum += sensorPID[j];
  }
  i = (int)((avg / sum) - 3500);
  kp = 1;
  kd = 1;
  iP = kp * i;
  iD = kd * (lastPos - i);
  iRet = (iP - iD);
  if ((iRet < -4500))
  {
    iRet = 0;
  }
  servoPwm = iRet / 20;
  lastPos = i;
}
void consolelog(){
  Serial.print("Input value: ");
  for (int i = 0; i < 8; i++){
  // Print the value to the serial monitor
  Serial.print(",");
  Serial.print(sensorValue[i]);
  }
  Serial.print("; ");
  Serial.print(servoPwm);
  Serial.println();
  
}