[전시회] 뱀 로봇

작성자
이재중
작성일
2022-11-16 23:35
조회
317
팀원: 김아린, 남궁혁, 신동호, 이재중

<로봇 소개>

뱀 로봇(Snake Robot)
뱀의 대표적인 움직임들 중 사행 운동(Serpentine Motion)을 구현한 로봇입니다. 실제로 뱀이 사행 운동을 할 때는 근육을 수축, 이완해서 몸을 물결 모양(횡파와 유사한 모양)으로 만듭니다.  그리고 몸의 구부러진 부분 아래에 있는 배판 비늘의 지면과의 마찰력을 이용해 나아갑니다. 이를 뱀 로봇에서는 10개의 서보모터로 로봇을 뱀처럼 구부릴 수 있게 하고, 소형 레고 바퀴를 로봇 하부에 장착해 뱀의 비늘 역할을 하게 만들었습니다.

                 

사행 운동을 하고 있는 뱀과 뱀 로봇의 비교 사진

 

<재료>

아두이노 우노, mg995서보모터 10개, 서보모터 브라켓 세트 10개, ㄴ자 사이드 브라켓 9개, 소형 레고 바퀴 20개, 9V전지 2개, 블루투스 모듈, 점퍼선 여러 개, 3D 프린터로 제작한 뱀의 외형(머리, 꼬리, 몸통) 등

 

<제작 동기>

조원 모두가 실습 세미나 이외의 로봇 제작은 처음이었기에 따라 만들어 볼 수 있는 로봇을 찾았습니다. 그러다 로봇의 재료, 제작 방법, 코드를 제시한 뱀 로봇을 발견했고 이를 참고 및 조원들의 의견을 반영해 저희만의 뱀 로봇을 만들어보기로 하였습니다.

 

<하드웨어>

3D 프린터로 제작한 머리와 꼬리 부분, 그리고 mg995 서보 모터 10개를 c형 브라켓과 ㄴ자 사이드 브라켓을 이용해 서로 연결했습니다. 그리고 서보모터에 9V 전지 2개로 외부 전원을 전달했고 우노와 연결하여 명령에 맞게 움직이도록 하였습니다.

                   

뱀 로봇의 머리(좌)와 몸통 부위(우)


마치 애벌레와 같은 외형을 갖춘 뱀 로봇

 

<소프트웨어>

뱀 로봇의 동작은 다음과 같이 이루어집니다. 블루투스 모듈(HC-06)을 통해 신호를 받다가, 1, 2, 3, 4 중 하나의 신호가 들어오면 해당 움직임을 수행하게 됩니다. ‘1’신호가 들어오면 전진, ‘2’신호가 후진, ‘3’이 우회전, ‘4’가 좌회전입니다. 예외로 ‘6’ 신호는 뱀 로봇의 모든 파츠를 일직선으로 정렬시키는 신호입니다. 움직임은 뱀이 사행 운동을 할 때의 형태가 사인파를 그린다고 가정하여 각 부위의 각도를 계산한 후 서보모터로 구현합니다.



움직임을 계속 반복하다가 블루투스로 ‘5’ 신호가 들어오게 되면 반복문을 벗어나 움직임을 멈추게 됩니다.

 

(영상>sior 유튜브에 올라가는 대로 첨부할 예정)

 

<소스코드>

#include 
#include  

SoftwareSerial BTSerial(12,13);

// Define servo objects for the snake segments
Servo s1; 
Servo s2;
Servo s3;
Servo s4; 
Servo s5;
Servo s6;
Servo s7;
Servo s8;
Servo s9; 
Servo s10;

  
// Define variables


int counter = 0; // Loop counter variable
float lag = .4712; // Phase lag between segments
int frequency = 1; // Oscillation frequency of segments.
int amplitude = 40; // Amplitude of the serpentine motion of the snake
int rightOffset = 5; // Right turn offset
int leftOffset = -5; // Left turn offset
int delayTime = 5; // Delay between limb movements
int startPause = 5000;  // Delay time to position robot
int test = -3; // Test varialble takes values from -6 to +5
int offset7=-45;  
void setup() 
{ 
  Serial.begin(9600);
  BTSerial.begin(9600);

 
  
// Attach segments to pins  
   s1.attach(2);
   s2.attach(3);
   s3.attach(4);
   s4.attach(5);
   s5.attach(6);
   s6.attach(7);
   s7.attach(8);
   s8.attach(9);
   s9.attach(10);
   s10.attach(11);

 
// Put snake in starting position
   s1.write(90+amplitude*cos(5*lag));
   s2.write(90+amplitude*cos(4*lag)); 
   s3.write(90+amplitude*cos(3*lag));
   s4.write(90+amplitude*cos(2*lag));
   s5.write(90+amplitude*cos(1*lag));
   s6.write(90+amplitude*cos(0*lag));
   s7.write(90+amplitude*cos(-1*lag));
   s8.write(90+amplitude*cos(-2*lag));
   s9.write(90+amplitude*cos(-3*lag));
   s10.write(90+amplitude*cos(-4*lag));
  delay(startPause);  // Pause to position robot
} 
  
  
void loop() 
{

   char bluetooth=BTSerial.read();

while (bluetooth=='6'){
    Serial.write("hold");
    for(counter = 0; counter < 360; counter += 1){
      delay(delayTime);
      s1.write(90);
      s2.write(90);
      s3.write(90);
      s4.write(90);
      s5.write(90);
      s6.write(90);
      s7.write(90);
      s8.write(90);
      s9.write(90);
      s10.write(90);
      if(BTSerial.read()=='5'){
      bluetooth=BTSerial.read();
    }
    }

  }
 
// Forward motion
  while (bluetooth=='1'){
    Serial.write("forward");
    for(counter = 0; counter < 360; counter += 1){ delay(delayTime); s1.write(90+amplitude*cos(frequency*counter*3.14159/180+5*lag)); s2.write(90+amplitude*cos(frequency*counter*3.14159/180+4*lag)); s3.write(90+amplitude*cos(frequency*counter*3.14159/180+3*lag)); s4.write(90+amplitude*cos(frequency*counter*3.14159/180+2*lag)); s5.write(90+amplitude*cos(frequency*counter*3.14159/180+1*lag)); s6.write(90+amplitude*cos(frequency*counter*3.14159/180+0*lag)); s7.write(90+amplitude*cos(frequency*counter*3.14159/180-1*lag)); s8.write(90+amplitude*cos(frequency*counter*3.14159/180-2*lag)); s9.write(90+amplitude*cos(frequency*counter*3.14159/180-3*lag)); s10.write(90+amplitude*cos(frequency*counter*3.14159/180-4*lag)); if(BTSerial.read()=='5'){ bluetooth=BTSerial.read(); } } } // Reverse motion while(bluetooth=='2'){ Serial.write("reverse"); for(counter = 360; counter > 0; counter -= 1)  {
      delay(delayTime);
      s1.write(90+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      if(BTSerial.read()=='5'){
      bluetooth=BTSerial.read();
    }
    }    
  }
  
// Right turn
   while(bluetooth=='3'){
    Serial.write("right");
// Ramp up turn offset
    for(counter = 0; counter < 10; counter += 1)  {
      delay(delayTime);
      s1.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+.1*counter*rightOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
     if(BTSerial.read()=='5'){
      bluetooth=BTSerial.read();
      break;
    }
    }  
// Continue right turn
    for(counter = 11; counter < 350; counter += 1)  {
      delay(delayTime);
      s1.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+rightOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
       if(BTSerial.read()=='5'){
      bluetooth=BTSerial.read();
      break;
    }
    }    
// Ramp down turn offset
    for(counter = 350; counter < 360; counter += 1)  {
      delay(delayTime);
      s1.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+offset7+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+.1*(360-counter)*rightOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      if(BTSerial.read()=='5'){
      bluetooth=BTSerial.read();
      break;
    }
    } 
  } 
  
// Left turn
  while(bluetooth=='4'){
    Serial.write("left");
// Ramp up turn offset
    for(counter = 0; counter < 10; counter += 1)  {
      delay(delayTime);
      s1.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+offset7+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+.1*counter*leftOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
      if(BTSerial.read()=='5'){
      bluetooth=BTSerial.read();
      break;
    }
    }  
// Continue left turn
    for(counter = 11; counter < 350; counter += 1)  {
      delay(delayTime);
      s1.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+offset7+leftOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+leftOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
    if(BTSerial.read()=='5'){
      bluetooth=BTSerial.read();
      break;
    }
    }    
// Ramp down turn offset
    for(counter = 350; counter < 360; counter += 1)  {
      delay(delayTime);
      s1.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+5*lag));
      s2.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+4*lag));
      s3.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+3*lag));
      s4.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+2*lag));
      s5.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+1*lag));
      s6.write(90+offset7+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180+0*lag));    
      s7.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180-1*lag));
      s8.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180-2*lag));    
      s9.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180-3*lag));
      s10.write(90+.1*(360-counter)*leftOffset+amplitude*cos(frequency*counter*3.14159/180-4*lag));
       if(BTSerial.read()=='5'){
      bluetooth=BTSerial.read();
      break;
    }
    } 
  } 
}
참고한 소스코드의 주소
https://content.instructables.com/FAQ/UZ9D/J16Y9UIU/FAQUZ9DJ16Y9UIU.txt
전체 0