아두이노 로봇팔 코드 - adu-ino lobospal kodeu

※ 아두이노 실행 프로그램이 설치되어 있지 않거나, 아두이노를 처음 접하시는 분들은 아래 버튼을 클릭해 사용법을 확인하시길 바랍니다.

목차 Show

로봇팔 만들기 – 3. 오픈소스 로봇팔 DIY (제어부)
로봇팔 만들기 – 3. 오픈소스 로봇팔 DIY (제어부)
4. 연결방법
5. 소스코드
완성품 테스트 영상

본 게시글은 로봇팔 조립을 모두 마친 이후, 아두이노에 소스 코드를 업로드하는 과정을 담고 있습니다.

완성한 로봇팔이 정상적으로 작동하기 위해, 조립된 서보모터의 각도를 초기화하는 작업이 꼭 필요합니다.

초기화 코드를 아두이노에 업로드하기 전, 반드시 서보모터와 연결된 프레임을 분리한 후 코드를 실행해주어야 합니다.

프레임을 분리하지 않은 채로 코드를 실행하게 되면 고정되어 있는 프레임 때문에 서보모터의 톱니(각도)가 움직일 수 없어 고장이 날 수 있습니다.

아래 사진을 참고하여 차근히 따라하시길 바랍니다.

소스코드는 게시글의 맨 하단에서 다운받을 수 있습니다.

로봇팔 반복 동작 시키기

내가 움직이는 조이스틱 값(x, y) 확인하기

조이스틱으로 로봇팔 움직이기

로봇팔 초기화 시키기

완성된 소스 코드에 대한 간략한 설명입니다.

아래 내용을 참고하여 조이스틱의 움직임(x, y 값)에 따라 로봇팔의 각도를 조절할 수 있습니다.

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

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

로봇팔 만들기 – 1. 손 (MECHANICAL CLAWS)
로봇팔 만들기 – 2. 관절 (JOINT)
로봇팔 만들기 – 3. 오픈소스 로봇팔 DIY (기구부 1)
로봇팔 만들기 – 3. 오픈소스 로봇팔 DIY (기구부 2)

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

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

4. 연결방법

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

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

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

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

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

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

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′ 근방에 한계가 있습니다. 이건 서보모터마다 틀립니다. 소스코드로 해결될 범위가 아닌 경우 조립을 풀고 서보모터에 연결된 관절의 위치를 조정하고 다시 조립해야 합니다.

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