스마트 액츄에이터

1. 스마트 액츄에이터란?

 

모바일 매니퓰레이터 블랙베리의 매니퓰레이터와 모바일베이스는 로보티즈社 의 Dynamixel XL430-W250-T(이하 XL430-W250) 모터로 구성되어 있습니다. 스마트 액츄에이터란 모터와 모터 제어기가 하나의 모듈로 구성된 모터를 의미합니다.

XL430-W250 모터는 회전 속도를 제어해 바퀴처럼 사용하거나 위치(각도)를 제어해 관절처럼 사용할 수 있고, 모터의 현재 위치 와 속도, 온도 등의 정보를 받을 수 있습니다. 블랙베리는 이 모터를 매니퓰레이터에 4개, 모바일베이스에 4개씩 사용하고있습니다.

블랙베리에서 사용하는 XL430-W250-T 모터는 5V TTL 레벨의 반이중 직렬통신 방식을 사용합니다. 블랙베리에는 아두이노 메가 보드의 UART통신(구체적으로는 5V TTL레벨 전이중 직렬통신)을 5V TTL레벨 반이중 통신으로 변환하는 회로가 내장 되어있어 모터와 보드의 통신 방식이 달라도 모터를 제어할 수 있습니다. 정해진 프로토콜 대로 보드에서 모터에 패킷을 전송하면 모터에 내장된 제어기가 패킷의 내용을 처리하고 모터를 제어합니다. 참고로 블랙베리에서는 모터를 제어하기 위해 Serial1객체를 사용합니다.

 

 

 

 

 

2. 스마트 액츄에이터 제어 라이브러리 (1)

 

XL430-W250 모터는 Dynamixel2Arduino 라이브러리를 사용하여 쉽게 제어할 수 있습니다.

Dynamixel2Arduino 라이브러리를 사용하기 위해 아두이노 소프트웨어에서 라이브러리를 추가해야 합니다.

1.Arduino 소프트웨어의 [스케치] 메뉴에서 “라이브러리 포함하기” è “라이브러리 관리”를 선택합니다.

2.라이브러리 매니저 창이 뜨면 “Dynamixel2Arduino”을 검색합니다.

“설치” 버튼을 클릭해 Dynamixel2Arduino의 최신 버전을 설치합니다.

 

 

 

 

 

 

3. 스마트 서보 모터 제어 라이브러리 (2)

 

Dynamixel 모터의 특징으로는 내부에 EEPROM 영역과 RAM 영역을 가진 컨트롤 테이블이 존재한다는 것입니다.

EEPROM 영역은 비휘발성 영역으로, 모터에 전원이 인가되지 않을 때도 유지되어야 하는 모터 설정 값들이 저장 되어있습니다.

RAM 영역은 휘발성 영역으로, 모터 제어에 필요한 값(위치, 속도 등)들이 존재하며 이 값들을 읽거나 수정하여 모터를 제어합니다. 전원이 인가될 때 마다 기본값으로 설정됩니다.

컨트롤 테이블은 XL430-W250 매뉴얼을 참고하세요. (https://emanual.robotis.com/docs/kr/dxl/x/xl430-w250/#%EC%BB%A8%ED%8A%B8%EB%A1%A4-%ED%85%8C%EC%9D%B4%EB%B8%94)

 

주의. EEPROM 영역의 값은 모터의 Torque가 Off일 때만 변경할 수 있습니다.

 

주소 : 데이터의 값의 위치를 나타냅니다

크기 : 데이터의 크기를 Byte 단위로 나타냅니다.

명칭 : 데이터의 이름입니다.

접근 : R는 읽기만 가능, RW는 읽기와 쓰기 모두 가능하다는 의미입니다.

기본값 : EEPROM의 기본값은 공장 초기화 시, RAM 영역의 기본값은 장치에 전원이

인가되었을 때 자동으로 설정되는 값을 의미합니다.

범위 : 해당 데이터가 가지는 값의 범위입니다.

단위 : 데이터의 값 1이 실제로 대응되는 값을 의미합니다.

 

 

 

 

 

 

 

 

 

 

 

4. 스마트 액츄에이터 제어 라이브러리 (3)

 

모터 제어를 위한 함수로는 간단하게 아래와 같은 것들이 있습니다.

Dynamixel2Arduino 클래스의 전체 함수는 깃허브의

ROBOTIS_GIT/Dynamixel2Arduino 리포지토리에 Dynamixel2Arduino/src/Dynamixel2Arduino.h를 참고하세요.

(https://github.com/ROBOTIS-GIT/Dynamixel2Arduino/blob/master/src/Dynamixel2Arduino.h)

 

 

또한, Dynamixel2Arduino 클래스는 Master 클래스를 상속받기 때문에 Master 클래스의 함수들을 사용할 수 있습니다. 후에 나오는 예제에서는 Master 클래스의 함수를 사용하기도 합니다.

Master 클래스의 전체함수는 깃허브의

ROBOTIS_GIT/Dynamixel2Arduino 리포지토리에 Dynamixel2Arduino/src/utility/master.h를 참고하세요.

(https://github.com/ROBOTIS-GIT/Dynamixel2Arduino/blob/master/src/utility/master.h)

 

 

5. 스마트 액츄에이터의 LED 제어 실습 (1)

 

여기서는 XL430-W250 모터와 통신하여 모터의 LED를 제어하는 실습을 할 것입니다.

XL430-W250 모터에는 빨간색 LED가 있어서 모터의 에러 상태를 표시하거나 직접 제어할 수 있습니다.

컨트롤 테이블에서 LED를 제어하기 위한 데이터의 주소는 65이며 데이터의 크기는 1바이트 입니다.

 

 

 

 

 

 

컨트롤 테이블의 값을 읽고 수정하기 위해 아래 함수들을 사용합니다.

 

bool writeControlTableItem(uint8_t item_idx, uint8_t id, int32_t data, uint32_t timeout = 100)

int32_t readControlTableItem(uint8_t item_idx, uint8_t id, uint32_t timeout = 100)

 

item_idx : 컨트롤 테이블 아이템의 주소입니다. Enum ControlTableItemIndex 을 참조하여 아이템 이름을 사용할 수 있습니다.

id : 사용하려는 모터의 아이디입니다.

data : 컨트롤 테이블에 저장 할 값입니다. Write 함수에만 사용합니다.

timeout : 시간제한을 ms 단위로 지정합니다. 값을 지정하지 않으면 100ms가 기본 값입니다.

 

 

6. 스마트 액츄에이터의 LED 제어 실습 (2)

 

Ping 함수를 사용하면 모터를 제어하기에 앞서 모터와 통신이 가능한 상태인지 확인할 수 있습니다.

사용 가능한 ping함수는 아래와 같습니다.

 

bool ping(uint8_t id = DXL_BROADCAST_ID)

uint8_t ping(uint8_t id, InfoFromPing_t *recv_ping_info_array, uint8_t recv_array_cnt, uint32_t timeout_ms = 10)

uint8_t ping(uint8_t id, uint8_t *p_recv_id_array, uint8_t recv_array_capacity, uint32_t timeout_ms = 10)

 

각 인자가 의미하는 바는 아래와 같습니다.

recv_ping_info_array : ping 체크 결과를 저장할 InfoFromPing_t 구조체 또는 구조체 배열의 포인터를 지정합니다.

recv_array_cnt : 인자로 넘긴 구조체 또는 구조체 배열의 길이를 지정합니다.

p_recv_id_array : ping 체크 결과, 통신 가능한 모터들의 아이디를 저장할 배열의 포인터를 지정합니다.

recv_array_capacity : 인자로 넘긴 배열 길이를 지정합니다.

timeout_ms : id 매개변수에 브로드캐스트 아이디를 넘기면 모든 모터에 ping instruction 패킷을 보냅니다. 이후 모든 모터가 status packet으로 응답하는데 모터 ID값이 클 수록 status packet을 늦게 보냅니다. 모터가 많으면 timeout_ms 값이 커야 모든 모터의 status packet을 받을 수 있습니다.

 

각 함수는 아래와 같은 값을 반환합니다.

 

Bool ping(uint8_t id = DXL_BROADCAST_ID)

: 핑 체크에 성공하면 true, 아니면 false를 반환합니다.

 

uint8_t ping(uint8_t id, InfoFromPing_t *recv_ping_info_array, uint8_t recv_array_cnt, uint32_t timeout_ms = 10)

: 핑 체크에 성공하면 통신 가능한 모터의 ping 정보를 반환합니다.

 

uint8_t ping(uint8_t id, uint8_t *p_recv_id_array, uint8_t recv_array_capacity, uint32_t timeout_ms = 10)

: 핑 체크에 성공하면 통신 가능한 모터의 개수를 반환합니다.

 

 

7. 스마트 액츄에이터의 LED 제어 코드 분석 (1)

 

 

 

(1) 모터 제어를 위해 Dynamixel2Arduino.h 헤더파일을 코드에 포함시킵니다.

 

(2) 모터 제어를 위한 Serial객체, dir 핀 번호, 제어할 모터 아이디(7번), 프로토콜 버전(기본 값 2.0)을 정의합니다. 정의한 값을 사용해 Dynamixel2Arduino의 인스턴스를 dxl이라는 이름으로 생성하고, 컨트롤 테이블 아이템의 이름을 사용하기 위한 네임스페이스 사용을 선언합니다.

 

(3) setup에서는 시리얼 모니터에 데이터를 출력하기 위해 Serial 객체를 115200 속도로 통신하도록 초기화 합니다. 모터는 57600 속도(기본 값)로 통신하기 때문에 dxl 객체를 57600 속도로 통신하도록 초기화 합니다

 

 

8. 스마트 액츄에이터의 LED 제어 코드 분석 (2)

 

 

 

(4) loop에서는 모터 프로토콜 버전과 찾으려는 모터 아이디를 시리얼 모니터에 출력합니다.

 

(5) ping 함수는 7번 모터와의 통신 연결 상태를 1(true, 모터 감지됨) 또는 0(false, 모터 감지되지 않음) 으로 반환합니다. ping 함수의 결과 값을 사용하면 모터가 감지되었을 때만 모터 동작을 하도록 코드를 작성할 수 있습니다.

 

1)모터가 감지되면 해당 모터의 모델 넘버를 출력합니다.

(I).LED의 현재 상태 값을 읽어와서 분기합니다. 상태 값이 1이면 컨트롤 테이블에 0으로 값을 써서 LED를 끕니다. 상태 값이 0이면 컨트롤 테이블이 1로 값을 써서 LED를 켭니다.

 

2)모터가 감지되지 않으면 에러코드를 출력합니다.

 

(6) 1초간 대기합니다.

 

(7) 통신 가능한 모든 모터의 정보를 출력합니다.

 

7번 모터와 통신이 정상적으로 연결되었다면, 2초에 한 번씩 LED가 점멸하는 동작을 하게 됩니다.

 

 

 

9. 스마트 액츄에이터의 LED 제어 코드 분석 (3)

 

 

FindServos 함수의 내용을 좀 더 자세히 살펴봅니다.

(8) DXL_BROADCAST_ID로는 모터 통신 버스에 있는 모든 모터의 ping을 체크할 수 있습니다. ping 함수에서 반환된  count_pinged 값이 0(통신 가능한 모터 없음을 의미)인지 아닌지에 따라 분기합니다.

 

1)count_pinged 값이 0이 아니라면

 

(I) count_pinged 값 만큼 i가 증가하면서 ping함수에 의해 업데이트 된 ping_info에 있는 id, model, firmware ver값을 시리얼 모니터에 출력합니다.

 

2)count_pinged 값이 0이라면 에러코드를 출력합니다.

 

 

 

 

 

//////////////  모터 구동 관련 선언
#include <Dynamixel2Arduino.h>

#define DXL_SERIAL   Serial1
const int DXL_DIR_PIN = 2; // DIR PIN

const uint8_t DXL_ID = 7;
const float DXL_PROTOCOL_VERSION = 2.0;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);

// 컨트롤 테이블 아이템의 이름을 사용하기 위해 이 네임스페이스가 필요함
using namespace ControlTableItem;


//////////////  메인 프로그램

void setup() {
  Serial.begin(115200);

  dxl.begin(57600);
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
}

void loop() {
  Serial.print("PROTOCOL ");
  Serial.print(DXL_PROTOCOL_VERSION, 1);
  Serial.print(", ID ");
  Serial.print(DXL_ID);
  Serial.print(": ");
  if(dxl.ping(DXL_ID) == true){  // 핑 체크 성공
    Serial.print("ping succeeded!");  // 모델 넘버를 출력
    Serial.print(", Model Number: ");
    Serial.println(dxl.getModelNumber(DXL_ID));
    
    if (dxl.readControlTableItem(DXL_LED, 7) == 1) {  // 현재 LED 상태가 On이면
      dxl.writeControlTableItem(DXL_LED, 7, 0);  // LED 끄기
    } else {  // 현재 LED 상태가 Off이면
      dxl.writeControlTableItem(DXL_LED, 7, 1);  // LED 켜기
    }
  }else{  // 핑 체크 실패
    Serial.print("ping failed!, err code: ");
    Serial.println(dxl.getLastLibErrCode());  // 에러코드 출력
  }
  delay(1000);

  FindServos();  // 통신 가능한 모든 모터의 정보를 출력하는 함수
}


/*
 * 통신 가능한 모든 모터를 찾아 모델 넘버와 펌웨어 버전을 출력하는 함수
 */
DYNAMIXEL::InfoFromPing_t ping_info[32];
void FindServos(void) {
  Serial.println("  Try Protocol 2 - broadcast ping: ");
  Serial.flush(); // flush it as ping may take awhile... 
      
  if (uint8_t count_pinged = dxl.ping(DXL_BROADCAST_ID, ping_info, 
    sizeof(ping_info)/sizeof(ping_info[0]), 100)) {
    Serial.print("pinged count : ");
    Serial.println(count_pinged);
    Serial.print("Detected Dynamixel : \n");
    for (int i = 0; i < count_pinged; i++)
    {
      Serial.print("    ");
      Serial.print(ping_info[i].id, DEC);
      Serial.print(", Model:");
      Serial.print(ping_info[i].model_number);
      Serial.print(", Ver:");
      Serial.println(ping_info[i].firmware_version, DEC);
    }
  } else{
    Serial.print("Broadcast returned no items : ");
    Serial.println(dxl.getLastLibErrCode());
  }
}