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

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기 활동의 일환으로 체험 제품을 제공받아 작성되었습니다."