5-th sensors ESP32 Robot code

#include <driver/ledc.h>

// Піни
#define IN1 14
#define IN2 27
#define IN3 26
#define IN4 25
#define ENA 32
#define ENB 33
int leftiy = 200;
int rightiy = 200;
int backdiy = 125;
int fordiy = 175;
int timeiy = 0;

int strongLeft = 215;     // Ìàêñèìàëüíî ð³çêî ë³âîðó÷
int softLeft = 180;       // Ì'ÿêèé ïîâîðîò ë³âîðó÷
int strongRight = 215;    // Ìàêñèìàëüíî ð³çêî ïðàâîðó÷
int softRight = 180;      // Ì'ÿêèé ïîâîðîò ïðàâîðó÷
int fastForward = 175;    // Âïåâíåíî âïåðåä
int slowForward = 140;    // Îáåðåæíî âïåðåä
// Ñåíñîðè
#define LL_SENSOR 23  // Êðàéí³é ë³âèé
#define L_SENSOR  5   // ˳âèé
#define C_SENSOR  17  // Öåíòðàëüíèé
#define R_SENSOR  4   // Ïðàâèé
#define RR_SENSOR 19  // Êðàéí³é ïðàâèé

// PWM êàíàëè
ledc_channel_t LEFT_CHANNEL = LEDC_CHANNEL_0;
ledc_channel_t RIGHT_CHANNEL = LEDC_CHANNEL_1;

int lastTurnDirection = 0; // -1 = ë³âî, 1 = ïðàâî
const int impulseDuration = timeiy; // òðèâàë³ñòü ïîòóæíîãî ïîâîðîòó (ìñ)
unsigned long lastActionTime = 0;

void setup() {
  pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);

  pinMode(LL_SENSOR, INPUT);
  pinMode(L_SENSOR, INPUT);
  pinMode(C_SENSOR, INPUT);
  pinMode(R_SENSOR, INPUT);
  pinMode(RR_SENSOR, INPUT);

  ledc_timer_config_t timer_conf = {
    .speed_mode = LEDC_LOW_SPEED_MODE,
    .duty_resolution = LEDC_TIMER_8_BIT,
    .timer_num = LEDC_TIMER_0,
    .freq_hz = 1000,
    .clk_cfg = LEDC_AUTO_CLK
  };
  ledc_timer_config(&timer_conf);

  ledc_channel_config_t left_ch = {
    .gpio_num = ENA,
    .speed_mode = LEDC_LOW_SPEED_MODE,
    .channel = LEFT_CHANNEL,
    .intr_type = LEDC_INTR_DISABLE,
    .timer_sel = LEDC_TIMER_0,
    .duty = 0
  };
  ledc_channel_config(&left_ch);

  ledc_channel_config_t right_ch = {
    .gpio_num = ENB,
    .speed_mode = LEDC_LOW_SPEED_MODE,
    .channel = RIGHT_CHANNEL,
    .intr_type = LEDC_INTR_DISABLE,
    .timer_sel = LEDC_TIMER_0,
    .duty = 0
  };
  ledc_channel_config(&right_ch);
}

void setMotorSpeed(ledc_channel_t channel, int speed) {
  speed = constrain(speed, 0, 255);
  ledc_set_duty(LEDC_LOW_SPEED_MODE, channel, speed);
  ledc_update_duty(LEDC_LOW_SPEED_MODE, channel);
}

// Íàïðÿìêè
void forward(int speed = fordiy) {
  digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
  setMotorSpeed(LEFT_CHANNEL, speed);
  setMotorSpeed(RIGHT_CHANNEL, speed);
}
void backward(int speed = backdiy) {
  digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH);
  setMotorSpeed(LEFT_CHANNEL, speed);
  setMotorSpeed(RIGHT_CHANNEL, speed);
}
void stopMotors() {
  setMotorSpeed(LEFT_CHANNEL, 0);
  setMotorSpeed(RIGHT_CHANNEL, 0);
}

void turnLeft(int speed = leftiy) {
  digitalWrite(IN1, LOW); digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH); digitalWrite(IN4, LOW);
  setMotorSpeed(LEFT_CHANNEL, speed);
  setMotorSpeed(RIGHT_CHANNEL, speed);
}

void turnRight(int speed = rightiy) {
  digitalWrite(IN1, HIGH); digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW); digitalWrite(IN4, HIGH);
  setMotorSpeed(LEFT_CHANNEL, speed);
  setMotorSpeed(RIGHT_CHANNEL, speed);
}

void loop() {
  // Ç÷èòóâàííÿ ñåíñîð³â
  int ll = digitalRead(LL_SENSOR) == LOW ? 1 : 0;
  int l  = digitalRead(L_SENSOR)  == LOW ? 1 : 0;
  int c  = digitalRead(C_SENSOR)  == LOW ? 1 : 0;
  int r  = digitalRead(R_SENSOR)  == LOW ? 1 : 0;
  int rr = digitalRead(RR_SENSOR) == LOW ? 1 : 0;

  int pattern = (ll << 4) | (l << 3) | (c << 2) | (r << 1) | rr;

  // Îñíîâíà ëîã³êà ðóõó
switch (pattern) {
  // === ?? 1. Âïåâíåí³ ñèòóàö³¿ ===
  case 0b00100:
  case 0b01110:
  case 0b11111:
  case 0b01010:
  case 0b10101:
  case 0b11011:
  case 0b10001:
    forward(fastForward);
    break;

  case 0b11100:
  case 0b11000:
  case 0b10000:
  case 0b01100:
  case 0b01000:
  case 0b10100:
    turnLeft(strongLeft);
    delay(impulseDuration);
    stopMotors();
    lastTurnDirection = -1;
    break;

  case 0b00111:
  case 0b00011:
  case 0b00001:
  case 0b00110:
  case 0b00010:
    turnRight(strongRight);
    delay(impulseDuration);
    stopMotors();
    lastTurnDirection = 1;
    break;

  // === ?? 2. Ìåíøà âïåâíåí³ñòü ===
  case 0b11110:
  case 0b11010:
  case 0b01101:
  case 0b11101:
  case 0b11001:
    turnLeft(softLeft);
    delay(impulseDuration / 2);
    stopMotors();
    lastTurnDirection = -1;
    break;

  case 0b01111:
  case 0b01011:
  case 0b00101:
  case 0b10111:
  case 0b10011:
    turnRight(softRight);
    delay(impulseDuration / 2);
    stopMotors();
    lastTurnDirection = 1;
    break;

  // === ?? 3. Ñóìí³âíà ñèòóàö³ÿ — ä³ÿ çàëåæèòü â³ä êîíòåêñòó ===
  case 0b10010:
  case 0b01001:
  case 0b10110:
    forward(slowForward);
    break;

  case 0b00000:
    backward();
    delay(1);
  default:
    /*if (lastTurnDirection == -1) {
          turnLeft(softLeft);
          } else if (lastTurnDirection == 1) {
          turnRight(softRight);
          } else {
          stopMotors();  
          }*/
    // Ïîâíà âòðàòà ë³í³¿
    stopMotors();  // íå ïàí³êóºìî — ïàóçà
    if (millis() - lastActionTime > 30) {
      if (lastTurnDirection == -1) {
        turnLeft(leftiy);
      } else {
        turnRight(rightiy);
      }
      delay(impulseDuration);
      stopMotors();
      lastActionTime = millis();
      delay(5); 
      break; 
    }
}
}