#include <Servo.h>

// константы для задания
#define timeForward 1   // время движения вперед
#define timeBack 1      // время движения назад
#define lenForward 300  // дистанция вперед мм
#define lenBack 300     // дистанция назад мм

// основные константы устройства
#define wheelDiameter 67              // диаметр колеса в мм
#define distBetWheel 140              // растояние между колес (центральными линиями шин) мм
#define wheelLen M_PI* wheelDiameter  // длинна окружности колеса

// пины управления моторами
#define leftDirPin 7     // направление вращения левого мотора (0, 1)
#define leftSpeedPin 6   // скорость вращения левого мотора (0 - 255)
#define rightSpeedPin 5  // скорость вращения правого мотора (0 - 255)
#define rightDirPin 4    // направление вращения правого мотора (0, 1)

// пины подключения энкодеров
#define leftTicksPin 2   // левый датчик энкодера
#define rightTicksPin 3  // правый датчик энкодера

//
#define left_line_pin A1;
#define right_line_pin A0;

// кнопка переключения этапов
#define keyOnPin A5
Servo Gor_servo;
// Счетчики тиков энкодера
volatile long CTLeft = 0;    // левый
volatile long CTRight = 0;   // правый
volatile long CTLeft1 = 0;   // левый для коррекции
volatile long CTRight1 = 0;  // правый для коррекции

// набор вспомогательных переменных
unsigned long startTime = 0;  // для времени

int oldCT = 0;         // для растояния
int deltaTime = 1500;  // время для расчета скорости Speed()
int stage = 0;         // в этой переменной мы фиксируем номер этапа, изменяется по отпускании кнопки keyOnPin
bool p_key = false;    // фиксируем событие нажатие кнопки keyOnPin
long d_error = 0;      // для ПИД регулятора, диференциальная ошибка
int distant = 0;
// начальная скорость моторов
int LS = 90;  // левый
int RS = 90;  // правый

// функции подсчета тиков на энкодерах, работают по прерываниям
void int_CTleft() {
  CTLeft++;
  CTLeft1++;
}
void int_CTRight() {
  CTRight++;
  CTRight1++;
}

void setup() {
  Serial.begin(9600);
  /* включаем работу по прерываниям от энкодеров
  igitalPinToInterrupt(leftTicksPin) - на каком пине ждем прерывания
  int_CTleft - запустить эту функцию если прерывание есть
  RISING (рост) - срабатывает при изменении сигнала с LOW на HIGH
  FALLING (падение) - срабатывает при изменении сигнала с HIGH на LOW 
  **CHANGE (изменение) - срабатывает при изменении сигнала (с LOW на HIGH и наоборот)
  LOW (низкий) - срабатывает постоянно при сигнале LOW (не поддерживается на ESP8266)
  */
  attachInterrupt(digitalPinToInterrupt(leftTicksPin), int_CTleft, CHANGE);
  attachInterrupt(digitalPinToInterrupt(rightTicksPin), int_CTRight, CHANGE);

  pinMode(keyOnPin, INPUT_PULLUP);  // кнопка этапов
  pinMode(leftDirPin, OUTPUT);
  pinMode(leftSpeedPin, OUTPUT);  // подключаем пины левого мотора
  pinMode(rightDirPin, OUTPUT);
  pinMode(rightSpeedPin, OUTPUT);  // подключаем пины правого мотора

  pinMode(leftTicksPin, INPUT_PULLUP);
  pinMode(rightTicksPin, INPUT_PULLUP);
  Gor_servo.attach(10);

  Gor_servo.write(90);  // установим направление для ультразвукового дальномера прямо вперед
  delay(500);

  Serial.println(wheelLen);
}

int Dist() {  // функция возвращает растояние до объекта в сантиметрах
  pinMode(8, INPUT);
  pinMode(9, OUTPUT);
  digitalWrite(9, 0);
  delayMicroseconds(3);
  digitalWrite(9, 1);
  delayMicroseconds(10);
  digitalWrite(9, 0);
  int d = pulseIn(8, 1);
  return d / 58;
}

void Go(int speedLeft, int speedRight) {  // функция движения машинки
  // устанавливаем направление вращения моторов
  digitalWrite(leftDirPin, (speedLeft > 0));
  digitalWrite(rightDirPin, (speedRight > 0));
  // устанавливаем скорости вращения моторов
  analogWrite(leftSpeedPin, abs(speedLeft));
  analogWrite(rightSpeedPin, abs(speedRight));
  delay(5);
}
void Stop() {  // функция безинерционной остановки моторов
  digitalWrite(leftDirPin, !digitalRead(leftDirPin));
  digitalWrite(rightDirPin, !digitalRead(rightDirPin));

  analogWrite(leftSpeedPin, 255);  //максимальная сигнал на моторы в течении 20 миллисекунд
  analogWrite(rightSpeedPin, 255);
  delay(20);

  analogWrite(leftSpeedPin, 0);  // обнуляем подачу сигнала на моторы
  analogWrite(rightSpeedPin, 0);
}


// функция движения по времени
void Go_time(int sTime, bool dir) {    // time - время работы моторов в секундах, dir - направление вращения моторов
  unsigned long curTime = millis();    // фиксируем время вхождения в цикл работы моторов
  unsigned long startTime = millis();  // фиксируем время вхождения в цикл работы моторов
  int DT = sTime * 1000;               // переводим секунды в миллисекунды

  // в зависимости от dir (1 - вперед, 0 - назад)
  int flag = 1;
  if (!dir) {
    LS = -LS;
    RS = -RS;
    flag = -1;
  }

  CTLeft1 = 0; // обнуляем колличество тиков энкодера для определения разницы в скорости вращения каждого мотора
  CTRight1 = 0;  // обнуляем колличество тиков энкодера для определения разницы в скорости вращения каждого мотора
  Serial.println((String)curTime + " " + startTime);
  unsigned long curentTime = millis();
  unsigned long startTimeTest = millis();
  while ((curTime - startTime) <= DT) {       // пока разница от текущего времени и времени старта
    int delta = (CTLeft1 - CTRight1) * flag;  // расчитываем разницу тиков левого и правого энкодера
    RS = RS + delta * 5;                      // изменяем скорость вращения правого мотора, 6 коэфицент пропорционального регулирования
    Go(LS, RS);                               // обращаемся к функции Go

    // каждые 50 миллисекунд обнуляем счетчики тиков на моторах
    curentTime = millis();
    if ((curentTime - startTimeTest) >= 10) {
      CTLeft1 = 0;
      CTRight1 = 0;
      startTimeTest = curentTime;
    }
    curTime = millis();
  }
  startTime = curTime;
  Stop();
}

// функция движения на расстояние 
void Go_Len(int len, bool dir) {  // len - растояние на которое необходимо проехать, dir - направление вращения моторов

  // расчитываем сколько тиков сделает энкодер для прохождения необходимоно расстояния
  float lens = (float(len) / float(wheelLen)) * 40;  // 40 - число изменений состояния сигнала с энкодера на 1 оборот колеса
  int flag = 1;
  if (!dir) {
    LS = -LS;
    RS = -RS;
    flag = -1;
  }
  CTLeft = 0;
  CTLeft1 = 0;
  CTRight1 = 0;  // обнуляем колличество тиков энкодера для определения разницы в скорости вращения каждого мотора

  unsigned long curentTime = millis();
  unsigned long startTimeTest = millis();
  while (CTLeft <= lens) {                    // пока не проехали нужное расстояние, делаем...
    int delta = (CTLeft1 - CTRight1) * flag;  // расчитываем разницу тиков левого и правого энкодера
    RS = RS + delta * 6;                      // изменяем скорость вращения правого мотора, 6 коэфицент пропорционального регулирования
    // каждые 50 миллисекунд обнуляем счетчики тиков на моторах
    curentTime = millis();
    if ((curentTime - startTime) >= 30) {
      CTLeft1 = 0;
      CTRight1 = 0;
      startTime = curentTime;
    }

    Go(LS, RS);  // обращаемся к функции Go
  }
  Stop();
}

// Функция выполнения танкового поворота
void Go_Angl_Tank(float angle) {  // танковый поворот на определенный градус
  bool flag = 1;
  if (angle < 0) {
    RS = -RS;
    angle = -angle;
  } else {
    LS = -LS;
  }
  //расчет сколько нужно тиков для поворота на заданый угол angle
  float len_a = (2 * M_PI * (distBetWheel / 2) * angle) / 360;
  // расчитываем сколько тиков сделает энкодер для прохождения необходимоно расстояния колесом
  Serial.println((String) "ugol   " + len_a);
  float lens = (float(len_a) / float(wheelLen)) * 38;  // 40 - число изменений состояния сигнала с энкодера на 1 оборот колеса
  Serial.println((String)lens);
  CTLeft = 0;
  CTRight = 0;  // обнуляем колличество тиков энкодера для определения пройденного расстояния всего левым колесом

  while ((CTLeft + CTRight) / 2 <= lens) {  // пока не проехали нужное расстояние, делаем...
    Go(LS, RS);                             // обращаемся к функции Go
  }
  Stop();
}

// функция поворота вокруг колеса
void Go_Angl_Gross(float angle) {  //расчет сколько нужно тиков для поворота на заданый угол angle

  bool flag = 1;
  if (angle < 0) {
    RS = 0;
  } else {
    LS = 0;
  }

  float len_a = (M_PI * distBetWheel * abs(angle)) / 360;
  // расчитываем сколько тиков сделает энкодер для прохождения необходимоно расстояния колесом

  float lens = (float(len_a) / float(wheelLen)) * 38;  // 40 - число изменений состояния сигнала с энкодера на 1 оборот колеса

  CTLeft = 0;
  CTRight = 0;  // обнуляем колличество тиков энкодера для определения пройденного расстояния всего левым колесом

  while ((CTLeft + CTRight) / 2 <= lens) {  // пока не проехали нужное расстояние, делаем...
    Go(LS, RS);                             // обращаемся к функции Go
  }
  Stop();
}

void Line_Stop() {
}


// остановка на растоянии от препятствия
void Go_dist(int dist) {
  while (Dist() > dist) {
    Go(LS, RS);
  }
  Stop();
}

void loop() {

  bool key1 = !digitalRead(keyOnPin);
  delay(20);

  if (key1 && !p_key) { p_key = true; }  // кнопку нажали но не отпустили (ничего не делаем)

  if (!key1 && p_key) {  // кнопку отпустили, и тут начинаем работать
    p_key = false;
    stage++;  // счетчик отпускания кнопки и в этой переменной храниться номер этапа который будем выполнять
    CTLeft = 0;
    CTRight = 0;
    LS = 70;  
    RS = 70;
    switch (stage) {
      case 0:
        break;

      case 1: 
        Go_dist(20);

        break;

      case 2:  
        Go_Angl_Tank(-90);

        break;

      case 3:  
 
        Go_Len(lenForward, 1);

        break;

      case 4:  
        Go_time(timeBack, 0);
        //Go_Len(lenBack, 0);

        break;

      case 5:  
        Go_dist(20);
        break;

      case 6:  //
        stage = 0;
        break;
    }
    Serial.println((String)LS + " " + RS);
  }
}