Урок 3: Программирование роботов

// Робот-уборщик: алгоритм Дейкстры
#include <Servo.h>

#define TRIG_PIN    9
#define ECHO_PIN   10
#define IR_PIN     A0
#define BUMP_PIN    7
#define MOTOR_L_EN 5
#define MOTOR_L_IN 2
#define MOTOR_R_EN 6
#define MOTOR_R_IN 3
#define SERVO_PIN 11

Servo armServo;

const int GRID_W = 15, GRID_H = 10;
int grid[GRID_H][GRID_W]; // 0=свободно, 1=мусор, 2=стена
int dist[GRID_H][GRID_H];
int prevR[GRID_H][GRID_W], prevC[GRID_H][GRID_W];

struct Pos { int r, c; };
Pos robotPos = {7, 7};
Pos basePos = {8, 1};
int binLoad = 0, binCapacity = 8;

float readDistance() {
  digitalWrite(TRIG_PIN, LOW); delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  return pulseIn(ECHO_PIN, HIGH) * 0.034 / 2.0;
}

bool bumpHit() { return digitalRead(BUMP_PIN) == LOW; }

void moveForward() {
  analogWrite(MOTOR_L_EN, 150); digitalWrite(MOTOR_L_IN, HIGH);
  analogWrite(MOTOR_R_EN, 150); digitalWrite(MOTOR_R_IN, HIGH);
  delay(300); stopMotors();
}

void turnLeft() {
  analogWrite(MOTOR_L_EN, 120); digitalWrite(MOTOR_L_IN, LOW);
  analogWrite(MOTOR_R_EN, 120); digitalWrite(MOTOR_R_IN, HIGH);
  delay(250); stopMotors();
}

void turnRight() {
  analogWrite(MOTOR_L_EN, 120); digitalWrite(MOTOR_L_IN, HIGH);
  analogWrite(MOTOR_R_EN, 120); digitalWrite(MOTOR_R_IN, LOW);
  delay(250); stopMotors();
}

void stopMotors() {
  analogWrite(MOTOR_L_EN, 0); analogWrite(MOTOR_R_EN, 0);
}

// Дейкстра: кратчайший путь от (sr,sc) к (tr,tc)
bool dijkstra(int sr, int sc, int tr, int tc, Pos path[], int &pathLen) {
  for (int r = 0; r < GRID_H; r++)
    for (int c = 0; c < GRID_W; c++) {
      dist[r][c] = 9999; prevR[r][c] = -1; prevC[r][c] = -1;
    }
  dist[sr][sc] = 0;

  for (int iter = 0; iter < GRID_W * GRID_H; iter++) {
    int minD = 9999, mr = -1, mc = -1;
    for (int r = 0; r < GRID_H; r++)
      for (int c = 0; c < GRID_W; c++)
        if (dist[r][c] < minD && prevR[r][c] != -2) {
          minD = dist[r][c]; mr = r; mc = c;
        }
    if (mr == -1) break;
    prevR[mr][mc] = -2; // пометка посещённой
    if (mr == tr && mc == tc) break;

    int dirs[][2] = {{-1,0},{1,0},{0,-1},{0,1}};
    for (auto &d : dirs) {
      int nr = mr + d[0], nc = mc + d[1];
      if (nr < 0 || nr >= GRID_H || nc < 0 || nc >= GRID_W) continue;
      if (grid[nr][nc] == 2) continue; // стена
      if (dist[mr][mc] + 1 < dist[nr][nc]) {
        dist[nr][nc] = dist[mr][mc] + 1;
        prevR[nr][nc] = mr; prevC[nr][nc] = mc;
      }
    }
  }

  if (prevR[tr][tc] == -1) return false; // путь не найден
  pathLen = 0;
  int r = tr, c = tc;
  while (r != sr || c != sc) {
    path[pathLen++] = {r, c};
    int pr = prevR[r][c], pc = prevC[r][c];
    r = pr; c = pc;
  }
  // переворачиваем путь
  for (int i = 0; i < pathLen / 2; i++) {
    Pos tmp = path[i]; path[i] = path[pathLen-1-i]; path[pathLen-1-i] = tmp;
  }
  return true;
}

Pos findNearestTrash() {
  int minD = 9999; Pos best = {-1, -1};
  for (int r = 0; r < GRID_H; r++)
    for (int c = 0; c < GRID_W; c++)
      if (grid[r][c] == 1) {
        int d = abs(r - robotPos.r) + abs(c - robotPos.c);
        if (d < minD) { minD = d; best = {r, c}; }
      }
  return best;
}

void collectWithArm() {
  armServo.attach(SERVO_PIN);
  armServo.write(0); delay(500);
  armServo.write(90); delay(500);
  armServo.write(0); delay(300);
  armServo.detach();
}

void returnToBase() {
  Pos path[100]; int pathLen;
  if (dijkstra(robotPos.r, robotPos.c, basePos.r, basePos.c, path, pathLen)) {
    followPath(path, pathLen);
    // разгрузка
    armServo.attach(SERVO_PIN);
    armServo.write(180); delay(600);
    armServo.write(0); delay(400);
    armServo.detach();
    binLoad = 0;
  }
}

void followPath(Pos path[], int len) {
  for (int i = 0; i < len; i++) {
    int dr = path[i].r - robotPos.r;
    int dc = path[i].c - robotPos.c;
    if (dc == 1) turnRight();
    else if (dc == -1) turnLeft();
    else if (dr == 1) { turnRight(); turnRight(); }
    moveForward();
    robotPos = path[i];
  }
}

void setup() {
  Serial.begin(9600);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(BUMP_PIN, INPUT_PULLUP);
  pinMode(MOTOR_L_EN, OUTPUT); pinMode(MOTOR_L_IN, OUTPUT);
  pinMode(MOTOR_R_EN, OUTPUT); pinMode(MOTOR_R_IN, OUTPUT);
  memset(grid, 0, sizeof(grid));
  grid[3][5] = 1; grid[6][8] = 1; grid[2][12] = 1;
  grid[5][2] = 1; grid[7][10] = 1;
  Serial.println("=== Dijkstra Robot ===");
}

void loop() {
  if (readDistance() < 15) { stopMotors(); return; }
  if (bumpHit()) { stopMotors(); delay(200); turnRight(); return; }

  Pos trash = findNearestTrash();
  if (trash.r == -1) {
    Serial.println("Весь мусор убран! Возврат на базу.");
    returnToBase();
    Serial.println("Робот на стоянке. Ожидание...");
    while (true) { delay(1000); }
  }

  Pos path[100]; int pathLen;
  if (dijkstra(robotPos.r, robotPos.c, trash.r, trash.c, path, pathLen)) {
    Serial.print("Мусор в ("); Serial.print(trash.r);
    Serial.print(","); Serial.print(trash.c);
    Serial.println("). Двигаемся...");
    followPath(path, pathLen);
    collectWithArm();
    grid[trash.r][trash.c] = 0;
    binLoad++;
    Serial.print("Собрано: "); Serial.print(binLoad);
    Serial.print("/"); Serial.println(binCapacity);
  }
}