Black Line GPT

// определяем пины для датчиков
const int SENSOR_1_PIN = A0;
const int SENSOR_2_PIN = A1;
const int SENSOR_3_PIN = A2;
const int SENSOR_4_PIN = A3;
const int SENSOR_5_PIN = A4;

// определяем пины для двигателей
// 0 - скорость
const int MOTOR_LEFT_0_PIN = 5;
const int MOTOR_LEFT_1_PIN = 6;
const int MOTOR_LEFT_2_PIN = 7;
const int MOTOR_RIGHT_0_PIN = 10;
const int MOTOR_RIGHT_1_PIN = 8;
const int MOTOR_RIGHT_2_PIN = 9;

// определяем скорость двигателей
const int MOTOR_SPEED = 50;
const float MOTOR_SPEED_RATE = 0.13;
const int MOTOR_SPEED_HARD_TURN = 60;
const int MOTOR_SPEED_LIGHT_TURN = MOTOR_SPEED + 5;

// считываем состояние датчиков
int sensor1 = 0;
int sensor2 = 0;
int sensor3 = 0;
int sensor4 = 0;
int sensor5 = 0;


void Forward() {
    //Left
    analogWrite(MOTOR_LEFT_0_PIN, MOTOR_SPEED);

    //Right
    analogWrite(MOTOR_RIGHT_0_PIN, MOTOR_SPEED);
    Serial.println("Forward");
}

int RightCircle() {
    //Left
    analogWrite(MOTOR_LEFT_0_PIN, MOTOR_SPEED_LIGHT_TURN);

    //Right
    analogWrite(MOTOR_RIGHT_0_PIN, MOTOR_SPEED * MOTOR_SPEED_RATE);

    Serial.println("RightCircle");
}

int LeftCircle() {
    //Left
    analogWrite(MOTOR_LEFT_0_PIN, MOTOR_SPEED * MOTOR_SPEED_RATE);

    //Right
    analogWrite(MOTOR_RIGHT_0_PIN, MOTOR_SPEED_LIGHT_TURN);

    Serial.println("LeftCircle");
}

int Right() {
    //Left
    analogWrite(MOTOR_LEFT_0_PIN, MOTOR_SPEED_HARD_TURN);

    //Right
    analogWrite(MOTOR_RIGHT_0_PIN, 0);

    Serial.println("Right");
}

int Left() {
    //Left
    analogWrite(MOTOR_LEFT_0_PIN, 0);

    //Right
    analogWrite(MOTOR_RIGHT_0_PIN, MOTOR_SPEED_HARD_TURN);

    Serial.println("Left");
}

void ReadSensors() {
  // считываем состояние датчиков
  sensor1 = digitalRead(SENSOR_1_PIN);
  sensor2 = digitalRead(SENSOR_2_PIN);
  sensor3 = digitalRead(SENSOR_3_PIN);
  sensor4 = digitalRead(SENSOR_4_PIN);
  sensor5 = digitalRead(SENSOR_5_PIN);
}

int config(int sensor1, int sensor2, int sensor3, int sensor4, int sensor5) {
  sensor1 = sensor1 * -1;

  sensor2 = sensor2 * -1;

  int position = sensor1 + sensor2 + sensor4 + sensor5;

  Serial.println(position);

  return position;
}

void setup() {
  // настраиваем пины для датчиков
  pinMode(SENSOR_1_PIN, INPUT);
  pinMode(SENSOR_2_PIN, INPUT);
  pinMode(SENSOR_3_PIN, INPUT);
  pinMode(SENSOR_4_PIN, INPUT);
  pinMode(SENSOR_5_PIN, INPUT);

  // настраиваем пины для двигателей
  pinMode(MOTOR_LEFT_0_PIN, OUTPUT);
  pinMode(MOTOR_LEFT_1_PIN, OUTPUT);
  pinMode(MOTOR_LEFT_2_PIN, OUTPUT);
  pinMode(MOTOR_RIGHT_0_PIN, OUTPUT);
  pinMode(MOTOR_RIGHT_1_PIN, OUTPUT);
  pinMode(MOTOR_RIGHT_2_PIN, OUTPUT);

  // начинаем движение вперед
  analogWrite(MOTOR_LEFT_0_PIN, MOTOR_SPEED);
  digitalWrite(MOTOR_LEFT_1_PIN, LOW);
  digitalWrite(MOTOR_LEFT_2_PIN, HIGH);

  analogWrite(MOTOR_RIGHT_0_PIN, MOTOR_SPEED);
  digitalWrite(MOTOR_RIGHT_1_PIN, HIGH);
  digitalWrite(MOTOR_RIGHT_2_PIN, LOW);

  Serial.begin(9600);
}

void loop() {

  ReadSensors();

  int position = config(sensor1, sensor2, sensor3, sensor4, sensor5)

  if (position == 0) {
    Forward();
  }
  else if (position == 2) {
    Left();
  }
  else if (position == 1) {
    LeftCircle();
  }
  else if (position == -1) {
    RightCircle();
  }
  else if (position == -2) {
    Right();
  }
  else {
    Forward();
  }
  //delay(100);
}