Obstacle-avoiding robot

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
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60

#include <Servo.h>

// motors (L298N)
const int ENA = 5, IN1 = 7, IN2 = 8;   // left
const int ENB = 6, IN3 = 2, IN4 = 4;   // right

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 TRIG = 12, ECHO = 11;
Servo neck;

long distanceCm() {
  digitalWrite(TRIG, LOW);  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  long us = pulseIn(ECHO, HIGH, 25000);   // ~4 m max
  return us ? us / 58 : 400;              // nothing heard = far away
}

long look(int angle) {
  neck.write(angle); delay(350);
  return distanceCm();
}

void setup() {
  Serial.begin(9600);
  setupMotors();
  pinMode(TRIG, OUTPUT); pinMode(ECHO, INPUT);
  neck.attach(3);
  neck.write(90);
  delay(1500);
}

void loop() {
  long ahead = distanceCm();
  Serial.println(ahead);
  if (ahead > 25) {
    motors(170, 170);
    delay(40);
    return;
  }
  motors(0, 0);
  motors(-150, -150); delay(250);   // a step back
  motors(0, 0);
  long left = look(160), right = look(20);
  neck.write(90); delay(200);
  if (left > right) motors(-170, 170); else motors(170, -170);
  delay(380);                       // about a quarter turn
  motors(0, 0);
}