Robot Di Động Cơ Bản

void moveForward() {
  digitalWrite(leftMotor1, HIGH);
  digitalWrite(leftMotor2, LOW);
  digitalWrite(rightMotor1, HIGH);
  digitalWrite(rightMotor2, LOW);
}

void turnLeft() {
  digitalWrite(leftMotor1, LOW);
  digitalWrite(leftMotor2, HIGH);
  digitalWrite(rightMotor1, HIGH);
  digitalWrite(rightMotor2, LOW);
}

void setup() {
  // Khởi tạo pins
}

void loop() {
  moveForward();
  delay(2000);
  turnLeft();
  delay(1000);
}

int scanDistance() {
  unsigned int distance = sonar.ping_cm();
  return distance == 0 ? 999 : distance;
}

void findBestPath() {
  int frontDistance = scanDistance();
  
  if (frontDistance < 20) {
    stopMotors();
    
    // Quét bên trái
    servoMotor.write(150);
    delay(500);
    int leftDistance = scanDistance();
    
    // Quét bên phải
    servoMotor.write(30);
    delay(500);
    int rightDistance = scanDistance();
    
    // Về giữa
    servoMotor.write(90);
    delay(500);
    
    if (leftDistance > rightDistance) {
      turnLeft();
      delay(300);
    } else {
      turnRight();
      delay(300);
    }
  } else {
    moveForward();
  }
}