//#define SERVO_TEST
#define MOTOR_TEST
//#define ENCODER_TEST
//#define SONAR_TEST

#define ENC_1_PIN 14
#define ENC_2_PIN 15
#define TRIG_PIN 11
#define ECHO_PIN 12

#ifdef SERVO_TEST
#include <Servo.h>
Servo myservo;
#endif

#ifdef SONAR_TEST
uint16_t sonar() {
  digitalWrite(TRIG_PIN, 1);
  delayMicroseconds(5);
  digitalWrite(TRIG_PIN, 0);

  uint32_t duration = pulseIn(ECHO_PIN, HIGH) / 58;
  return duration;
}

void sonarInit() {
  pinMode(TRIG_PIN, 1);
  pinMode(ECHO_PIN, 0);
}
#endif

#ifdef ENCODER_TEST
volatile int enc1 = 0;
volatile int enc2 = 0;

void encoder1() {
  if (digitalRead(ENC_1_PIN))
  enc1++;
  else
  enc1--;
}

void encoder2() {
  if (!digitalRead(ENC_2_PIN))
  enc2++;
  else
  enc2--;
}
#endif

void setup()
{
  Serial.begin(9600);
  //КОНФИГУРАЦИЯ ВЫХОДОВ ДЛЯ УПРАВЛЕНИЯ НАПРЯВЛЕНИЕМ ВРАЩЕНИЯ ДВИГАТЕЛЕЙ
  motorInit();

#ifdef SERVO_TEST //ИНИЦИАЛИЗАЦИЯ И ПРОВЕРКА СЕРВОПРИВОДА
  Serial.print("SERVO_TEST...\n");
  myservo.attach(9);
  delay(3500);
  myservo.write(0);
  delay(3500);
  myservo.write(180);
  delay(3500);
  myservo.write(90);
#endif

#ifdef SONAR_TEST
  sonarInit();
#endif

#ifdef ENCODER_TEST
  encoderInit();
#endif

#ifdef MOTOR_TEST //ПРОВЕРКА ДВИГАТЕЛЕЙ

  uint32_t motorTestTime = millis();
  while (true) {
  if (millis() - motorTestTime > 7000)
  break;
  else if (millis() - motorTestTime > 5500)
  motor(256, 256);
  else if (millis() - motorTestTime > 4000)
  motor(-256, -256);
  else if (millis() - motorTestTime > 3000)
  motor(0, -256);
  else if (millis() - motorTestTime > 2000)
  motor(-256, 0);
  else if (millis() - motorTestTime > 1000)
  motor(0, 256);
  else if (millis() - motorTestTime < 1000)
  motor(256, 0);

#ifdef ENCODER_TEST
  Serial.print("enc1 = ");
  Serial.print(enc1);
  Serial.print("\tenc2 = ");
  Serial.println(enc2);
#endif
  }
  motor(0, 0);
#endif
}

void loop()
{
#ifndef SONAR_TEST
#ifndef ENCODER_TEST
  for (int i = 0; i < 4; i++)
  {
  Serial.print("A");
  Serial.print(i);
  Serial.print(" = ");
  Serial.print(analogRead(i));
  Serial.print("\t");
  }
#endif
#endif

#ifdef SONAR_TEST
  Serial.print("sonar() = ");
  Serial.print(sonar());
  Serial.print(" cm");
  Serial.print("\t");
#endif

#ifdef ENCODER_TEST
  Serial.print("enc1 = ");
  Serial.print(enc1);
  Serial.print("\tenc2 = ");
  Serial.print(enc2);
#endif
  Serial.print("\n");
}