장애물 피하는 로봇 제작 1 프로젝트를 시작한 이후 1주일 만에 제대로된 트로젝트를 올립니다.

장애물 피하는 로봇 제작 1 (http://kocoafab.cc/make/view/581)

그간 기숙사에 있어서 잠시 덮어 뒀던 장애물 피하는 로봇을 소개해 드릴게요.

<작동 영상>

몸체는 아래 사진과 같이 2개의 DC모터, 1개의 서보모터, 초음파 센서, 오랜지 보드, 모터쉴드로 구성되어 있습니다. 서보 모터위에 초음파 센서가 연결되어 있어 서보모터가 180도로 회전하면서 거리를 측정해 장애물에대한 회피율이 상당히 높은것 같지만 사실 좁은 통로에서는 장애물을 잘 피하지 못한다는 단점도 있습니다.

자세히 보시면 서보모터에 달린 전선을 한번 절단하고 순서를 바꾸어 연결한 흔적이 있는데 이 서보모터가 GND와 신호선 위치가 아두이노 전용 서보모터와 위치가 반대여서 어쩔수 없이 절단 했습니다.(그 사실을 알아 차리는데 30분 걸렸다곤 말 못해요 ㅋㅋ) 구동 모터가 2개이다 보니 안정성을 위해(한 평면을 결정하기 위해선 최소 점 3개가 필요하죠) 앞쪽에 ball을 달고 몸체와 육각기둥 나사와 연결했습니다. 나머지 연결 상태는 다음 소스 코드를 참고하시면 이해가 잘 될겁니다.^^

<소스코드>

#include 

Servo myservo; 
int pos = 0;
int trig = 7;
int echo = 6;

void setup() {
  Serial.begin(9600);
  myservo.attach(5); 
  pinMode(trig, OUTPUT);
  pinMode(echo, INPUT);
  pinMode(12, OUTPUT); 
  pinMode(9, OUTPUT); 
  pinMode(13, OUTPUT); 
  pinMode(8, OUTPUT); 
}

void loop() {
  
  for(pos = 0; pos < 180; pos +=5)//서보모터 각도 pos를 5도 단위로 회전 
  {    
    myservo.write(pos);    
    delay(15); //서보모터가 돌아가기 위한 시간
    
     digitalWrite(trig, LOW);
  digitalWrite(echo, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);//초음파 센서 작동

  unsigned long duration1 = pulseIn(echo, HIGH);
  float distance1 = duration1 / 29.0 / 2.0;//측정한 거리를 cm단위로
    
  if (distance1 > 15)//만약 거리가 15cm 이상이면
  { 
   forward();//전진 함수
  }
  
  if (distance1 < 15 && pos < 90)//만약 15cm이하, 왼쪽에 장애물이 있으면
  { 
   turn_right();//우회전 함수
  }
  
  if (distance1 < 15 && pos > 90)//만약 15cm이하, 오른쪽에 장애물이 있으면
  { 
   turn_left();//좌회전 함수
  }
  
  }
  
  for(pos = 180; pos>=1; pos-=5)//이 아래부터는 서보모터를 반대로 회전하면서 같은 과정을 거침    
  {                                
    myservo.write(pos);              
    delay(15);
    
  digitalWrite(trig, LOW);
  digitalWrite(echo, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);

  unsigned long duration2 = pulseIn(echo, HIGH);
  float distance2 = duration2 / 29.0 / 2.0;
    
  if (distance2 > 15)
  { 
   forward();
  }
  
  if (distance2 < 15 && pos < 90)
  { 
   turn_right();
  }
  
  if (distance2 < 15 && pos > 90)
  { 
   turn_left();
  }
  
  } 
  
}

void freeze()//혹시 몰라 만들어본 정지 함수
{
  digitalWrite(12, LOW);  
  digitalWrite(9, HIGH);  
  analogWrite(3, 100);    
  digitalWrite(13, HIGH); 
  digitalWrite(8, HIGH);   
  analogWrite(11, 100);  
  delay(400); 
}

void forward()//전진 함수
{
  digitalWrite(12, LOW);  
  digitalWrite(9, LOW);   
  analogWrite(3, 200);   
  digitalWrite(13, HIGH); 
  digitalWrite(8, LOW);  
  analogWrite(11, 200);   
}

void turn_right()//우회전 함수
{ 
  digitalWrite(12, HIGH); 
  digitalWrite(9, LOW);   
  analogWrite(3, 200);   
  digitalWrite(13, HIGH);
  digitalWrite(8, LOW);   
  analogWrite(11, 200);   
  delay(400); 
}

void turn_left()//좌회전 함수
{ 
  digitalWrite(12, LOW); 
  digitalWrite(9, LOW);   
  analogWrite(3, 200);   
  digitalWrite(13, LOW);
  digitalWrite(8, LOW);   
  analogWrite(11, 200);   
  delay(400); 
}

void back()//유턴 함수
{ 
  digitalWrite(12, HIGH); 
  digitalWrite(9, LOW);   
  analogWrite(3, 200);    
  digitalWrite(13, HIGH); 
  digitalWrite(8, LOW);   
  analogWrite(11, 200);   
  delay(1000); 
}

코드는 학교에서 짬짬히 배운 허접한 C++실력으로 만들어 보았습니다. 간단하게 설명을 덧붙히면 서보모터가 돌아간 각도를 판단하여 장애물 위치를 파악하고 회전하는 간단한 알고리즘 입니다. 지금은 여기에 블루투스 모듈을 연결해 무선제어도 시작했는데요 생각보다 잘 안되내요 ㅋㅋ(아두이노 배운지 얼마 안됬는데 만들고 싶은 건 많네요) 사실 프로젝트로 올리기에는 허접한 작품이지만 많은 사람들에게 도움이 되길 바랍니다. ^^