본문 바로가기
Robot/Device Control

허큘렉스 서보모터 제어 기초

by lifeseed 2014. 4. 14.
반응형

Reference ::

[1] DRS-0101 User Guide

 

 

0. 서보모터 제어 구성

HerkuleX 서보모터 DRS-0101은 UART를 통해 제어 된다.

연결도는 다음과 같으며, 제어기로부터 TX를 통해 Request Packet가 보내지고, RX를 통해 제어기로 Ack Packet로 들어온다.

각각의 모터는 서로다른 ID값을 가지고 있으며, ID를 통해 각각의 서보모터에 명령을 전달하거나 상태를 읽어올 수 있다.

 

 

UART의 통신설정은 다음과 같다.

Default Baud Rate는 115200 이며, 특별한 문제가 없다면 제어기에 연결된 Host의 UART를 115200으로 설정하면 무난하게 동작할 수 있다.

 

 

1. PACKET Format

서보모터를 위해 Request 및 ACK Packet를 보내거나 받게 되는데, 이 때 Packet의 Format은 다음과 같이 고정 7 Byte에 필요에 따라 추가적인 데이터가 추가되며 고정 Packet을 포함하여 최대 223 Byte 까지 송 수신이 가능하다.

 

1) Header  (2 Byte)

0xFF 0xFF 의 2 Byte로 구성된다.  Packet의 시작을 나타낸다.

 

2) Packet Size (1 Byte)

고정 Packet Size를 포함한 총 Packet의 길이를 나타낸다.

즉, 최소값은 7이며 최대 값은 223이 된다.

 

3) pID (1 Byte)

서보모터의 고유 ID이며, 각각의 서보 모터는 0 ~ 0xFD 까지 고유의 ID를 가진다. ID를 0xFE로 설정할 경우 Broadcast 모드로 동작하며, 연결된 모든 서보모터에 동일한 명령이 전달된다.

 

4) CMD (1 Byte)

서보 모터에 전달되는 명령으로 1~9 까지는 Request Command 로 할당되어 있으며, 0x41~0x49 는 ACK type으로 할당되어 있다.

 

5) CheckSum1 (1 Byte)

Packet의 무결성을 검토하기 위해 CheckSum1 과 CheckSum2를 Packet에 추가하여 보낸다.

CheckSum1의 계산 공식은 다음과 같다.

CheckSum1 = (PacketSize^pID^CMD^Data[0]^Data[1]^...^[Data[n])&0xFE

※ A^B : A와 B의 Bit Exclusive OR 연산. A와 B의 각 Bit를 비교해서 다를경우 1, 같으면 0이 되도록 하는 연산

 

C Source Code

 
  1. unsigned char getChksum1(unsigned char * buf){
  2.     unsigned char CHksum1;
  3.     unsigned char i
  4.    
  5.     CHksum1 = (buf[2])^(buf[3])^(buf[4]);
  6.  
  7.     for(i=0; i<buf[2]-7; i++ )
  8.         CHksum1 ^= buf[i+7];
  9.        
  10.     return CHksum1&0xFE;
  11. }

 

 

 

 

6) CheckSum2 (1 Byte)

CheckSum1과 함께 Packet의 무결성을 보장하기 위한 데이터이다.

CheckSum1의 값의 Bit Not Operation을 이용한다. 

CheckSum2 = (~CheckSum1)&0xFE

※ ~A : A의 Bit Not 연산. A의 각 Bit가 0이면 1, 1이면 0이 되도록 하는 연산

 

C Source Code

 
  1. unsigned char getChksum2(unsigned char chksum1){
  2.     unsigned char CHksum2;
  3.     CHksum2 = ~chksum1 & 0xFE;
  4.     return CHksum2;
  5. } 

 

 

 

 

7) Data[n]

각 CMD의 필요에 따라 선택적으로 추가되는 Data로 개수는 각 CMD마다 다르며, 없을수도 있다.

 

 

2. Command Set

1) Request Packet - to Servo Motor

 항목

CMD 

Description 

EEP_WRITE

 0x01

EEP Register Address에서 Length개의 값 변경

추가 데이터 길이 : 2 + length byte

EEP_READ

 0x02 

EEP Register Address에서 Length개의 값 요청

r(ACK Policy)의 설정에 따라 응답하지 않을 수 있음

추가 데이터 길이 : 2 byte

RAM_WRITE

 0x03

RAM Register Address에서 Length개의 값 변경

추가 데이터 길이 : 2 + length byte

RAM_READ

 0x04

RAM Register Address에서 Length개의 값 요청

r(ACK Policy)의 설정에 따라 응답하지 않을 수 있음

추가 데이터 길이 : 2 byte

I_JOG

 0x05

최대 43개의 서보에 JOG 명령 전송

I_JOG는 각 서보의 동작 시간 개별 설정 가능

S_JOG

 0x06

최대 53개의 서보에 JOG 명령 전송

S_JOG는 각 서보의 동작 시간 모두 동일

STAT

 0x07

Status Error, Status Details 요청

r(ACK Policy)와 무관하게 항상 응답

ROLLBACK

 0x08

Factory Default값으로 모든 EEP Register를 변경

변경된 설정은 전원 Off후 재 기동시 적용

ID및 Baud Rate는 Skip설정에 따라 초기화에서 제외 가능

REBOOT

 0x09

Rebooting 요청

 

2) ACK Packet - to Controller

 항목

CMD 

Description 

EEP_WRITE

 0x41

CMD 0x01의 응답 Packet

Default상태는 응답하지 않지만, r(ACK Policy)의 설정을 변경하면 응답

EEP_READ

 0x42 

CMD 0x01의 응답 Packet

Default상태는 응답하지 않지만, r(ACK Policy)의 설정을 변경하면 응답

RAM_WRITE

 0x43

EEP Register Address에서 n개의 값 회신

r(ACK Policy)의 설정에 따라 응답하지 않을 수 있음

RAM_READ

 0x44

r(Status Error, Status Detail) 회신

r(ACK Policy)의 설정에 따라 응답하지 않을 수 있음

I_JOG

 0x45

CMD 0x05의 응답 Packet

Default상태는 응답하지 않지만, r(ACK Policy)의 설정을 변경하면 응답

S_JOG

 0x46

CMD 0x06의 응답 Packet

Default상태는 응답하지 않지만, r(ACK Policy)의 설정을 변경하면 응답

STAT

 0x47

r(Status Error, Status Detail) 회신, r(ACK Policy)와 무관한게 항상 응답

ROLLBACK

 0x48

CMD 0x08의 응답 Packet

Default상태는 응답하지 않지만, r(ACK Policy)의 설정을 변경하면 응답

REBOOT

 0x49

CMD 0x09의 응답 Packet

Default상태는 응답하지 않지만, r(ACK Policy)의 설정을 변경하면 응답

 

※ ACK 여부는 r(ACK Policy)로 변경 가능

※ ACK Packet의 CMD는 요청 Packet CMD에 0x40을 더한 값

※ ACK Packet 마지막 2Byte는 r(Status Error, Status Detail)을 추가로 전달

 

3) Command Example :: RAM_WRITE  (CMD : 0x03)

. RAM Register Address에서 Length개의 값 변경

추가 Data 구성 

Data[0] : RAM Address

Data[1] : Length N

Data[2] : RAM Address의 0번째에 쓰여질 데이타

...

Data[2+N-1] : RAM Address의 N-1번째에 쓰여질 데이타

 

Ex) ID(253), RAM Address(0x35(53))의 값을 1로 설정

 

Header (0, 1)

Packet Size (2)

pID (3)

CMD (4)

CheckSum1 (5)

CheckSum2 (6)

0xFF 0xFF

7+(2+1) : 0x0A

0xFD

0x03

0xC0

0x3E

 

Data[0] (7)

Data[1] (8) 

Data[2] (9) 

-

-

 RAM Address

Length 

Data 0 

-

-

-

 0x35

0x01 

0x01 

 

 

3. Torque Control 및 LED Control

Torque 및 LED는 RAM 에 약속된 값을 써 주는 것으로 동작을 시킬 수 있다.

따라서 RAM_WRITE (0x03) 명령을 이용하여 Control한다.

 

1) Torque Control

RAM Address : 52 (0x34)

Byte : 1 Byte

Valid Range :  MASK 0x60

 

- 0x00 : Torque Free. 손으로 관절을 쉽게 움직일 수 있는 상태

- 0x40 : Break On. I_JOG 및 S_JOG 의 명령이 실행되지 않음.

- 0x60 : Torque On. I_JOG 및 S_JOG 명령을 이용하여 모터를 움직일 수 있음

 

C Source Code

 

  1. void TorqueOnOff(unsigned char id, int bOn)
  2. {
  3.     unsigned char packet[255];
  4.  
  5.     packet[0] = 0xFF;
  6.     packet[1] = 0xFF;
  7.     packet[2] = 0x0A;   // 7 + 3
  8.     packet[3] = 0x03;   // RAM WRITE
  9.     packet[4] = id; // pID
  10.     packet[7] = 0x34;   // ram address
  11.     packet[8] = 0x01;   // length
  12.     if(bOn == 0)
  13.         packet[9] = 0x00;   // torque off
  14.     else
  15.         packet[9] = 0x60;   // torque on
  16.  
  17.     packet[5] = getChksum1(packet);     // chksum1
  18.     packet[6] = getChksum2(packet[5]);        // chksum2
  19.  
  20.     sendpacket(packet, 10);
  21. } 

 

 

 

2) LED Control

RAM Address : 53 (0x35)

Byte : 1 Byte

Valid Range :  0x0~0x7

 

- 0x01 : Green On

- 0x02 : Blue On

- 0x04 : Red On

 

C Source Code

 

  1. void LEDOnOff(unsigned char id, int bGreen, int bBlue, int bRed)
  2. {
  3.     unsigned char packet[255];
  4.  
  5.     packet[0] = 0xFF;
  6.     packet[1] = 0xFF;
  7.     packet[2] = 0x0A;   // 7 + 3
  8.     packet[3] = 0x03;   // RAM WRITE
  9.     packet[4] = id; // pID
  10. //  packet[5] = ;   // chksum1
  11. //  packet[6] = ;   // chksum2
  12.     packet[7] = 0x34;   // ram address
  13.     packet[8] = 0x01;   // length
  14.  
  15.     packet[9] = 0x00;   // init ram data
  16.  
  17.     if(bGreen != 0)
  18.         packet[9] = packet[9]|0x1;  // Green LED On
  19.  
  20.     if(bBlue != 0)
  21.         packet[9] = packet[9]|0x2;  // Green LED On
  22.  
  23.     if(bRed != 0)
  24.         packet[9] = packet[9]|0x4;  // Green LED On
  25.  
  26.     packet[5] = getChksum1(packet);     // chksum1
  27.     packet[6] = getChksum2(packet[5]);        // chksum2
  28.  
  29.     sendpacket(packet, 10);
  30. }

 

 

 


4. Position Control

1) I_JOG & S_JOG
서보모터의 위치 제어는 I_JOG 및 S_JOG를 이용하여 동시에 여러모터를 제어 할 수 있다.

I_JOG와 S_JOG의 차이점은 모터가 구동되는 동작 시간을 모터별로 설정할수 있느냐 없느냐이다.

즉, I_JOG는 동시에 여러대의 모터를 구동하는데, 각각 다른 동작 시간을 가지도록 설정 할 수있으며, S_JOG는 동작시간이 동일하도록 여러대의 모터를 구동하도록 되어 있다.

 

각각의 Packet구성은 다음과 같다.

[I_JOG]

Basic Packet 7 Byte : Header(2) + Size(1) + pID(1) + CMD(1) + checksum1(1) + checksum2(1)

Optional Data :

<JOG_LSB(1) + JOG_MSB(1) + SET(1) + PlayTime(1)> 제어하고자 하는 모터 개수만큼 <> 반복

 

[S_JOG]

Basic Packet 7 Byte : Header(2) + Size(1) + pID(1) + CMD(1) + checksum1(1) + checksum2(1)

Optional Data :

PlayTime(1) +

<JOG_LSB(1) + JOG_MSB(1) + SET(1)> 제어하고자 하는 모터 개수만큼 <> 반복복


SET는 모터의 구동 방법 (위치제어/무한회전) 및 LED 등의 Control을 지원하며, I_JOG및 S_JOG와의 차이점과 함께 데이터들의 구성 및 세부 Component들의 구성은 다음 표에서 확인 가능하다.

 

 

2) I_JOG를 이용한 모터 위치 제어

기본적으로 I_JOG가 S_JOG 보다 Packet Size가 커지는 단점이 있으나, 동작 적인 측면에서 S_JOG의 기능을 포함하므로, 모터의 위치제어는 LED 제어와 함께 I_JOG를 이용하여 컨트롤 하도록 한다.

 

- 모터 하나를 Control하는 C Source Code

 

  1. void movePosOne(unsigned char id, int pos, , unsigned char playtime, unsigned char led)
  2. {
  3.     unsigned char packet[255];
  4.  
  5.     if(playtime<=0x00)
  6.         playtime = 0x00;
  7.     if(playtime >= 0xFE)
  8.         playtime = 0xFE;
  9.  
  10.         if(id < 0 || id > 253) return;
  11.  
  12.     packet[0] = 0xFF;
  13.     packet[1] = 0xFF;
  14.     packet[2] = 0x0C;   // 7 + 5
  15.     packet[3] = 0x05;   // I_JOG
  16.     packet[4] = 0xFE;   // pID : BROCAST
  17.  
  18.     // Optional Data
  19.     if(pos<=0)
  20.         pos = 0;
  21.     if(pos>=1024)
  22.         pos = 1024;
  23.  
  24.     packet[7] = (unsigned char) pos;                 // LSB
  25.     packet[8] = (unsigned char) ((pos&0xFF00)>>8);  // MSB
  26.     packet[9] = (unsigned char)(led<<2);            // SET, (position + LED control)
  27.     packet[10] = (unsigned char)(id);
  28.     packet[11] = (unsigned char)(playtime);
  29.  
  30.     packet[5] = getChksum1(packet);     // chksum1
  31.     packet[6] = getChksum2(packet[5]);        // chksum2
  32.  
  33.     sendpacket(packet, 12);
  34. }

 

 

 

- 여러개의 모터를 동시에 제어하는 C Source Code

하나의 I_JOG Data에 대한 배열들을 2중으로 관리하는 이중 포인터를 사용하였다.

포인트 개념이 힘든 사람은 조금 어려울 수 있으나, 결국 set로 묶어서 데이터를 처리하기 위한 방안이라 생각하면 될 듯하다.

 

 

  1. void movePos(int **motorvalue, int numOfctrl)
  2. {
  3.     unsigned char packet[255];
  4.     int pos, led, id, playtime;
  5.     int i;
  6.  
  7.     packet[0] = 0xFF;
  8.     packet[1] = 0xFF;
  9.     packet[3] = 0x05;   // I_JOG
  10.     packet[4] = 0xFE;   // pID : BROCAST
  11.  
  12.     for(i=0; i<numOfctrl; i++)
  13.     {
  14.         pos = motorvalue[i][0];
  15.         led = motorvalue[i][1];
  16.         id = motorvalue[i][2];
  17.         playtime = motorvalue[i][3];
  18.  
  19.         if(playtime<=0x00)
  20.             playtime = 0x00;
  21.         if(playtime >= 0xFE)
  22.             playtime = 0xFE;
  23.  
  24.             if(id < 0 || id > 253) return;
  25.  
  26.         // Optional Data
  27.         if(pos<=0)
  28.             pos = 0;
  29.         if(pos>=1024)
  30.             pos = 1024;
  31.  
  32.         packet[7+i*5] = (unsigned char) pos;                 // LSB
  33.         packet[8+i*5] = (unsigned char) ((pos&0xFF00)>>8);  // MSB
  34.         packet[9+i*5] = (unsigned char)(led<<2);            // SET, (position + LED control)
  35.         packet[10+i*5] = (unsigned char)(id);
  36.         packet[11+i*5] = (unsigned char)(playtime);
  37.     }
  38.  
  39.     packet[2] = 7 + 5*i;        // packet size
  40.  
  41.     packet[5] = getChksum1(packet);     // chksum1
  42.     packet[6] = getChksum2(packet[5]);      // chksum2
  43.  
  44.     sendpacket(packet, (7+5*i));
  45. }

 

위 함수를 이용하여 2개의 모터를 제어 하는 simple test 코드는 다음과 같이 구성할 수 있다.

 

  1. void motor_test(void)
  2. {
  3.     int motor_value[2][4];
  4.  
  5.     motor_value[0][0] = pos1;
  6.     motor_value[0][1] = led1;
  7.     motor_value[0][2] = id1;
  8.     motor_value[0][3] = playtime1;
  9.  
  10.     motor_value[1][0] = pos2;
  11.     motor_value[1][1] = led2;
  12.     motor_value[1][2] = id2;
  13.     motor_value[1][3] = playtime2;
  14.  
  15.     movePos(motor_value, 2)
  16. }

 

 

3) 모터의 position 값 가져오기

- RAM 영역의 Address 0x3A (58) 에 2Byte로 Absolute Global Position 이 기록되어 있다. Read Only 값이며, RAM READ 명령을 통하여 해당 값을 가져온다.

 

Response Packet은 CMD 0x4를 가지는 Basic Packet 7Byte + RAM Address 1 Byte + Lenght 1Byte로 구성되며, Address는 앞서 언급한 것 처럼 0x3A 이며 2 Byte가 필요하므로 Lenght는 2로 설정하면 된다.

 

원하는 RAM 값은 RAM_READ 명령에 대한 ACK의 Data Field에 저장되며, 역시 Response Packet가 전송되면 CMD 0x44를 가지는 Basic Packet 7Byte + RAM Address 1 Byte + Lenght 1Byte + RAM Data Length Byte + Status 2Byte 를 수신하게 된다.

 

즉 position 값은 수신된 packet의 10번째 11번째 packet에 저장된다.

 

C Source Code

 

  1. int getPos(int id){
  2.     int position;
  3.     unsigned char packet[255];
  4.  
  5.     if (id < 0 || id > 253) return -1;
  6.  
  7.     packet[0] = 0xFF;
  8.     packet[1] = 0xFF;
  9.     packet[2] = 0x09;       // 7 + 2
  10.     packet[3] = 0x04;       // RAM READ
  11.     packet[4] = id;     // pID
  12.     packet[7] = 0x3A;       // ram address
  13.     packet[8] = 0x02;       // length
  14.  
  15.     packet[5] = getChksum1(packet);     // chksum1
  16.     packet[6] = getChksum2(packet[5]);        // chksum2
  17.  
  18.     sendpacket(packet, 9);
  19.  
  20.     usleep(10000);
  21.  
  22.     if (receivePacket() < 13) return -1;
  23.  
  24.     position = ((readbuf[9]&0xff) | ((readbuf[10]&0x03) << 8));
  25.     return position;
  26. }

 

 

5. 마치며...

허큘렉스 서보모터는 모터 각각이 제어를 위한 CPU 및 F/W가 구동되고 있으며, 이를 UART Packet을 이용하여 명령어를 주고 받으면서 동작시키는 방식이다.

따라서 모터 제어를 손쉽게 할 수 있으며, UART 병렬 연결을 이용하므로 Host Controller가 UART 포트 하나만 있어도 여러대의 모터들을 제어할 수 있는 장점이 있다.

반면... 모터에 기본적인 제어를 위한 하드웨어가 추가되다 보니 가격이 비싼것이 단점이라면 단점이라고 할 수 있겠다. 또한 기본적인 PWM제어를 직접 하면서 모터를 제어하고자 하는 이들 또한 약간은 불만(?)이 있을 수도 있겠다.

 

어쨌든.. 이로써 기본적인 모터구동을 위한 C 코드가 준비되었으며, 이 코드들은

이제 라즈베리파이 뿐만이 아니라, UART가 지원되는 MCU, 등에 포팅되어 사용가능하다.

 

"본 제품은 동부로봇에서 후원하고 아이씨뱅큐 무상체험단 13기 활동의 일환으로 체험 제품을 제공받아 작성되었습니다."

댓글1

  • imbed 2017.01.18 21:57

    시간이 많이 지나서 답변이 달릴지는 모르겠지만 몇 가지 질문을 드리고 싶습니다. 저도 Herkulex 모터를 제어하려고 공부하고 있는데요, 혹시 코드 짜실 때 동부로봇에서 제공하는 라이브러리를 포함시켜서 그 형식대로 코딩하신건가요? 또, 위 I_JOG 코드 packet 배열에서 pID와 CMD순서가 바뀌어있는데 저 코드대로 실행했을 때 모터가 움직이던가요?? 아무리 코드를 수정해도 모터가 돌아가지 않네요 ㅠㅠ
    답글