#include <Arduino.h>

const byte AIN1 = 12; // Лівий мотор: вперед
const byte AIN2 = 11; // Лівий мотор: назад
const byte PWMA = 10; // Швидкість лівого мотора
const byte BIN1 = 5; // Правий мотор: вперед
const byte BIN2 = 4; // Правий мотор: назад
const byte PWMB = 3; // Швидкість правого мотора
const bool LEFT_REVERSED = false; // true, якщо ліві колеса крутяться назад
const bool RIGHT_REVERSED = false; // true, якщо праві колеса крутяться назад
const int MAX_SPEED = 230;
const int SPEED = 170;
const unsigned long START_DELAY_MS = 3000; // Час прибрати руки після ввімкнення
const unsigned long TURN_MS = 600;
const unsigned long PAUSE_MS = 500;

void setMotor(byte in1, byte in2, byte pwm, int speed) {
  speed = constrain(speed, -MAX_SPEED, MAX_SPEED);

  if (speed > 0) {
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
  } else if (speed < 0) {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
  } else {
    digitalWrite(in1, LOW);
    digitalWrite(in2, LOW);
  }

  analogWrite(pwm, abs(speed)); // Знак — напрямок, число — швидкість
}

void drive(int left, int right) {
  setMotor(AIN1, AIN2, PWMA, LEFT_REVERSED ? -left : left);
  setMotor(BIN1, BIN2, PWMB, RIGHT_REVERSED ? -right : right);
}

void move(int left, int right, unsigned long ms) {
  drive(left, right);
  delay(ms);
  drive(0, 0);
  delay(PAUSE_MS);
}

void setup() {
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);
  drive(0, 0);
  Serial.begin(115200);
  Serial.println("Start in 3 s");
  delay(START_DELAY_MS);
  Serial.println("Forward");
  move(SPEED, SPEED, 1500);
  Serial.println("Backward");
  move(-SPEED, -SPEED, 1000);
  Serial.println("Spin left");
  move(-SPEED, SPEED, TURN_MS);
  Serial.println("Done");
}

void loop() {
}