RobotCode

#include <driver/ledc.h>

// Піни моторів
#define ENA 14
#define IN1 27
#define IN2 26
#define ENB 32
#define IN3 25
#define IN4 33

// Піни сенсорів
#define L_SENSOR 5
#define C_SENSOR 17
#define R_SENSOR 4

// Швидкості
#define BASE_SPEED 160
#define TURN_SPEED 200
#define SEARCH_SPEED 180
#define PUSH_DURATION 180  // Тривалість пошуку

// PWM
#define LEDC_TIMER_NUM      LEDC_TIMER_0
#define LEDC_MODE           LEDC_LOW_SPEED_MODE
#define LEDC_CHANNEL_LEFT   LEDC_CHANNEL_0
#define LEDC_CHANNEL_RIGHT  LEDC_CHANNEL_1
#define PWM_FREQ            1000

bool isSearching = false;

void setup() {
  Serial.begin(115200);
  
  pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);
  pinMode(L_SENSOR, INPUT_PULLUP);
  pinMode(C_SENSOR, INPUT_PULLUP);
  pinMode(R_SENSOR, INPUT_PULLUP);

  ledc_timer_config_t timer_conf = {
    .speed_mode = LEDC_MODE,
    .duty_resolution = LEDC_TIMER_8_BIT,
    .timer_num = LEDC_TIMER_NUM,
    .freq_hz = PWM_FREQ,
    .clk_cfg = LEDC_AUTO_CLK
  };
  ledc_timer_config(&timer_conf);

  ledc_channel_config_t left_channel = {
    .gpio_num = ENA, .speed_mode = LEDC_MODE,
    .channel = LEDC_CHANNEL_LEFT, .intr_type = LEDC_INTR_DISABLE,
    .timer_sel = LEDC_TIMER_NUM, .duty = 0
  };
  ledc_channel_config(&left_channel);

  ledc_channel_config_t right_channel = {
    .gpio_num = ENB, .speed_mode = LEDC_MODE,
    .channel = LEDC_CHANNEL_RIGHT, .intr_type = LEDC_INTR_DISABLE,
    .timer_sel = LEDC_TIMER_NUM, .duty = 0
  };
  ledc_channel_config(&right_channel);

  setForward();
}

// --- Функції ---
void setSpeed(int left, int right) {
  ledc_set_duty(LEDC_MODE, LEDC_CHANNEL_LEFT, left);
  ledc_set_duty(LEDC_MODE, LEDC_CHANNEL_RIGHT, right);
  ledc_update_duty(LEDC_MODE, LEDC_CHANNEL_LEFT);
  ledc_update_duty(LEDC_MODE, LEDC_CHANNEL_RIGHT);
}

void setForward() {
  digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
}

void setBackward() {
  digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH);
}

void rotateLeft() {
  digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH);  // ліве колесо назад
  digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);  // праве колесо вперед
}

void rotateRight() {
  digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH);
}

void stopMotors() {
  setSpeed(0, 0);
}

void searchLine() {
  static bool turnLeftNext = true;
  if (turnLeftNext) {
    rotateLeft();
  } else {
    rotateRight();
  }
  setSpeed(SEARCH_SPEED, SEARCH_SPEED);
  delay(PUSH_DURATION);
  stopMotors();
  turnLeftNext = !turnLeftNext;
}

void loop() {
  int l = !digitalRead(L_SENSOR);
  int c = !digitalRead(C_SENSOR);
  int r = !digitalRead(R_SENSOR);

  Serial.print("Sensors: ");
  Serial.print(l); Serial.print(c); Serial.println(r);

  if (c) {
    isSearching = false;
    setForward();
    setSpeed(BASE_SPEED, BASE_SPEED);
  } 
  else if (l || r) {
    isSearching = false;
    if (l) {
      rotateLeft();
      setSpeed(TURN_SPEED, TURN_SPEED);
    } else if (r) {
      rotateRight();
      setSpeed(TURN_SPEED, TURN_SPEED);
    }
  } 
  else {
    if (!isSearching) {
      isSearching = true;
      stopMotors();
      delay(150);
    }
    searchLine();
  }

  delay(20);
}