Line Tracing Jansen Mechanism

라인 트레이싱 유튜브 영상

Team Member

[수염난 피터팬] 김현준 이수홍

Special thanks 김영수, 차혜민, 공현배, 강남규, 김승하

Development Period

2017.2 ~ 3

Using Device

아두이노, 적외선 센서, DC 모터, 과학상자 기어 및 축, 3D 프린터(신도 DP200)

Schematic Diagram

About the project & Goal

여러개의 적외선 센서 조합을 이용하여 Jansen이 검은색 선을 따라갈 수 있도록 한다.

HW design

다리 길이의 비율은, 참조[1]의 다리 길이 비율을 차용하였고, 30*30*30cm의 경진 대회 규격을 고려하여 최대한 길게 길이를 정했습니다.

1. 보행중에 다리의 흔들림을 줄이기 위해서
2. 축의 흔들림으로 인한 내부 마찰을 줄이기 위해서

위와 같은 목적으로 Joint에서의 디자인은 사용, 와셔와 볼트로 두 linkage 고정

동력 전달부에서, 기구의 강도가 가장 약해짐
이를 해결하기 위해, 알맞은 공차 설계로 동력 전달 중 흔들림으로 인한 파워 로스를 줄여줌

무게 중심을 낮추기 위해, 부품 중 가장 무거운 모터의 위치를 아래로 내릴 수 있도록 바디를 설계 하였다.

Algorithm

// 센서
int left2 = A2;
int left1 = A3;
int right1 = A4;
int right2 = A5;

int l2 = 0;
int l1 = 0;
int r1 = 0;
int r2 = 0;
//모드
int mode=0;
int r = 1;

int straight=255;
int rotateH=255;
int rotateL=180;

void setup() {
  pinMode(left2,INPUT);
  pinMode(left1,INPUT);
  pinMode(right1,INPUT);
  pinMode(right2,INPUT);
  pinMode(4, OUTPUT); // 방향 HIGH, LOW +,-
  pinMode(5, OUTPUT); // PWM
  pinMode(6, OUTPUT); // PWM
  pinMode(7, OUTPUT); // 방향 HIGH, LOW +,-
  Serial.begin(9600);
}

void loop() {
  // put your main code here, to run repeatedly
  l2 = analogRead(left2);// leftstate값 확인필요!, 흰색 = 750, 검은색 = 0
  l1 = analogRead(left1); //rightstate 확인필요!, 흰색 = 750, 검은색 = 0
  r1 = analogRead(right1);
  r2 = analogRead(right2);
 
  Serial.print(l2);
  Serial.print("/");
  Serial.print(l1);
  Serial.print("/");
  Serial.print(r1);
  Serial.print("/");
  Serial.print(r2);
  Serial.print("/");
  Serial.println(mode); 

  r = r + 1;
  if(r>10){ r = 1; }
  mode_selector();
  
  switch(mode){
    case 0:  digitalWrite(4,0);
             analogWrite(5,straight);
             analogWrite(6,straight);
             digitalWrite(7,0);
             break;
    case -1:  digitalWrite(4,0);  // 좌회전
             analogWrite(5,rotateL);
             analogWrite(6,rotateH);
             digitalWrite(7,0);
             break;
    case -2:  digitalWrite(4,0);  // 좌강회전
             analogWrite(5,0);
             analogWrite(6,rotateH);
             digitalWrite(7,0);
             break;  
    case 1:  digitalWrite(4,0);  // 우회전
             analogWrite(5,rotateH);
             analogWrite(6,rotateL);
             digitalWrite(7,0);
             break;
    case 2:  digitalWrite(4,0);  // 우강회전
             analogWrite(5,rotateH);
             analogWrite(6,0);
             digitalWrite(7,0);
             break;
             
    case 5:  digitalWrite(4,0);  // 정지
             analogWrite(5,0);
             analogWrite(6,0);
             digitalWrite(7,0);
             break;    
   case -5:  digitalWrite(4,1);  // 후진
            analogWrite(5,180);
            analogWrite(6,180);
            digitalWrite(7,1);
            break;
  }
  delay(10);
}
void mode_selector(){
  if(l2>200 && r2>200){
    mode=0;                              //0**0 직진
    if(l1>200 && r1>200){
    mode=-5;                             //0000 후진
    }
    if(l1<200 && r1 >200){mode=-1;}      //0100 좌
 
    if(r1<200 && l1 >200){mode=1;}       //0010 우
  }
  if(l2<200){mode=-2;}                   //1000 좌강
  if(r2<200){mode=2;}                    //0001 우강
 
  if(l2<200 && r2<200){pow(-1,r);}       //좌우 흔들어 줘서, 계속 앞 뒤로 움직이며 정체 된 것을 해결해 줌
}

Awarded

Thanks to

경진 대회 참가 할 수 있는 기회를 주신 이수홍 교수님
기본 적인 얀센 구조를 알려주고, 연구실 팀의 작품 완성을 위해 헌신해준 영수
마지막날 까지 같이 작업하며, 센서 배치 구조, 알고리즘 등 많은 도움을 준 혜민,현배
그리고 경진대회로 고생 많이 한 남규 승하 형 감사합니다.

Reference

[1] JIN YONGZHU, 지형근. (2016). 얀센 메커니즘을 이용한 보행 로봇의 최적설계에 관한 연구. 한국CDE학회지, 22(2), 18-22. 2. 트레이싱 알고리즘
[2] m.sketch 프로그램 - 2D mechanism modeling
[3] 에디슨 홈페이지