Labyrinth Quant

// определяем пины для двигателей
// 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 BUZZER_PIN = 4;

const int MOTOR_SPEED = 80;
const float MOTOR_SPEED_RATE = 0.6;
const int MOTOR_SPEED_LIGHT_TURN = MOTOR_SPEED - 5;
const int MOTOR_SPEED_HARD_TURN_HIGH = MOTOR_SPEED + 5;
const int MOTOR_SPEED_HARD_TURN_LOW = 35;

const int pinSensorA5 = A5; //Left
const int pinSensorA6 = A6; //Center
const int pinSensorA7 = A7; //Right

//Пороговые значения для лабиринта
const int thresholdLeft = 100;
const int thresholdCenter = 110;
const int thresholdRight = 100;

//Значения при которых нужно отехать назад
const int STUCK_IN_WALL = 74;

int sensorA5;
int sensorA6;
int sensorA7;

void Forward() {
  //Left
  analogWrite(MOTOR_LEFT_0_PIN, MOTOR_SPEED);
  digitalWrite(MOTOR_LEFT_1_PIN, LOW);
  digitalWrite(MOTOR_LEFT_2_PIN, HIGH);

  //Right
  analogWrite(MOTOR_RIGHT_0_PIN, MOTOR_SPEED);
  digitalWrite(MOTOR_RIGHT_1_PIN, HIGH);
  digitalWrite(MOTOR_RIGHT_2_PIN, LOW);
  Serial.println("Forward");
}

void Stop() {
  //Left
  analogWrite(MOTOR_LEFT_0_PIN, 0);

  //Right
  analogWrite(MOTOR_RIGHT_0_PIN, 0);
  Serial.println("Stop");
}

void Backward() {
  //Left
  analogWrite(MOTOR_LEFT_0_PIN, MOTOR_SPEED);
  digitalWrite(MOTOR_LEFT_1_PIN, HIGH);
  digitalWrite(MOTOR_LEFT_2_PIN, LOW);

  //Right
  analogWrite(MOTOR_RIGHT_0_PIN, MOTOR_SPEED);
  digitalWrite(MOTOR_RIGHT_1_PIN, LOW);
  digitalWrite(MOTOR_RIGHT_2_PIN, HIGH);
  Serial.println("Forward");
}

int RightCircle() {
  //Left
  analogWrite(MOTOR_LEFT_0_PIN, MOTOR_SPEED_LIGHT_TURN);
  digitalWrite(MOTOR_LEFT_1_PIN, LOW);
  digitalWrite(MOTOR_LEFT_2_PIN, HIGH);

  //Right
  analogWrite(MOTOR_RIGHT_0_PIN, MOTOR_SPEED * MOTOR_SPEED_RATE);
  digitalWrite(MOTOR_RIGHT_1_PIN, HIGH);
  digitalWrite(MOTOR_RIGHT_2_PIN, LOW);

  Serial.println("RightCircle");
}

int LeftCircle() {
  //Left
  analogWrite(MOTOR_LEFT_0_PIN, MOTOR_SPEED * MOTOR_SPEED_RATE);
  digitalWrite(MOTOR_LEFT_1_PIN, LOW);
  digitalWrite(MOTOR_LEFT_2_PIN, HIGH);

  //Right
  analogWrite(MOTOR_RIGHT_0_PIN, MOTOR_SPEED_LIGHT_TURN);
  digitalWrite(MOTOR_RIGHT_1_PIN, HIGH);
  digitalWrite(MOTOR_RIGHT_2_PIN, LOW);

  Serial.println("LeftCircle");
}

int Right() {
  //Left
  analogWrite(MOTOR_LEFT_0_PIN, MOTOR_SPEED_HARD_TURN_HIGH);
  digitalWrite(MOTOR_LEFT_1_PIN, LOW);
  digitalWrite(MOTOR_LEFT_2_PIN, HIGH);

  //Right
  analogWrite(MOTOR_RIGHT_0_PIN, MOTOR_SPEED_HARD_TURN_LOW);
  digitalWrite(MOTOR_RIGHT_1_PIN, HIGH);
  digitalWrite(MOTOR_RIGHT_2_PIN, LOW);

  Serial.println("Right");
}

int Left() {
  //Left
  analogWrite(MOTOR_LEFT_0_PIN, MOTOR_SPEED_HARD_TURN_LOW);
  digitalWrite(MOTOR_LEFT_1_PIN, LOW);
  digitalWrite(MOTOR_LEFT_2_PIN, HIGH);

  //Right
  analogWrite(MOTOR_RIGHT_0_PIN, MOTOR_SPEED_HARD_TURN_HIGH);
  digitalWrite(MOTOR_RIGHT_1_PIN, HIGH);
  digitalWrite(MOTOR_RIGHT_2_PIN, LOW);

  Serial.println("Left");
}

void sensorRead() {
  sensorA5 = analogRead(pinSensorA5);
  sensorA6 = analogRead(pinSensorA6);
  sensorA7 = analogRead(pinSensorA7);

  Serial.print("Left: "); Serial.print(sensorA5); Serial.println(" ");
  Serial.print("Center: "); Serial.print(sensorA6); Serial.println(" ");
  Serial.print("RIght: "); Serial.print(sensorA7); Serial.println(" ");
}

int config(int Left, int Right)
{
  Left = Left * -1;
  return Right - Left;
}

void setup() {

  //Left
  digitalWrite(MOTOR_LEFT_1_PIN, LOW);
  digitalWrite(MOTOR_LEFT_2_PIN, HIGH);

  //Right
  digitalWrite(MOTOR_RIGHT_1_PIN, HIGH);
  digitalWrite(MOTOR_RIGHT_2_PIN, LOW);

  tone(BUZZER_PIN, 1000);
  digitalWrite(BUZZER_PIN, HIGH);
  delay(500);
  noTone(BUZZER_PIN);
  digitalWrite(BUZZER_PIN, LOW);
  delay(500);
  sensorRead();
  delay(1000);
  tone(BUZZER_PIN, 500);
  digitalWrite(BUZZER_PIN, HIGH);
  delay(500);
  noTone(BUZZER_PIN);
  digitalWrite(BUZZER_PIN, LOW);

  Serial.begin(9600);
}

void loop() {
  sensorRead();

  //  delay(1000);

  if (sensorA5 < sensorA7 && sensorA6 < thresholdCenter)
  {
    LeftCircle();
  }
  else if (sensorA5 < sensorA7 && sensorA6 > thresholdCenter)
  {
    Left();
  }
  else if (sensorA5 > sensorA7 && sensorA6 < thresholdCenter)
  {
    RightCircle();
  }
  else if (sensorA5 > sensorA7 && sensorA6 > thresholdCenter)
  {
    Right();
  }
  else
  {
    Forward();
  }

  if (sensorA5 > STUCK_IN_WALL || sensorA6 > STUCK_IN_WALL || sensorA7 > STUCK_IN_WALL)
  {
    Backward();
  }
}