Робот, едущий по линии

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

// моторы (L298N)
const int ENA = 5, IN1 = 7, IN2 = 8;   // левый
const int ENB = 6, IN3 = 2, IN4 = 4;   // правый

void motors(int left, int right) {     // -255 … 255
  digitalWrite(IN1, left >= 0);  digitalWrite(IN2, left < 0);
  digitalWrite(IN3, right >= 0); digitalWrite(IN4, right < 0);
  analogWrite(ENA, abs(left));   analogWrite(ENB, abs(right));
}

void setupMotors() {
  int pins[] = {ENA, IN1, IN2, ENB, IN3, IN4};
  for (int p : pins) pinMode(p, OUTPUT);
  motors(0, 0);
}

const int LEFT = 12, RIGHT = 11;   // HIGH = над чёрной линией
const int SPEED = 150, TURN = 170;

void setup() {
  setupMotors();
  pinMode(LEFT, INPUT); pinMode(RIGHT, INPUT);
  delay(1500);
}

void loop() {
  bool l = digitalRead(LEFT), r = digitalRead(RIGHT);
  if (!l && !r)      motors(SPEED, SPEED);    // линия между датчиками
  else if (l && !r)  motors(-TURN / 2, TURN); // линия под левым: поворот налево
  else if (!l && r)  motors(TURN, -TURN / 2); // под правым: поворот направо
  else               motors(0, 0);            // оба на чёрном: перекрёсток или финиш
}