🤖 ماموریت ۳۹: ربات مانع‌گریز

سیم‌کشی

1. Arduino Uno (VCC5) ⟷ L293D Motor Driver (P16)
2. L293D Motor Driver (P4) ⟷ Arduino Uno (GND1)
3. 9V Battery (کتابی) (VCC) ⟷ L293D Motor Driver (P8)
4. 9V Battery (کتابی) (GND) ⟷ Arduino Uno (GND2)
5. Arduino Uno (D4) ⟷ L293D Motor Driver (P2)
6. Arduino Uno (D5) ⟷ L293D Motor Driver (P7)
7. Arduino Uno (D9) ⟷ L293D Motor Driver (P1)
8. L293D Motor Driver (P3) ⟷ موتور DC (MOTOR_POS)
9. L293D Motor Driver (P6) ⟷ موتور DC (MOTOR_NEG)
10. Arduino Uno (D6) ⟷ L293D Motor Driver (P10)
11. Arduino Uno (D7) ⟷ L293D Motor Driver (P15)
12. Arduino Uno (D10) ⟷ L293D Motor Driver (P9)
13. L293D Motor Driver (P11) ⟷ موتور DC (MOTOR_POS)
14. L293D Motor Driver (P14) ⟷ موتور DC (MOTOR_NEG)
15. Arduino Uno (VCC5) ⟷ سروو موتور (VCC)
16. سروو موتور (GND) ⟷ Arduino Uno (GND1)
17. Arduino Uno (D11) ⟷ سروو موتور (SIG)
18. Arduino Uno (VCC5) ⟷ Ultrasonic (HC-SR04) (VCC)
19. Arduino Uno (D12) ⟷ Ultrasonic (HC-SR04) (TRIG)
20. Ultrasonic (HC-SR04) (ECHO) ⟷ Arduino Uno (D13)
21. Ultrasonic (HC-SR04) (GND) ⟷ Arduino Uno (GND1)

کد

#include <Servo.h>
Servo head;
const int trig = 12, echo = 13;

// موتور چپ
const int l1 = 4, l2 = 5, lEn = 9;
// موتور راست
const int r1 = 6, r2 = 7, rEn = 10;

void motors(int lSpeed, int rSpeed) {
  digitalWrite(l1, lSpeed >= 0);
  digitalWrite(l2, lSpeed < 0);
  analogWrite(lEn, lSpeed >= 0 ? lSpeed : -lSpeed);

  digitalWrite(r1, rSpeed >= 0);
  digitalWrite(r2, rSpeed < 0);
  analogWrite(rEn, rSpeed >= 0 ? rSpeed : -rSpeed);
}

void setupMotors() {
  int pins[] = {l1, l2, lEn, r1, r2, rEn};
  for (int i = 0; i < 6; i++) pinMode(pins[i], OUTPUT);
}

long look() {
  digitalWrite(trig, LOW);  delayMicroseconds(2);
  digitalWrite(trig, HIGH); delayMicroseconds(10);
  digitalWrite(trig, LOW);
  long d = pulseIn(echo, HIGH, 30000);
  return d == 0 ? 400 : d / 58;
}

long lookAt(int angle) {
  head.write(angle);
  delay(350);
  return look();
}

void setup() {
  head.attach(11);
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  setupMotors();
  head.write(90);
  delay(500);
  Serial.begin(9600);
}

void loop() {
  long ahead = lookAt(90);
  Serial.print("jolo:");
  Serial.println(ahead);

  if (ahead > 25) {
    motors(170, 170);
    return;
  }

  motors(0, 0);
  delay(200);

  long right = lookAt(20);
  long left  = lookAt(160);
  head.write(90);

  if (left > right) motors(-160, 160);
  else              motors(160, -160);

  delay(500);
  motors(0, 0);
}