로봇팔 만들기 – 3. 오픈소스 로봇팔 DIY (제어부)

by godstale2 · 2015년 03월 20일

오픈소스 로봇팔 DIY 마지막 강좌입니다.

이제 로봇팔을 제어할 수 있도록 조이스틱을 더하고 아두이노와 배터리를 이용해서 회로를 만들겁니다.

여기서는 손 부분은 빼버렸습니다. 손 부분 동작이 시원찮기도하고, 펜이나 다른 물체를 연결해서 쓸 수 있도록 하려구요. 따라서 소스코드 상에도 손 부분을 제어하는 코드는 없습니다.(서보모터 3개만 제어합니다.) 이 부분은 MeArm 원작자가 구현한 소스코는 4개의 서보모터를 모두 제어하므로 그걸로 테스트 해보셔도 좋습니다. 링크에서 확인하세요.

completed02

4. 연결방법

팔 움직임을 위해 아래처럼 생긴 조이스틱 모듈을 이용합니다. (PC로도 제어 가능. 이 부분은 뒤에 설명) 조이스틱에 대한 상세한 내용은 링크를 참고하세요.

464441753_660

조이스틱의 XY축 방향으로의 움직임을 analog 신호로 보내줍니다. 따라서 아두이노 아날로그 핀(A0~A5)으로 연결해서 들어오는 값을 읽으면 0~1023 까지의 값을 읽을 수 있습니다. X(horizonal)축 Y(vertical)축 방향의 움직임이 감지되면 (조이스틱 움직임이 없을 때, 500 기준) 이 값의 크기에 따라 팔의 회전과 앞 뒤 움직임을 담당하는 서보모터를 움직여주면 됩니다.

그런데 팔의 회전은 지지대에 달린 서보모터를 돌려서 만들면 되는데… 팔의 앞-뒤, 상-하 움직임은 서보모터 2개가 동시에 관련되어 있습니다. 이걸 어떻게 하느냐는 소스코드 만드는 사람 나름입니다. 여기서는 어떻게 하느냐면…

조이스틱 모듈에는 버튼도 탑재되어 있습니다. 마우스 휠이 클릭도 가능하듯이 조이스틱도 누르면 버튼 클릭이 됩니다. 그래서 5개의 핀 중 하나가 버튼이 눌려졌는지는 알려주는 핀(Button interface)입니다. 이 핀을 아두이노 digital 핀에 연결하고 버튼이 눌려졌는지 확인합니다. 버튼이 눌려지면 제1 관절과 제2 관절을 번갈아 선택하도록 합니다. 그렇게 앞-뒤, 상-하 움직임은 버튼으로 2개의 서보모터를 선택하고 Y(Vertical) 방향의 움직임으로 서보모터의 회전각을 컨트롤 해서 구현합니다.

아래와 같이 회로를 구성합니다. 마이크로 서보모터 3~4개는 아두이노로 감당하기에 전류 소모량이 버겁습니다. 그래서 외부의 배터리를 사용합니다.

MeArm_custom_bb

조이스틱 -> 아두이노로 다음과 같이 연결합니다.

VCC –> 5V
GND –> GND
V(Vertical) –> A1
H(Horizontal) –> A2
KEY(or S or SEL) –> D4

서보모터의 갈색(흑색), 빨간색 선은 각각 외부전원의 (-), (+)로 연결되도록 합니다. 3개의 서보모터 황색 신호선은 D4(구동부 회전), D5(제1관절), D9(제2관절) 핀으로 연결되도록 합니다. 아두이노 GND 핀과 외부전원 (-)는 서로 연결되어야 합니다.

5. 소스코드

아래 소스코드를 아두이노에 업로드 합니다.

#include <Servo.h> 

//----- Servo settings
Servo servo_joint1;
Servo servo_joint2;
Servo servo_joint3;

int servo_joint1_pin = 5;
int servo_joint2_pin = 6;
int servo_joint3_pin = 9;

int cur_angle_joint1 = 0; // current servo position in degrees
int cur_angle_joint2 = 0;
int cur_angle_joint3 = 0;

int current_servo_index;    // [1: joint1], [2: joint2], [3: claw]

int CENTER_SERVO = 90;  // center servo position
int joint1_min_angle     =  45;  // minimum servo position
int joint1_max_angle     =  135; // maximum servo position
int joint2_min_angle     =  75;  // minimum servo position
int joint2_max_angle     =  180; // maximum servo position
int joint3_min_angle     =  30;  // minimum servo position
int joint3_max_angle     =  180; // maximum servo position
int turnRate     =  1;  // servo turn rate increment (larger value, faster rate)  

//----- Joystick settings
const unsigned long BUTTON_READ_INTERVAL = 200;
const unsigned long SEL_CHANGE_INTERVAL = 750;
const int VERT = 1; // analog A1
const int HORIZ = 2; // analog A2
const int SEL = 4; // digital D4
const int SEL_LED = 13;
boolean selStatus = false;    // false = joint2 is selected(13 LED off), true = joint3 is selected(13 LED on)
unsigned long last_button_input = 0;
unsigned long last_sel_change = 0;


void setup() {
  Serial.begin(9600);
  Serial.println("Robot Arm Control");
  Serial.println("By pressing 1, 2, 3 you can select servo");
  Serial.println("Press a or s to move, spacebar to center");
  Serial.println();
  
  servo_joint1.attach(servo_joint1_pin);
  servo_joint2.attach(servo_joint2_pin);
  servo_joint3.attach(servo_joint3_pin);
  
  cur_angle_joint1 = CENTER_SERVO;
  cur_angle_joint2 = CENTER_SERVO;
  cur_angle_joint3 = CENTER_SERVO;
  
  servo_joint1.write(cur_angle_joint1);
  servo_joint2.write(cur_angle_joint2);
  servo_joint3.write(cur_angle_joint3);
  
  changeServo(1);    // Set joint1 as current servo
  
  pinMode(SEL,INPUT);    // Set button pin as input mode
  pinMode(SEL_LED, OUTPUT);    // Joint2, Joint3 status LED
}


void loop() {
  // Read serial input
  if (Serial.available() > 0) {
    char command = Serial.read();

    // Servo control command: 'a'==97, 's'==115, ' '==32
    if (command == 97) { moveServo(-1); }  // move -1 step
    else if (command == 115) { moveServo(1); }  // move 1 step
    else if (command == 32) { moveServo(0); }  // 0 step means 'move to center position'
    // Select servo command: '1'==49, '2'==50, '3'==51
    else if (command == 49) { changeServo(1); }  // select joint1 servo
    else if (command == 50) { changeServo(2); }  // select joint2 servo
    else if (command == 51) { changeServo(3); }  // select claw servo
  }
  
  // Read button input
  unsigned long current = millis();
  if(current - last_button_input > BUTTON_READ_INTERVAL) {
    int vertical, horizontal, select;
    vertical = analogRead(VERT); // will be 0-1023
    horizontal = analogRead(HORIZ); // will be 0-1023
    select = digitalRead(SEL); // will be HIGH (1) if not pressed, and LOW (0) if pressed
    
    int verti_move, hori_move;
    verti_move = (vertical - 500) / 100;    // Scaling joystick movement to servo movement
    hori_move = (horizontal - 500) / 100;
    hori_move = hori_move * -1;    // convert direction
    
    if(verti_move != 0) {
      if(selStatus) {
        // Joint3 mode
        verti_move = verti_move * -1;    // convert direction
        changeServo(3);
        moveServo(verti_move);
      } else {
        // Joint2 mode
        changeServo(2);
        moveServo(verti_move);
      }
    }
    if(hori_move != 0) {
      changeServo(1);
      moveServo(hori_move);
    }
    if(select == LOW && current - last_sel_change > SEL_CHANGE_INTERVAL) {
      selStatus = !selStatus;
      digitalWrite(SEL_LED, selStatus);
      last_sel_change = current;
    }
    Serial.print("# Joystick: vert=");
    Serial.print(verti_move);
    Serial.print("# Joystick: hori=");
    Serial.print(hori_move);
    Serial.print("# Joystick: sel=");
    Serial.println(select);
    
    last_button_input = current;
  }
}


void changeServo(int index) {
  if(index == 1) {
    current_servo_index = 1;
    Serial.println("# Joint1 servo selected");
  } else if(index == 2) {
    current_servo_index = 2;
    Serial.println("# Joint2 servo selected");
  } else {
    current_servo_index = 3;
    Serial.println("# Claw servo selected");
  }
}

// Move servo to target angle
void moveServo(int steps) {
  int c_angle = 0;
  int target_angle = 0;
  
  if(current_servo_index == 1) {
    c_angle = cur_angle_joint1;
    if(steps == 0)
      target_angle = CENTER_SERVO;
    else
      target_angle = cur_angle_joint1 + steps*turnRate;
    if(target_angle > joint1_max_angle) target_angle = joint1_max_angle;
    if(target_angle < joint1_min_angle) target_angle = joint1_min_angle;
  }
  else if(current_servo_index == 2) {
    c_angle = cur_angle_joint2;
    if(steps == 0)
      target_angle = CENTER_SERVO;
    else
      target_angle = cur_angle_joint2 + steps*turnRate;
    if(target_angle > joint2_max_angle) target_angle = joint2_max_angle;
    if(target_angle < joint2_min_angle) target_angle = joint2_min_angle;
  }
  else {
    c_angle = cur_angle_joint3;
    if(steps == 0)
      target_angle = CENTER_SERVO;
    else
      target_angle = cur_angle_joint3 + steps*turnRate;
    if(target_angle > joint3_max_angle) target_angle = joint3_max_angle;
    if(target_angle < joint3_min_angle) target_angle = joint3_min_angle;
  }
  
  int step_angle = 1;
  if(target_angle < c_angle) { step_angle = -1; }
  
  // This code makes smooth movement
  for(int i = c_angle; i != target_angle; i+=step_angle) {
    //if((target_angle - i) < step_angle && (target_angle - i) > step_angle * -1)  // close to target position, break this loop
    //  break;
    if(current_servo_index == 1) {
      servo_joint1.write(i);
    }
    else if(current_servo_index == 2) {
      servo_joint2.write(i);
    }
    else {
      servo_joint3.write(i);
    }
    delay(15);
  }
  
  if(current_servo_index == 1) {
    cur_angle_joint1 = target_angle;
  }
  else if(current_servo_index == 2) {
    cur_angle_joint2 = target_angle;
  }
  else {
    cur_angle_joint3 = target_angle;
  }
  
  // for debug
  if(current_servo_index == 1) {
    Serial.print("Servo 1 angle = ");
    Serial.println(cur_angle_joint1);
  }
  else if(current_servo_index == 2) {
    Serial.print("Servo 2 angle = ");
    Serial.println(cur_angle_joint2);
  }
  else {
    Serial.print("Servo 3 angle = ");
    Serial.println(cur_angle_joint3);
  }
}

업로드 후 조이스틱으로 테스트를 해보세요. 서보모터의 회전범위가 적절한지 확인해야 합니다.

조이스틱 모듈이 없으신 분은 PC에서 아두이노 개발환경 – Serial Monitor를 실행합니다. 상단 입력창에 아래 명령을 입력하면 관절들을 움직일 수 있습니다.

1 : 구동부 회전 서보모터를 선택 (좌우회전용)
2 : 제1관절 서보모터를 선택
3 : 제2관절 서보모터를 선택
a : 서보모터의 회전축 위치를 1′ 줄임
s : 서보모터의 회전축 위치를 1′ 늘림
스페이스(공백) : 서보모터를 중앙 위치로 (90′)

여러개의 명령어를 동시에 내릴수도 있습니다. [1aaaaa] 이렇게 입력하고 전송하면 1번 서보모터의 회전각을 20′ 줄입니다.

테스트를 해보시고 만약 서보모터의 회전각을 좀 더 늘리거나 줄여야 한다면 소스코드의 아래 부분을 손봐주면 됩니다. PC에서 아두이노 개발환경 – Serial Monitor 창을 켜두면 현재 움직이는 모터와 회전각을 알 수 있습니다.

int joint1_min_angle     =  45;  // minimum servo position
int joint1_max_angle     =  135; // maximum servo position
int joint2_min_angle     =  75;  // minimum servo position
int joint2_max_angle     =  180; // maximum servo position
int joint3_min_angle     =  30;  // minimum servo position
int joint3_max_angle     =  180; // maximum servo position

서보모터는 0~180′ 전체를 움직이지는 못합니다. 0′, 180′ 근방에 한계가 있습니다. 이건 서보모터마다 틀립니다. 소스코드로 해결될 범위가 아닌 경우 조립을 풀고 서보모터에 연결된 관절의 위치를 조정하고 다시 조립해야 합니다.

보정 작업이 끝나면 로봇팔을 가지고 이런저런 실험을 해보세요!!

completed03

완성품 테스트 영상

아이가 가지고 놀 장난감 만든다는 핑계로 만든 로봇팔인데, 아이가 가지고 놀기에는 너무 허약합니다. 더 좋은 넘으로 업그레이드가 절실하네요.

로봇팔 만들기 – 3. 오픈소스 로봇팔 DIY (제어부)

4. 연결방법

5. 소스코드

완성품 테스트 영상

You may also like...

최신 글

카테고리

로봇팔 만들기 – 3. 오픈소스 로봇팔 DIY (제어부)

4. 연결방법

5. 소스코드

완성품 테스트 영상

You may also like...

게임 메이커: 아두이노 게임기 만들기 PART7 – 총알과 해골 움직임, 마무리

아두이노를 위한 프로그래밍 기초 4 – 지역변수, 클래스

아두이노 보드 종류 및 선택 가이드

최신 글