#12/12 아두이노 로봇 HowTo

최종 코드

 

/******************************************************
 *                AutoRobot 
 *                                  made by Sohyemini
 *                                  2020.10.04
 ******************************************************/
 #include <Servo.h>
 #include <SoftwareSerial.h>
 #include <Wire.h>
 #include <LiquidCrystal_I2C.h>
/*
 *  두 개의 바퀴를 위한 디지털 Input/output D1에서 D4까지 선언
 *  D1, D2는 왼쪽 바퀴, D3, D4는 오른쪽 바퀴임
 */
int LeftWheel_D1  =   2;
int LeftWheel_D2  =   3;
int RightWheel_D3 =   4;
int RightWheel_D4 =   5;

// 초음파 센서를 움직이기 위한 서보 모터 핀 번호
int ServoPin = 6;
Servo servo;

// 적외선 장애물 감지 센서는 디지털 7번 핀으로 설정합니다.
int infrared  = 7;

// 정면, 좌, 우의 물체 거리를 저장할 변수
float distanceFront;
float distanceLeft;
float distanceRight; 

// 초음파 센서 각각의 핀 번호, 거리 측정을 위함
int echoPin = 9;
int trigPin = 8;

// 장애물이 이 범위에 들어오면 방향을 틀어야 함
// 단위는 센티미터
#define STOP_DISTANCE 40 

//Bluetooth setup
SoftwareSerial BT(10,11);  //rx, tx

// 리모컨을 사용할 것인지, 자동 주행을 할 것인지?
// 디폴트는 리모컨 사용
bool  Auto_Drive = false;

// 0x27 I2C 주소를 가지고 있는 16x2 LCD객체를 생성합니다.(I2C 주소는 LCD에 맞게 수정해야 합니다.)
LiquidCrystal_I2C lcd(0x27, 16, 2);
 
/*
 *  로봇의 이동을 담당할 함수 선언 및 delay 선언
 */
void Robot_Stop();
void Robot_Go();
void Robot_RightTurn();
void Robot_LeftTurn();
void Robot_180Turn();
void Robot_Back();

void Detect_Front();
void Detect_Object();

void I2C_Address_Check(); // I2C_Address 확인

void printLCD1(String str);
void printLCD2(String str);
/*
 * 초기화 함수로 아두이노 시작하고 단 한번만 동작됨
 */
void setup() {
  // 디버깅 메시지를 보기 위한 Serial 초기화
  Serial.begin(9600);
  //Serial.println("");  
  while(!Serial);
  BT.begin(9600);
  
  // 휠의 핀 모드를 정의함
  pinMode(LeftWheel_D1, OUTPUT); 
  pinMode(LeftWheel_D2, OUTPUT); 
  pinMode(RightWheel_D3, OUTPUT); 
  pinMode(RightWheel_D4, OUTPUT); 
  
  // 모든 휠을 정지상태로 초기화 한다
  Robot_Stop();

  // 서보 모터 초기화
  servo.attach(ServoPin);
  servo.write(90); // 정면을 바라보도록 설정

  // 초음파 센서의 핀 모드 설정 
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // 적외선 장애물 감지 센서 핀을 INPUT으로 설정합니다.
  pinMode(infrared, INPUT);

  //Wire.begin();
  
  // I2C LCD를 초기화 합니다..
  lcd.init();
  // I2C LCD의 백라이트를 켜줍니다.
  lcd.backlight();
  
  //1초 쉼
  delay(1000);
  Detect_Front();
}

void loop() {
  I2C_Address_Check(); // I2C address check

  /*
  if (BT.available()) {
    Serial.write(BT.read());
  }
  if (Serial.available()) {
    BT.write(Serial.read());
  }
  return;
  */
  // 적외선 감지 센서 부터 센서값을 읽습니다.
  // 감지되면 0, 감지되지 않으면 1이 나옵니다.
  int state = digitalRead(infrared);
  
  if(state == 0 && Auto_Drive) 
  {
    Robot_Back();
    delay(200);
    Robot_180Turn();
    return;
  }
  // 측정된 센서값을 시리얼 모니터에 출력합니다.
  /*
  Serial.print("Infrared = ");
  Serial.println(state);
  return;
  */
  bool bBT = BT.available();
  
  if(!Auto_Drive)
  {  
    printLCD1("Manual mode");
    switch(BT.read())
    {
      case 'a':
          Serial.println("Stop Btn");
          printLCD2("Stop");
          Robot_Stop();
          break;
      case 'b':
          Serial.println("Right Btn");
          printLCD2("Turn Right");
          Robot_RightTurn();
          break;
      case 'c':
          Serial.println("Left Btn");
          printLCD2("Turn Left");
          Robot_LeftTurn();
          break;
      case 'd':
          Serial.println("Go Btn");
          printLCD2("Go Go ...");
          Robot_Go();
          break;
      case 'e':
          Serial.println("Back Btn");
          printLCD2("Back ...");
          Robot_Back();
          break;
      case 'o':
          Serial.println("# Btn / 매뉴얼 모드");
          Auto_Drive = false;
          break;
      case 'p':
          Serial.println("* Btn / 오토 모드");
          Robot_Stop();
          Detect_Object();
          Auto_Drive = true;
          break;    
    }       
  } else {
    printLCD1("Auto Drive Mode");
    printLCD2("Made by SoHyeMin");
      if('o' == BT.read()){
         Serial.println("# Btn / 매뉴얼모드");
         Auto_Drive = false;
         Robot_Stop();
         return;
      } 
    // 자율주행 모드
    if(distanceFront > STOP_DISTANCE) // 정의된 거리보다 크면 앞으로 전진하고 물체 확인
    {
      Robot_Go();
    } else {              // 정면의 거리가 정의된 거리보다 작을 경우
      Detect_Object();    // 좌우의 거리를 구한다
      // 둘다 지정거리보다 작으면
      if (distanceRight < STOP_DISTANCE && distanceLeft < STOP_DISTANCE){ 
        Robot_180Turn();  // 180도 회전
      }
      else {
        if(distanceRight >= distanceLeft) // 오른쪽 거리가 더 멀면
          Robot_RightTurn();              // 우회전
        else                              // 왼쪽 거리가 더 멀면
          Robot_LeftTurn();               // 좌회전
      }
    }
    Detect_Front(); // 정면 거리 측정
  }    
}

/*
 * 로봇을 180도 회전시킨다
 */
void Robot_180Turn(){
  Serial.println("로봇 180도 회전");
  digitalWrite(LeftWheel_D1, HIGH);
  digitalWrite(LeftWheel_D2, LOW);
  digitalWrite(RightWheel_D3, LOW);
  digitalWrite(RightWheel_D4, HIGH);  
  delay(530);
  Robot_Stop(); 
  delay(250);
}

/*
 * 로봇을 오른쪽으로 회전시킨다
 */
void Robot_RightTurn(){
  Serial.println("로봇 우회전");
  digitalWrite(LeftWheel_D1, HIGH);
  digitalWrite(LeftWheel_D2, HIGH);
  digitalWrite(RightWheel_D3, HIGH);
  digitalWrite(RightWheel_D4, LOW);  
  //if(Auto_Drive)
  {
    delay(300);
    Robot_Stop();
    delay(250);
  }
}

/*
 * 로봇을 왼쪽으로 회전시킨다
 */
void Robot_LeftTurn(){
  Serial.println("로봇 좌회전");
  digitalWrite(LeftWheel_D1, HIGH);
  digitalWrite(LeftWheel_D2, LOW);
  digitalWrite(RightWheel_D3, HIGH);
  digitalWrite(RightWheel_D4, HIGH);  
  //if(Auto_Drive)
  {
    delay(300);
    Robot_Stop();
    delay(250);
  }
}

/*
 * 로봇을 정지킨다
 */
void Robot_Stop(){
  Serial.println("로봇 멈춤");
  digitalWrite(LeftWheel_D1, HIGH);
  digitalWrite(LeftWheel_D2, HIGH);
  digitalWrite(RightWheel_D3, HIGH);
  digitalWrite(RightWheel_D4, HIGH);  
}

/*
 * 로봇이 앞으로 간다
 */
void Robot_Go(){
  Serial.println("로봇 전진");
  digitalWrite(LeftWheel_D1, HIGH);
  digitalWrite(LeftWheel_D2, LOW);
  digitalWrite(RightWheel_D3, HIGH);
  digitalWrite(RightWheel_D4, LOW);  
  if(Auto_Drive)
  {
    delay(300);
    Robot_Stop();
  }  
}


/*
 * 로봇을 뒤로 움직인다
 */
void Robot_Back(){
  Serial.println("로봇 후진");
  digitalWrite(LeftWheel_D1, LOW);
  digitalWrite(LeftWheel_D2, HIGH);
  digitalWrite(RightWheel_D3, LOW);
  digitalWrite(RightWheel_D4, HIGH);   
}

/*
 * 초음파 센서의 거리를 구하는 함수
 */
float sensor_distance() {
  float distance = 0;

  // 트리거 핀을 이용해 초음파를 내 보내고 대기 모드로 전환
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(20);
  digitalWrite(trigPin, LOW);

  // 초음파가 들어온 시간, 마이크로 초
  // 초당 340 미터를 이동하는데 여기서는 마이크로 초이고
  // 왕복 거리이므로 2로 나눠준다. 
  // 복잡하므로 58.5로 나눠도 된다.
  distance = pulseIn(echoPin, HIGH) / 58.8;
  return distance;
}

/*
 * 좌, 우 거리를 각각 구한다
 */
void Detect_Object(){
  servo.write(0);
  delay(250);
  distanceRight = sensor_distance();

  servo.write(179);
  delay(250);
  distanceLeft = sensor_distance(); 
}

/*
 * 정면의 거리를 구한다
 */
void Detect_Front(){
  servo.write(90);
  delay(250);
  distanceFront = sensor_distance(); 
}

/*
 * I2C의 번지수를 구한다. 
 * 한번 사용 후에는 함수가 호출 된 후 바로 return 하여
 * 동작을 하지 않도록 설정함
 */
void I2C_Address_Check(){
  return;
  byte error, address;
  int nDevices;
  Serial.println("Scanning...");
  nDevices = 0;
  for(address = 1; address < 127; address++ ) 
  {
    Wire.beginTransmission(address);
    error = Wire.endTransmission();

    if (error == 0)
    {
      Serial.print("I2C device found at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.print(address,HEX);
      Serial.println("  !");

      nDevices++;
    }
    else if (error==4) 
    {
      Serial.print("Unknow error at address 0x");
      if (address<16) 
        Serial.print("0");
      Serial.println(address,HEX);
    }    
  }
  if (nDevices == 0)
    Serial.println("No I2C devices found\n");
  else
    Serial.println("done\n");

  delay(5000);
}

void printLCD1(String str){
    // 0번째 줄 0번째 셀부터 입력하게 합니다.
    lcd.setCursor(0,0);
    // 첫 문장을 출력합니다.
    lcd.print(str);
    for(byte i = str.length(); i < 16; i++)
      lcd.write(' ');
}

void printLCD2(String str){
    // 1번째 줄 0번째 셀부터 입력하게 합니다.
    lcd.setCursor(0,1);
    // 두 번째 문장을 출력합니다.
    lcd.print(str);
    for(byte i = str.length(); i < 16; i++)
      lcd.write(' ');
}