목차 1. 요약 1-1. 의개요와특징 Page 주요사양요약 Page 3 2. Dynamixel 기동 2-1. 기계부조립방법 Page Connector 조립 Page Dynamixel의배선연결 Page 5 3. Communicatio

Transcription

1 User s Manual Closer to Real, Dynamixel

2 목차 1. 요약 1-1. 의개요와특징 Page 주요사양요약 Page 3 2. Dynamixel 기동 2-1. 기계부조립방법 Page Connector 조립 Page Dynamixel의배선연결 Page 5 3. Communication Protocol 3-1. Communication 개요 Page Instruction Packet Page Status Packet Page Control Table Page Instruction Set과그사용예 4-1. WRITE DATA Page READ DATA Page REG WRITE와 ACTION Page PING Page RESET Page SYNC WRITE Page 22 ` 5. Example Page 23 ` Appendix Page 29 1

3 1. Dynamixel 1-1. 의개요와특징 Dynamixel Dynamixel은감속기, Driver, Control Unit 및 Network기능까지일체형으로구성되어있는 Module형 Smart Actuator이다. Dynamixel은 Compact size임에도불구하고큰 Torque를낼수있고, 강한외력에견딜수있는특수한재질로만들어졌다. 또한내부온도변화나공급전압의변화등내부상황을스스로인식하고처리할수있는기능을갖고있다. Dynamixel 는다음과같은장점이있다. 정밀제어 1024 단계의 resolution 으로 position 과 speed 를제어할수있다. Compliance Driving 위치제어에있어서탄력의정도를설정할수있다. Feedback 위치각, 현재속도는물론, 구동중인 Load 의크기까지 Feedback 해줄수있다. Alarm System 내부온도, Torque, 공급전압등이사용자가지정한동작범위를벗어났을때이를 알려줄뿐아니라 (Alarming) 스스로대처할수있는기능 (Torque Off) 이있다. Communication Daisy chain 으로연결되어배선이간단하며통신속도는 1M BPS 까지지원된다. Distributed Control 한번의명령 Packet 을전달하여속도, 위치, Compliance, Torque 를동시에설정할 수있기때문에메인프로세서는매우적은 Resource 로여러개의 Dynamixel 을 Control 할수있다. Engineering Plastic Main Body 에 Engineering Plastic 이사용되어큰 Torque 에도견딜수있다. Axis Bearing 기어의최종축에 Bearing 을사용하였으므로축이강한외력을받을경우도효율이 감소하지않는다. Status LED ERROR 상황을 LED 를통하여사용자에게알려준다. Frames Hinge 와 side mount frame 을기본으로제공한다 주요사양요약 2

4 Weight (g) 55 Gear Reduction Ratio 1/254 Input Voltage (V) at 7V at 10V Final Max Holding Torque(kgf.cm) Sec/60degree Resolution 0.35 Operating Angle 300, Endless Turn Voltage 7V~10V (Recommended voltage: 9.6V) Max. Current 900mA Operate Temperature -5 ~ +85 Command Signal Digital Packet Protocol Type Half duplex Asynchronous Serial Communication (8bit,1stop,No Parity) Link (Physical) TTL Level Multi Drop (daisy chain type Connector) ID 254 ID (0~253) Communication Speed 7343bps ~ 1 Mbps Feedback Position, Temperature, Load, Input Voltage, etc. Material Engineering Plastic 3

5 2-1. 기계부조립방법 2. Dynamixel 의설치 기본제공 Frames 와함께기본으로제공되는 Frame 은다음과같다. OF-12SH OF-12S OF-12SH 의적용 OF-12SH 는다음과같은방법으로적용된다. 조립방법 조립된모습 OF-12S 의적용 OF-12S 는다음과같은방법으로적용된다. OF-12S 는 의 3 면 ( 좌, 우, 아래면 ) 에 결합될수있다. Horn2Body 조립방법 조립된모습 Body2Body 조립방법 조립된모습 4

6 2-2. Connector 조립아래그림순서대로 Connector를조립한다. Wire Former를사용해서터미널을선에압착시킨다. Wire Former를사용할수없을때는터미널과선을납땜하여동작중에선이빠지지않도록한다. 5

7 2-3. Dynamixel 의배선연결 Pin Assignment Connector 의 Pin 배열은다음과같으며두개의 Connector 는 Dynamixel 내부에서 Pin2Pin 으로연결되어있으므로하나의커넥터에만연결해서 를구동할수있다. PIN1: GND PIN2: VDD PIN3: Data PIN1: GND PIN2: VDD PIN3: Data Wiring 아래그림과같이 Pin2Pin 으로연결한다. 이러한방식으로연결하여여러개의 AX- 12 들를하나의 BUS 상에서제어할수있다. Control Box CM-5 Main Controller Dynamixel 을구동할 Main Controller 는 TTL level 의 Half Duplex UART 를지원해야 한다. Main Controller 는직접자작해도좋으나 Dynamixel 전용 Controller 인 CM-5 를 사용하는것을추천한다. PC LINK CM-5 를경유하여 PC 로 Dynamixel 을제어할수있다. RS232 Level TTL Level PC CM-5 Dynamixels 6

8 bioloid CM-5 와 를연결하는것만으로도로봇을구성할수있다. CM-5 와 를 기반으로하여구성된로봇으로서 Bioloid 라는 Edutainment 만능로봇 KIT 가있다. Bioloid 로만든로봇예 Bioloid 의보다자세한내용은해당 Manual 을참조하기바란다. UART 와의연결 Dynamixel 를제어하기위해서는 Main Controller UART 의신호를 Half duplex type 으로변환시켜줘야한다. 다음은그권장회로도이다. DIRECTION_PORT 5V TXD 74HC126 10K 74HC04 RXD 74HC126 DATA DATA(PIN3) 9.6V VDD(PIN2) GND GND(PIN1) CM-5 internal circuit (HALF DUPLEX UART) 전원은 Main Controller 의 Molex3P Connector 의 Pin1, Pin2 를통하여 Dynamixel 로 공급된다. ( 위의회로는 Half duplex 방식을설명하기위해도시한것이며, CM-5 에는 이미위의회로가내장되어있으므로단지 Dynamixel 을연결만하면된다 ) 위의회로도에서 TTL Level 의 TxD 와 RxD 는 DIRECTION_PORT 의 Level 에따라다음과 같이 Data 신호의방향이결정된다. 7

9 DIRECTION_PORT 의 Level 이 High 인경우 : TxD 의신호가 Data 로출력 DIRECTION_PORT 의 Level 이 Low 인경우 : Data 의신호가 RxD 로입력 Half Duplex UART Half Duplex UART 을사용함으로써하나의 Node 에여러개의 Dynamixel 을연결하는 Multi-Drop Link 방식이가능하다. 때문에 Dynamixel 을제어할때는여러군데에서 동시에 Data 를송신하지않도록 Protocol 이운영되어야한다. Main Controller [Multi Drop Link] 주의 배선시엔 Pin 배열이틀리지않도록각별히주의한다. 최초로전원을인가할때전류 소비량을확인한다. Standby 상태에서한개의 Dynamixel 의소비전류는 50mA 이하 이다. 연결확인 배선을통하여 Dynamixel 에전원이올바르게공급되었다면 Dynamixel 의 LED 가두 번깜박인다. 점검 위의과정을성공하지못했을경우 Connector 의 Pin 배열을다시확인한다. 전원공 급장치의전압및전류허용량을확인한다. 8

10 3. Communication Protocol 3-1. Communication 의개요 Packet Main Controller 와 Dynamixel 은 packet 을주고받으며통신한다. packet 의종류로는 Main Controller 에서 Dynamixel 로전송되는 Instruction Packet 과 Dynamixel 에서 Main Controller 로전송되는 Status Packet 이있다. Main Controller Instruction Packet Status Packet Communication 아래의그림과같이연결된시스템에서, Main Controller 가 ID=N 으로설정된 Instruction Packet 을전송할경우여러개의 Dynamixel 중 ID 가 N 인 Dynamixel 만이 Status Packet 을 return 하고그 Instruction 을수행한다. Instruction Packet(ID=N) Main Controller ID=0 ID=1 ID=N Status Packet(ID=N) Unique ID 여러개의 Dynamixel 이동시에 Packet 을선로로전송하면 Packet 충돌이일어나서 통신에문제를일으킨다. 그러므로 Network Node 안에 ID 가같은 Dynamixel 이존재 하지않도록 ID 설정을해야한다. Protocol Dynamixel 은 8 bit, 1 Stop bit, None Parity 의 Asynchronous Serial 통신을한다. 9

11 3-2. Instruction Packet Instruction Packet 은 Main Contoller 가 Dynamixel 에게동작을지시하는 Packet 이 다. Instruction Packet 의구조는다음과같다. Instruction Packet OXFF 0XFF ID LENGTH INSTRUCTION PARAMETER1 PARAMETER N CHECK SUM Packet 을이루는각 byte 들의의미는다음과같다. 0XFF 0XFF 맨앞에위치한두개의 0XFF 는 Packet 의시작을알리는신호이다. ID Dynamixel 의 ID 이다. Dynamixel 의 ID 는 0X00 ~ 0XFD 까지 254 개가가능하다. Broadcasting ID ID 0XFE 는연결되어있는 Dynamixel 전체를지정하는 Broadcast ID 이다. ID 를 0XFE 로 설정한 Packet 은연결된모든 Dynamixel 에게유효하다. 그러므로 Broadcasting 으로 전달된 Packet 의경우는 Status Packet 이 return 되지않는다. LENGTH Packet 의길이로서, 그값은 Parameter 개수 (N) + 2 이다. INSTRUCTION Dynamixel 에게수행하라고지시하는명령. PARAMETER0 N Instruction 외에추가정보가더필요할경우사용된다. CHECK SUM Check Sum 의계산방법은다음과같다. Check Sum = ~( ID + Length + Instruction + Parameter1 + Parameter N ) 계산된값이 255 보다클경우결과값의하위 byte 가 Checksum 이다. ~ 은 Not Bit 연산자이다 Status Packet(Return Packet) Status Packet 은 Dynamixel 이 Instruction Packet 을전송받은후그응답으로 Main Controller 에게 return 하는 Packet 이다. Status Packet 의구조는다음과같다. OXFF 0XFF ID LENGTH ERROR PARAMETER1 PARAMETER2 PARAMETER N CHECK SUM 10

12 Packet 을이루는각 byte 들의의미는다음과같다. 0XFF 0XFF 맨앞에위치한두개의 0XFF 는 Packet 의시작을알리는신호이다. ID Packet 을 Return 하는 Dynamixel 의 ID 이다. 초기값은 1 이다. LENGTH Status Packet 의길이로서, 그값은 Parameter 개수 (N) + 2 이다. ERROR 그다음에위치한 ERROR 는 Dynamixel 이동작중에발생된 Error 상태를나타내며, 각 Bit 별의미는다음표와같다. Bit 명칭내용 Bit Bit 6 Instruction Error 정의되지않은 Instruction 이전송된경우. 또는 reg_write 명령없이 action 명령이전달된경우 1 로설정됨 Bit 5 Overload Error 지정된최대 Torque 로현재의하중을제어할수없을때 1 로설정됨 Bit 4 Checksum Error 전송된 Instruction Packet 의 Checksum 이맞지않을때 1 로설정됨 Bit 3 Range Error 사용범위를벗아난명령일경우 1 로설정됨. Bit 2 Bit 1 Bit 0 Overheating Error Angle Limit Error Input Voltage Error Dynamixel 내부온도가 Control Table 에설정된동작온도범위를벗어났을때 1 로설정됨 Goal Position 이 CW Angle Limit ~ CCW Angle Limit 범위밖의값으로 Writing 되었을때 1 로설정됨 인가된전압이 Control Table 에설정된동작전압범위를벗어났을경우 1 로설정됨 PARAMETER0 N 추가정보가필요할경우사용된다. CHECK SUM Check Sum 의계산방법은다음과같다. Check Sum = ~( ID + Length + Instruction + Parameter1 + Parameter N ) 계산된값이 255 보다클경우결과값의하위 byte 가 Checksum 이다. ~ 은 Not Bit 연산자이다. 11

13 3-4. Control Table EEPROM Area RAM Area Address Item Access Initial Value 0(0X00) Model Number(L) RD 12(0x0C) 1(0X01) Model Number(H) RD 0(0x00) 2(0X02) Version of Firmware RD? 3(0X03) ID RD,WR 1(0x01) 4(0X04) Baud Rate RD,WR 1(0x01) 5(0X05) Return Delay Time RD,WR 250(0xFA) 6(0X06) CW Angle Limit(L) RD,WR 0(0x00) 7(0X07) CW Angle Limit(H) RD,WR 0(0x00) 8(0X08) CCW Angle Limit(L) RD,WR 255(0xFF) 9(0X09) CCW Angle Limit(H) RD,WR 3(0x03) 10(0x0A) (Reserved) - 0(0x00) 11(0X0B) the Highest Limit Temperature RD,WR 85(0x55) 12(0X0C) the Lowest Limit Voltage RD,WR 60(0X3C) 13(0X0D) the Highest Limit Voltage RD,WR 190(0xBE) 14(0X0E) Max Torque(L) RD,WR 255(0XFF) 15(0X0F) Max Torque(H) RD,WR 3(0x03) 16(0X10) Status Return Level RD,WR 2(0x02) 17(0X11) Alarm LED RD,WR 4(0x04) 18(0X12) Alarm Shutdown RD,WR 4(0x04) 19(0X13) (Reserved) RD,WR 0(0x00) 20(0X14) Down Calibration(L) RD? 21(0X15) Down Calibration(H) RD? 22(0X16) Up Calibration(L) RD? 23(0X17) Up Calibration(H) RD? 24(0X18) Torque Enable RD,WR 0(0x00) 25(0X19) LED RD,WR 0(0x00) 26(0X1A) CW Compliance Margin RD,WR 0(0x00) 27(0X1B) CCW Compliance Margin RD,WR 0(0x00) 28(0X1C) CW Compliance Slope RD,WR 32(0x20) 29(0X1D) CCW Compliance Slope RD,WR 32(0x20) 30(0X1E) Goal Position(L) RD,WR [Addr36]value 31(0X1F) Goal Position(H) RD,WR [Addr37]value 32(0X20) Moving Speed(L) RD,WR 0 33(0X21) Moving Speed(H) RD,WR 0 34(0X22) Torque Limit(L) RD,WR [Addr14] value 35(0X23) Torque Limit(H) RD,WR [Addr15] value 36(0X24) Present Position(L) RD? 37(0X25) Present Position(H) RD? 38(0X26) Present Speed(L) RD? 39(0X27) Present Speed(H) RD? 40(0X28) Present Load(L) RD? 41(0X29) Present Load(H) RD? 42(0X2A) Present Voltage RD? 43(0X2B) Present Temperature RD? 44(0X2C) Registered Instruction RD,WR 0(0x00) 45(0X2D) (Reserved) - 0(0x00) 46[0x2E) Moving RD 0(0x00) 47[0x2F) Lock RD,WR 0(0x00) 48[0x30) Punch(L) RD,WR 32(0x20) 49[0x31) Punch(H) RD,WR 0(0x00) 12

14 Control Table Control Table 은 Dynamixel 의상태와구동에관한 Data 로구성되어있다. Control Table 의값들을 Writing 함으로써 Dynamixel 을구동시키고, Control Table 의값을 Reading 하여 Dynamixel 의상태를파악할수있다. RAM and EEPROM RAM Area 의 Data 는전원이인가될때마다다시초기값으로설정된다. 그러나 EEPROM Area Data 의경우값을설정하면전원이 Off 되어도그값이보존된다. Initial Value Control Table 의우측에표시된 Initial Value 는 EEPROM Area Data 인경우 Factory Default Value 이고, RAM Area Data 인경우는전원이인가되었을때갖는초기값을 의미한다. 다음은 Control Table 의각 Address 에지정된 Data 의의미를설명하였다. 이내용 은 Instruction 만큼의미있는내용이므로사용자가꼭숙지하여야한다. Address 0x00,0x01 Model Number. 경우값은 0X000C(12) 이다. Address 0x02 Firmware Version. Address 0x03 ID. Dynamixel 을식별하기위한고유번호이다. Link 된 Dynamixel 들은서로다른 ID 가 할당되어야한다. Address 0x04 Baud Rate. 통신 Speed 를결정한다. 산출공식은다음과같다. Speed(BPS) = /(Address4+1) 주요 Baud Rate 별 Data Value Adress4 설정 BPS 목표 BPS 오차 % % % % % % % % % 참고 UART 는 Baud Rate 오차가 3% 이내이면통신에지장이없다. 주의 Baudrate 의초기값은 1( bps) 이다. 13

15 Address 0x05 Return Delay Time. Instruction Packet 전송후 Satus Packet 이 return 되기까지걸리 는지연시간. 2uSec * Address5 값만큼지연된다. Address 0x06,0x07,0x08,0x09 Operating Angle Limit. Dynamixel이동작이허용되는 Angle구간을설정한다. Goal Position은 CW Angle Limit <= Goal Potion <= CCW Angle Limit의범위내에서사용되어야하며, 범위를벗어날경우 Angle Limit Error가발생한다. Address 0x0B the Highest Limit Temperature. Dynamixel의동작제한온도의상한선. Dynamixel의내부온도가이값을넘으면 Status Packet의 ERROR의 Over Heating Error Bit(Bit2) 가 1 로 return되고, Address17,18에설정된대로 Alarm이실행된다. 값은실제섭씨온도와일치한다. Address 0x0C,0x0D the Lowest (Highest) Limit Voltage. Dynamixel의동작전압범위의상한선과하한선을지정하는 Data이다. Present Voltage(Address42) 가이범위를벗어날경우 Status Packet의 ERROR의 Voltage Range Error Bit(Bit0) 가 1 로 return되고 Address17,18에서설정된대로 Alarm이실행된다. 실제전압의 10배를값으로한다. 예를들어 Address12의값이 80일경우동작하한선전압을 8V로설정한것이다. Address 0x0E,0x0F, 0x22,0x23 Max Torque. Dynamixel의최대 Torque 출력값을설정한다. 이값을 0 으로설정할경우 Torque가없는 Free Run상태가된다. Max Torque(Torque Limit) 는 ROM(Address 0X0E,0x0F) 과 RAM(Address 0x22,0x23) 두곳에할당이되어있는데, P 전원이 On될때 EEPROM의값이 RAM으로복사된다. Dynamixel의 Torque는 RAM에위치한값 (Address0x22,0x23) 에의해제한된다. Address 0X10 Status Return Level. Instruction Packet 이전송된후 Dynamixel 이 Status Packet 을 Return 해줄지여부를결정한다. Address16 Status Packet 의 Return 0 모든 Instruction 에대해 Return 하지않음 1 READ_DATA 명령에대해서만 Return 함 2 모든 Instruction 에대해 Return 함 Broadcast ID(0XFE) 의 instruction packet 의경우는 Address 0x10 의값에상관없이 14

16 Status Packet 이 return 되지않는다. Address 0X11 Alarm LED. Error 가발생했을때, 해당 Bit 가 1 로설정되어있으면 LED 가깜빡인다. Bit 기능 Bit 7 0 Bit 6 1로설정해놓으면 Instruction Error발생시 LED가깜빡임 Bit 5 1로설정해놓으면 Overload Error발생시 LED가깜빡임 Bit 4 1로설정해놓으면 Checksum Error발생시 LED가깜빡임 Bit 3 1로설정해놓으면 Range Error발생시 LED가깜빡임 Bit 2 1로설정해놓으면 Overheating Error발생시 LED가깜빡임 Bit 1 1로설정해놓으면 Angle Limit Error발생시 LED가깜빡임 Bit 0 1로설정해놓으면 Input Voltage Error발생시 LED가깜빡임 각 Bit의기능은 OR 의논리로작동된다. 즉, 0X05로설정되었을경우 Input Voltage Error가발생해도 LED는깜빡이고, Overheating Error가발생해도 LED가깜빡인다. Error가발생했다가정상상황으로복귀하면 2초후에 LED는깜빡임을멈춘다. Address 0X12 Alarm Shutdown. Error 가발생했을때해당 Bit 가 1 로설정되어있을경우 Dynamixel 은 Torque off 된다. Bit 기능 Bit 7 0 Bit 6 1로설정해놓으면 Instruction Error발생시 Torque Off Bit 5 1로설정해놓으면 Overload Error발생시 Torque Off Bit 4 1로설정해놓으면 Checksum Error발생시 Torque Off Bit 3 1로설정해놓으면 Range Error발생시 Torque Off Bit 2 1로설정해놓으면 Overheating Error발생시 Torque Off Bit 1 1로설정해놓으면 Angle Limit Error발생시 Torque Off Bit 0 1로설정해놓으면 Input Voltage Error발생시 Torque Off 각 Bit의기능은 Alarm LED와마찬가지로 OR 의논리로작동된다. 그러나 Alarm LED와는달리 Error가발생했다가정상상황으로복귀해도 Torque 0FF상태는계속유지된다. Shutdown상태에서벗어나려면 Torque Enable(Address0X18) 를 1로재설정해야한다. Address 0x14~0x17 Calibration. Potentio Meter 의제품간편차를보상하기위한 Data 이다. 사용자가변 경할수없는영역이다. 이어질 Address 0x18 부터는 RAM 영역이다. 15

17 Address 0x18 Torque Enable. Digital Mode 에서 Dynamixel 에전원을인가하면 Torque 가발생하지 않는 Free Run 상태이다. Address 0x18 에 1 을설정하면 Torque Enable 상태로된다. Address 0x19 LED. 1 로설정되어있으면 LED 가켜지고 0 으로설정되어있으면 LED 가꺼진다. Address 0x1A~0x1D Compliance Margin과 Slope. Dynamixel에서는 Margin과 Slope를설정하여 Compliance를조절한다. Compliance를잘활용하면충격흡수를하는효과를낼수있다. 다음의 Position Error에따른출력곡선에서 A,B,C,D의길이가 Compliance값이다. CW Goal Position CCW E E CW X axis:position Error CCW Y axis:output Torque A B C D A : CCW Compliance Slope(Address0x1D) B : CCW Compliance Margin(Address0x1B) C : CW Compliance Margin(Address0x1A) D : CW Compliance Slope (Address0x1C) E : Punch(Address0x30,31) Address 0X1E,0x1F Goal Position. Dynamixel 이이동하고자하는위치. 값 0x3ff 로설정하면 300 로이 동한다. 150 (Goal Position = 0x1ff) 300 (Goal Position = 0x3ff) 300~360 Invalid Angle 0 (Goal Position = 0) 16

18 Address 0x20,0x21 Moving Speed. Goal Position으로이동하는속도. 최대값인 0x3ff로설정되면전압공급이충분할경우 114RPM의속도로움직인다.( 속도가 1로 Setting될경우가최저속이며, 0으로 Setting되어있을경우는현재인가전압상에서낼수있는최대속도로움직임, 즉속도제어를하지않음 ) Address 0x24,0x25 Present Position. Dynamixel 의현재위치. Address 0x26,0x27 Present Speed. Dynamixel 의현재속도 Data. Address 0x28,0x29 Present Load. Dynamixel 의현재구동하는 Load 의크기. Bit10 은 Load 가걸려있는 방향이다. BIT 15~ Value 0 Load Direction Load Value Load Direction = 0 : CCW Load, Load Direction = 1: CW Load Address 0x2A Present Voltage. Dynamixel 에현재인가되고있는전압. 이값은실제전압의 10 배 이다. 즉 10V 일경우 100(0x64) 이읽혀진다. Address 0x2B Present Temperature. Dynamixel 내부의섭씨온도. Address 0x2C Registered Instruction. REG_WRITE 명령에의해명령이등록되어있을때 1 로설정되 고, Action 명령에의해등록된명령이수행완료된후에는 0 으로변한다. Address 0x2E Moving. Dynamixel 이자신의동력에의한 Moving 상태일때 1 로 Setting 된다. Address 0x2F Lock. 1 로 Setting 되면 Address 0X18~ Address0x23 의값만 Writing 할수있고나 머지영역은 Writing 이금지된다. 한번 Lock 되면 Power Off 로만 Unlock 할수있다. Address 0x30,0x31 Punch. 구동시에모터에공급되는최소전류량. 초기값은 0x20 이며최고 0x3ff 까지 설정할수있다. Endless Turn Goal Speed Setting CW Angle Limit과 CCW Angle Limit이모두 0으로설정되면 Goal Speed를설정하여 Endless Turn을구현할수있다. Endless Turn은바퀴구동등에사용된다. BIT 15~ Value 0 Turn Direction Speed Value 17

19 Turn Direction = 0 : CCW Direction Turn, Load Direction = 1: CW Direction Turn Range 각 Data들은유효한범위가정해져있다. 이를벗어난 Write명령이전송될경우 Error가 return된다. 아래표에사용자가 Write할수있는 Data의길이, 그리고범위를정리하였다. 16bit Data는 (L) 과 (H), 두 byte로표시된다. 이두 byte는하나의 Instruction Packet으로동시에 Write되어야한다. Write Address Writing Item Length (bytes) Min Max 3(0X03) ID (0xfd) 4(0X04) Baud Rate (0xfe) 5(0X05) Return Delay Time (0xfe) 6(0X06) CW Angle Limit (0x3ff) 8(0X08) CCW Angle Limit (0x3ff) 11(0X0B) the Highest Limit Temperature (0x96) 12(0X0C) the Lowest Limit Voltage 1 50(0x32) 250(0xfa) 13(0X0D) the Highest Limit Voltage 1 50(0x32) 250(0xfa) 14(0X0E) Max Torque (0x3ff) 16(0X10) Status Return Level (0X11) Alarm LED (0x7f) 18(0X12) Alarm Shutdown (0x7f) 19(0X13) (Reserved) (0X18) Torque Enable (0X19) LED (0X1A) CW Compliance Margin (0xfe) 27(0X1B) CCW Compliance Margin (0xfe) 28(0X1C) CW Compliance Slope (0xfe) 29(0X1D) CCW Compliance Slope (0xfe) 30(0X1E) Goal Position (0x3ff) 32(0X20) Moving Speed (0x3ff) 34(0X22) Torque Limit (0x3ff) 44(0X2C) Registered Instruction (0X2F) Lock (0X30) Punch (0x3ff) [Control Table Data Range and Length for Writing] 18

20 4. Instruction Set 과그사용예 다음과같은종류의 Instruction 이있다. Instruction Function Value Number of Parameter PING 수행내용없음. Dynamixel 이 Status Packet 을 return 받고자할경우사용. 0x01 0 READ DATA Control Table 의값을읽는명령. 0x02 2 WRITE DATA Control Table 에값을쓰는명령. 0x03 2 ~ REG WRITE WRTE_DATA 와내용은유사하나, 대기상태로있다가 ACTION 명령이도착하면 Write 된다. 0x04 2 ~ ACTION REG_WRITE 으로등록된동작을시작하라는명령 0x05 0 RESET Dynamixel 내의 Control Table 값을 Factory Default Value 로바꾼다. 0x06 0 SYNC WRITE 한번에여러개의 Dynamixel 들을동시에제어하고자할때사용되는명령 0x83 4~ 4-1. WRITE_DATA Function Dynamixel 내부 Control Table에 Data를쓰는명령 Length N+3 (Writing Data가 N개일경우 ) Instruction 0X03 Parameter1 Data를쓰고자하는곳의시작 Address Parameter2 쓰고자하는첫번째 Data Parameter3 쓰고자하는두번째 Data Parameter N+1 쓰고자하는 N번째 Data Example 1 연결된 Dynamixel 의 ID 를 1 로설정하고자하는경우 Control Table 의 Address 3 에 1 를 Writing 한다. ID 는 Broadcasting ID(0xFE) 로전송 하기로한다. 19

21 Instruction Packet : 0XFF 0XFF 0XFE 0X04 0X03 0X03 0X01 0XF6` ID LENGTH INSTRUCTION PARAMETERS CHECKSUM Broadcast ID(0XFE) 로전송되었으므로 Status Packet 은 return 되지않는다 READ_DATA Function Length Instruction Parameter1 Parameter2 Dynamixel 내부 Control Table의 Data를읽는명령 0X04 0X02 Read하고자하는 data의시작 Address Read하고자하는 data의길이 Example 2 ID 가 1 인 Dynamixel 의현재내부온도를읽고자하는경우 Control Table 의 Address 0x2B 값에서 1byte 를읽는다. Instruction Packet : 0XFF 0XFF 0X01 0X04 0X02 0X2B 0X01 0XCC` ID LENGTH INSTRUCTION PARAMETERS.. CHECKSUM 이에대하여 return 되는 Status Packet 은다음과같다. Status Packet : 0XFF 0XFF 0X01 0X03 0X00 0X20 0XDB ID LENGTH ERROR PARAMETER1 CHECKSUM 읽혀진 Data 값은 0x20 이다. 현재 Dynamixel 의내부온도는약 32 (0X20) 이다 REG_WRITE 과 ACTION REG_WRITE Function REG_WRITE 명령은 WRITE_DATA 명령과기능은유사하나, 명령이수행되는시점이다 20

22 르다. Instruction Packet이도착하면그값이 Buffer에저장되어있고 Write 동작은대기상태로남아있는다. 이때, Registered Instruction(Address0x2C) 이 1로설정된다. 이후에 Action Instruction Packet이도착하면비로소등록되어있던 Write명령이실행된다. Length N+3 (Writing Data가 N개일경우 ) Instruction 0X04 Parameter1 Data를쓰고자하는곳의시작 Address Parameter2 쓰고자하는첫번째 Data Parameter3 쓰고자하는두번째 Data Parameter N+1 쓰고자하는 N번째 Data ACTION Function Length Instruction Parameter REG_WRITE로등록된 WRITE 작업을수행하라는명령 0X02 0X05 NONE ACTION 명령은여러개의 Dynamixel을동시에정확히움직여야하는경우유용하다. 여러개의구동장치를통신에의해제어할때, 맨처음명령을전달받는구동장치와맨마지막에명령을전달받는구동장치는구동시점에약간의시간차이가발생한다. Dynamixel에서는 ACTION Instruction을사용하여이러한문제를해결하였다. Broadcasting 두개이상의 Dynamixel 에 ACTION 명령을전송할경우 Broadcast ID(0XFE) 를사용하 여야하는데, 이때 Packet 은 Return 되지않는점에유의한다 PING Function Length Instruction Parameter 아무것도지시하지않는다. 단지 Status Packet을받고자할때나특정 ID를갖는 Dynamixel의존재를확인하기위해사용된다. 0X02 0X01 NONE 21

23 Example 3 ID 가 1 인 Dynamixel 의 Status Packet 을얻고싶을때 Instruction Packet : 0XFF 0XFF 0X01 0X02 0X01 0XFB` ID LENGTH INSTRUCTION CHECKSUM 이에대하여 return 되는 Status Packet 은다음과같다. Status Packet : 0XFF 0XFF 0X01 0X02 0X00 0XFC ID LENGTH ERROR CHECKSUM Broadcasting ID 가지정되거나 Status Return Level(Address16) 이 0 이더라도, PING Instruction 에대해서는무조건 Status Packet 을 return 한다 RESET Function Dynamixel을 Factory Default 상태로 Control Table을되돌려놓는다. Length 0X02 Instruction 0X06 Parameter NONE Example 4 ID 가 0 인 Dynamixel 을 RESET 하고자할경우 Instruction Packet : 0XFF 0XFF 0X00 0X02 0X06 0XF7` ID LENGTH INSTRUCTION CHECKSUM 이에대하여 return 되는 Status Packet 은다음과같다. Status Packet : 0XFF 0XFF 0X00 0X02 0X00 0XFD ID LENGTH ERROR CHECKSUM RESET 명령수행이후엔 Dynamixel 의 ID 가 1 로바뀌어있음을유의한다. 22

24 4-6. SYNC WRITE Function 한번의 Instruction Packet전송으로여러개의 Dynamixel들을동시에제어하고자할때사용되는명령어이다. Sync Write명령을사용하면여러개의명령을한번에전달하므로다수의 Dynamixel을제어할때통신시간을줄어든다. 단, 각 Dynamixel들에 writing하고자하는 Control Table의 Address와 Length가모두동일해야 SYNC WRITE 명령을사용할수있다. 또한 ID는 Broadcasting ID로전송되어야한다. ID 0XFE Length (L+1) X N + 4 (L:Dynamixel별 Data Length, N:Dynamixel의개수 ) Instruction 0X83 Parameter1 Data를쓰고자하는곳의시작 Address Parameter2 쓰고자하는 Data의길이 (L) Parameter3 첫번째 Dynamixel의 ID Parameter4 첫번째 Dynamixel의첫번째 Data Parameter5 첫번째 Dynamixel의두번째 Data 첫번째 Dynamixel에대한 Data Parameter L+3 첫번째 Dynamixel의 L번째 Data Parameter L+4 Parameter L+5 Parameter L+6 Parameter 2L+4. 두번째 Dynamixel의 ID 두번째 Dynamixel의첫번째 Data 두번째 Dynamixel의두번째 Data 두번째 Dynamixel의 L번째 Data 두번째 Dynamixel 에대한 Data Example 5 4 개의 Dynamixel 에대하여각각다음과같은위치와속도를설정하고자하는경우 ID 0인 Dynamixel : 0x010위치로속도 0x150으로이동 ID 1인 Dynamixel : 0x220위치로속도 0x360으로이동 ID 2인 Dynamixel : 0x030위치로속도 0x170으로이동 ID 3인 Dynamixel : 0x220위치로속도 0x380으로이동 Instruction Packet : 0XFF 0XFF 0XFE 0X18 0X83 0X1E 0X04 0X00 0X10 0X00 0X50 0X01 0X01 0X20 0X02 0X60 0X03 0X02 0X30 0X00 0X70 0X01 0X03 0X20 0X02 0X80 0X03 0X12` Broadcasting ID로전송하므로 Status Packet은 return되지않는다. 23

25 5. Example Dynamxiel 은 Reset 상태인 ID = 1, Baudrate = 57142BPS 이라고가정하고예제를설 명한다. Example 6 ID 가 1 인 Dynamixel 의 Model Number 와 Firmware Version 을읽는다. Instruction Packet Communication Instruction = READ_DATA, Address = 0x00, Length = 0x03 ->[Dynamixel]:FF FF F5 (LEN:008) <-[Dynamixel]:FF FF D (LEN:009) Status Packet Result Model Number = 116(0x74)(DX-116 인경우임 ) Firmware Version = 0x08 Example 7 ID 가 1 인 Dynamixel 의 ID 를 0 으로변경한다. Instruction Packet Communication Instruction = WRITE_DATA, Address = 0x03, DATA = 0x00 ->[Dynamixel]:FF FF F4 (LEN:008) <-[Dynamixel]:FF FF FC (LEN:006) Status Packet Result NO ERROR Example 8 Dynamixel 의 Baud Rate 를 1M bps 로변경한다. Instruction Packet Communication Instruction = WRITE_DATA, Address = 0x04, DATA = 0x01 ->[Dynamixel]:FF FF F3 (LEN:008) <-[Dynamixel]:FF FF FD (LEN:006) Status Packet Result NO ERROR Example 9 ID 가 0 인 Dynamixel 의 Return Delay Time 을 4uSec 로재설정한다. Reurn Delay Time Value 1 이 2uSec 에해당한다. 24

26 Instruction Packet Communication Instruction = WRITE_DATA, Address = 0x05, DATA = 0x02 ->[Dynamixel]:FF FF F1 (LEN:008) <-[Dynamixel]:FF FF FD (LEN:006) Status Packet Result NO ERROR Return Delay Time 은 Main Controller 의허용범위내에서최소값으로설정하는것이 좋다. Example 10 ID 가 0 인 Dynamixel 의동작각을 0~150 로제한한다. CCW Angle Limit 가 0x3ff 일경우 300 이므로 150 에해당하는값은 0x1ff 이다. Instruction Packet Communication Instruction = WRITE_DATA, Address = 0x08, DATA = 0xff, 0x01 ->[Dynamixel]:FF FF FF 01 EF (LEN:009) <-[Dynamixel]:FF FF FD (LEN:006) Status Packet Result NO ERROR Example 11 ID 가 0 인 Dynamixel 의동작온도상한선을 80 로재설정한다. Instruction Packet Communication Instruction = WRITE_DATA, Address = 0x0B, DATA = 0x50 ->[Dynamixel]:FF FF B 50 9D (LEN:008) <-[Dynamixel]:FF FF FD (LEN:006) Status Packet Result NO ERROR Example 12 ID 가 0 인 Dynamixel 의동작전압을 10V ~ 17V 로설정한다. 10V 는 100(0x64), 17V 는 170(0xAA) 의값으로표시된다. Instruction Packet Communication Instruction = WRITE_DATA, Address = 0x0C, DATA = 0x64, 0xAA ->[Dynamixel]:FF FF C 64 AA DD (LEN:009) <-[Dynamixel]:FF FF FD (LEN:006) Status Packet Result NO ERROR 25

27 Example 13 ID 가 0 인 Dynamixel 이항상 Torque 를최대값의 50% 만발휘하도록한다. ROM 영역에위치한 MAX Torque 의값을최대값인 0x3ff 의 50% 인 0x1ff 로설정한다. Instruction Packet Communication Instruction = WRITE_DATA, Address = 0x0E, DATA = 0xff, 0x01 ->[Dynamixel]:FF FF E FF 01 E9 (LEN:009) <-[Dynamixel]:FF FF FD (LEN:006) Status Packet Result NO ERROR 전원을 Off 한후다시공급해야 Max Torque 값을조정한효과를확인할수있다. Example 14 ID 가 0 인 Dynamixel 이항상 Status Packet 을 Return 하지않도록한다. Instruction Packet Communication Instruction = WRITE_DATA, Address = 0x10, DATA = 0x00 ->[Dynamixel]:FF FF E8 (LEN:008) <-[Dynamixel]:FF FF FD (LEN:006) Status Packet Result NO ERROR Status Packet 은다음번 Instruction 부터 Return 되지않는다. Example 15 Instruction Packet Communication 동작온도가설정된한계온도보다높은값을가질경우 LED를깜빡이고, Shutdown (Torque off) 되도록 Alarm을설정한다. Overheating Error는 Bit 2이므로 Alarm값을 0x04로설정한다. Instruction = WRITE_DATA, Address = 0x11, DATA = 0x04, 0x04 ->[Dynamixel]:FF FF DE (LEN:009) <-[Dynamixel]:FF FF FD (LEN:006) Status Packet Result NO ERROR 26

28 Example 16 ID 가 0 인 Dynamixel 의 LED 를켜고, Torque 를 Enable 시킨다. Instruction Packet Communication Instruction = WRITE_DATA, Address = 0x18, DATA = 0x01, 0x01 ->[Dynamixel]:FF FF DD (LEN:009) <-[Dynamixel]:FF FF FD (LEN:006) Status Packet Result NO ERROR 손으로 Dynamixel 의축을만져보면 Torque Enable 상태를확인할수있다. Example 17 Compliance ID 가 0 인 Dynamixel 을 Compliance Margin = 1, Compliance Slope 를 0x40 으로설정 한다. Angle Error 와 Torque Output 은다음과같은 Graph 로나타낼수있다. CW Goal Position CCW CW X:Angle Error CCW 위치가 Goal Position으로부터 CW방향으로조금만벗어나도 CCW방향으로큰 Torque를발생시켜 Goal Position으로위치를수렴시키고있다. 그러나관성이고려되어야하므로실제제어방식은이와다소차이가있다. 위의예제에서제시한조건을이러한방식의 Graph로표시해보면다음과같다. CW Goal Position CCW CW Angle(Position) Output Torque CCW A B C D A : CCW Compliance Slope(Address0x1D) = 0x40( 약 18.8 ) B : CCW Compliance Margin(Address0x1B) = 0x01 ( 약 0.29 ) 27

29 C : CW Compliance Margin(Address0x01A) = 0x01( 약 0.29 ) D : CW Compliance Slope(Address0x1C) = 0x40 ( 약 18.8 ) Instruction Packet Communication Instruction = WRITE_DATA, Address = 0x1A, DATA = 0x01, 0x01, 0x40, 0x40 ->[Dynamixel]:FF FF A (LEN:011) <-[Dynamixel]:FF FF FD (LEN:006) Status Packet Result NO ERROR Compliance Slope 는 2 n (n 은정수 ) 를경계로효과가변화한다. 즉 Compliance 값 0x11~0x20 은그효과가동일하다. Example 18 ID 가 0 인 Dynamixel 을 57RPM 의속도로 Position 180 에위치시킨다. Address 0x1E(Goal Position) = 0x200, Address 0x20(Moving Speed) = 0x200 으로설정한다. Instruction Packet Communication Instruction = WRITE_DATA, Address = 0x1E, DATA = 0x00, 0x02, 0x00, 0x02 ->[Dynamixel]:FF FF E D3 (LEN:011) <-[Dynamixel]:FF FF FD (LEN:006) Status Packet Result NO ERROR Example 19 Instruction Packet Communication ID 가 0 인 Dynamixel 은 Position 0 에, ID 가 1 인 Dynamixel 은 Position 300 에, 위 치시킨다. 단두 Dynamixel 은똑같은시점에움직이기시작하도록한다. WRITE_DATA 명령을사용하면두 Dynamixel 을똑같은시점에출발시킬수없다. 그러므로 REG_WRITE 와 ACTION 을사용한다. ID=0, Instruction = REG_WRITE, Address = 0x1E, DATA = 0x00, 0x00 ID=1, Instruction = REG_WRITE, Address = 0x1E, DATA = 0xff, 0x03 ID=0xfe(Broadcasting ID), Instruction = ACTION, ->[Dynamixel]:FF FF E D8 (LEN:009) <-[Dynamixel]:FF FF FD (LEN:006) ->[Dynamixel]:FF FF E FF 03 D5 (LEN:009) <-[Dynamixel]:FF FF FC (LEN:006) ->[Dynamixel]:FF FF FE FA (LEN:006) <-[Dynamixel]: //No return packet against broadcasting ID Status Packet Result NO ERROR 28

30 Example 20 Instruction Packet Communication ID가 0인 Dynamixel을 Address0x18 ~ Address0x23의값이외에는변경할수없도록한다. Address 0x2F(Lock) 을 1로설정한다. Instruction = WRITE_DATA, Address = 0x2F, DATA = 0x01 ->[Dynamixel]:FF FF F 01 C8 (LEN:008) <-[Dynamixel]:FF FF FD (LEN:006) Status Packet Result NO ERROR Lock 이되면전원을제거해야만 unlock 할수있다. Lock 된상태에서다른데이터를 Access 하면 Error 가 Return 된다. ->[Dynamixel]:FF FF (LEN:009) <-[Dynamixel]:FF FF F5 (LEN:006) Range Error Example 21 ID 가 0 인 Dynamixel 의최소출력값 (Punch) 을 0x40 으로한다. Instruction Packet Communication Instruction = WRITE_DATA, Address = 0x30, DATA = 0x40, 0x00 ->[Dynamixel]:FF FF (LEN:009) <-[Dynamixel]:FF FF FD (LEN:006) Status Packet Result NO ERROR 29

31 Appendix Half duplex UART Half duplex UART란동시에 TxD, RxD가동시에수행될수없는 Serial 통신방식을의한다. 보통하나의 BUS에여러개의통신장치를연결할경우에사용하는방식이다. 여러개의장치가동일한 BUS에연결되어있으므로하나의장치가송신하는동안그외의다른모든장치들은입력상태이어야한다. Dynamixel을제어하는 Main Controller는통신방향을입력으로설정하고있다가 Instruction Packet을전송하는동안만통신방향을출력으로설정한다. RS485 Direction Output Duration Instruction Packet Status Packet Return Delay Time Return Delay Time Dynamixel이 Instruction Packet을받은후 Status Packet을 return하는데걸리는시간을의미한다. Default Value는 160uSec이며, Control Table Address 5를설정하여 Return Delay Time을변경할수있다. Main Controller는 Instruction Packet 전송후 Return Delay time 구간안에서 Direction Port을입력상태로전환해야한다. Tx,Rx Direction Half Duplex UART 에서는송신이끝나는 Timing 을잘맞춰서 Direction 을수신 Mode 로바꾸어야주어야한다. CPU 에서는일반적으로 UART_STATUS 를표시해주는 REGISTER 내에다음과같은의미의 BIT 가있다. TXD_BUFFER_READY_BIT : Transmission DATA 를 Buffer 에적재할수있는상태임를 뜻한다. 주의할점은이상태는 SERIAL TX BUFFER 가비어있다는의미이지, 이전에 전송한데이터가모두 CPU 밖으로배출된상태를의미하는것은아니다. TXD_SHIFT_REGISTER_EMPTY_BIT : Transmission Data 가모두 CPU 밖으로배출되었을 때 SET 된다. TXD_BUFFER_READY_BIT 의경우는 Serial 통신에서한 Byte 를송신할때사용되며그 예는다음과같다. TxDByte(byte bdata) while(!txd_buffer_ready_bit); //wait until data can be loaded. SerialTxDBuffer = bdata; //data load to TxD buffer 30

32 Direction 을전환하는시점에서는 TXD_SHIFT_REGISTER_EMPTY_BIT 를확인해야한다. 다음은 Instruction packet 을전송하는예제프로그램이다. LINE 1 LINE 2 LINE 3 LINE 4 LINE 5 LINE 6 LINE 7 LINE 8 LINE 9 LINE 10 LINE 11 LINE 12 DIRECTION_PORT = TX_DIRECTION; TxDByte(0xff); TxDByte(0xff); TxDByte(bID); TxDByte(bLength); TxDByte(bInstruction); TxDByte(Parameter0); TxDByte(Parameter1); DisableInterrupt(); // interrupt should be disable TxDByte(Checksum); //last TxD while(!txd_shift_register_empty_bit); //Wait till last data bit has been sent DIRECTION_PORT = RX_DIRECTION; //Direction change to RXD EnableInterrupt(); // enable interrupt again 주의할부분은 LINE 8부터 LINE12이다.. LINE 8이필요한이유는그시점에서 Interrupt가발생하여 Return Delay Time 보다긴시간동안 Interrupt routine이수행될경우 Status Packet의앞부분이손상되기때문이다. Byte to Byte Time Instruction Packet 을전송할때 Byte 와 Byte 사이의 Delay time 을의미하는데, 이시 간이 100mSec 가넘을경우 Dynamixel 은전송장해가발생한것으로간주하고, 다 시 Packet 의 header(0xff 0xff) 를기다린다. 0xFF 0xFF ID Length Byte To Byte Time 다음은 Atmega128 로 Dynamixel 을 Access 하는 Example.c 의 source 이다. 31

33 C Language Example : Dinamixel access with Atmega128 /* * The Example of Dynamixel Evaluation with Atmega128 * Date : * Author : BS KIM */ /* * included files */ #define ENABLE_BIT_DEFINITIONS //#include <io.h> #include <inttypes.h> #include <avr/io.h> #include <avr/interrupt.h> #include <avr/signal.h> #define cbi(reg8,bitnum) REG8 &= ~(_BV(BITNUM)) #define sbi(reg8,bitnum) REG8 = _BV(BITNUM) #define P_PRESENT_TEMPERATURE (43) #define P_REGISTERED_INSTRUCTION (44) #define P_PAUSE_TIME (45) #define P_MOVING (46) #define P_LOCK (47) #define P_PUNCH_L (48) #define P_PUNCH_H (49) //--- Instruction --- #define INST_PING 0x01 #define INST_READ 0x02 #define INST_WRITE 0x03 #define INST_REG_WRITE 0x04 #define INST_ACTION 0x05 #define INST_RESET 0x06 #define INST_DIGITAL_RESET 0x07 #define INST_SYSTEM_READ 0x0C #define INST_SYSTEM_WRITE 0x0D #define INST_SYNC_WRITE 0x83 #define INST_SYNC_REG_WRITE 0x84 typedef unsigned char byte; typedef unsigned int word; #define ON 1 #define OFF 0 #define _ON 0 #define _OFF 1 //--- Control Table Address --- //EEPROM AREA #define P_MODEL_NUMBER_L 0 #define P_MODOEL_NUMBER_H 1 #define P_VERSION 2 #define P_ID 3 #define P_BAUD_RATE 4 #define P_RETURN_DELAY_TIME 5 #define P_CW_ANGLE_LIMIT_L 6 #define P_CW_ANGLE_LIMIT_H 7 #define P_CCW_ANGLE_LIMIT_L 8 #define P_CCW_ANGLE_LIMIT_H 9 #define P_SYSTEM_DATA2 10 #define P_LIMIT_TEMPERATURE 11 #define P_DOWN_LIMIT_VOLTAGE 12 #define P_UP_LIMIT_VOLTAGE 13 #define P_MAX_TORQUE_L 14 #define P_MAX_TORQUE_H 15 #define P_RETURN_LEVEL 16 #define P_ALARM_LED 17 #define P_ALARM_SHUTDOWN 18 #define P_OPERATING_MODE 19 #define P_DOWN_CALIBRATION_L 20 #define P_DOWN_CALIBRATION_H 21 #define P_UP_CALIBRATION_L 22 #define P_UP_CALIBRATION_H 23 #define P_TORQUE_ENABLE (24) #define P_LED (25) #define P_CW_COMPLIANCE_MARGIN (26) #define P_CCW_COMPLIANCE_MARGIN (27) #define P_CW_COMPLIANCE_SLOPE (28) #define P_CCW_COMPLIANCE_SLOPE (29) #define P_GOAL_POSITION_L (30) #define P_GOAL_POSITION_H (31) #define P_GOAL_SPEED_L (32) #define P_GOAL_SPEED_H (33) #define P_TORQUE_LIMIT_L (34) #define P_TORQUE_LIMIT_H (35) #define P_PRESENT_POSITION_L (36) #define P_PRESENT_POSITION_H (37) #define P_PRESENT_SPEED_L (38) #define P_PRESENT_SPEED_H (39) #define P_PRESENT_LOAD_L (40) #define P_PRESENT_LOAD_H (41) #define P_PRESENT_VOLTAGE (42) #define CLEAR_BUFFER gbrxbufferreadpointer = gbrxbufferwritepointer #define DEFAULT_RETURN_PACKET_SIZE 6 #define BROADCASTING_ID 0xfe #define TxD8 TxD81 #define RxD8 RxD81 //Hardware Dependent Item #define DEFAULT_BAUD_RATE 34 //57600bps at 16MHz ////// For CM-5 #define RS485_TXD PORTE &= ~_BV(PE3),PORTE = _BV(PE2) //PORT_485_DIRECTION = 1 #define RS485_RXD PORTE &= ~_BV(PE2),PORTE = _BV(PE3) //PORT_485_DIRECTION = 0 /* ////// For CM-2 #define RS485_TXD PORTE = _BV(PE2); //_485_DIRECTION = 1 #define RS485_RXD PORTE &= ~_BV(PE2);//PORT_485_DIRECTION = 0 */ //#define TXD0_FINISH UCSR0A,6 //This bit is for checking TxD Buffer in CPU is empty or not. //#define TXD1_FINISH UCSR1A,6 #define SET_TxD0_FINISH sbi(ucsr0a,6) #define RESET_TXD0_FINISH cbi(ucsr0a,6) #define CHECK_TXD0_FINISH bit_is_set(ucsr0a,6) #define SET_TxD1_FINISH sbi(ucsr1a,6) #define RESET_TXD1_FINISH cbi(ucsr1a,6) #define CHECK_TXD1_FINISH bit_is_set(ucsr1a,6) #define RX_INTERRUPT 0x01 #define TX_INTERRUPT 0x02 #define OVERFLOW_INTERRUPT 0x01 #define SERIAL_PORT0 0 #define SERIAL_PORT1 1 #define BIT_RS485_DIRECTION0 0x08 //Port E #define BIT_RS485_DIRECTION1 0x04 //Port E #define BIT_ZIGBEE_RESET PD4 //out : default 1 //PORTD #define BIT_ENABLE_RXD_LINK_PC PD5 //out : default 1 #define BIT_ENABLE_RXD_LINK_ZIGBEE PD6 //out : default 0 #define BIT_LINK_PLUGIN PD7 //in, no pull up void TxD81(byte btxddata); void TxD80(byte btxddata); void TxDString(byte *bdata); void TxD8Hex(byte bsentdata); void TxD32Dec(long llong); byte RxD81(void); void MiliSec(word wdelaytime); void PortInitialize(void); void SerialInitialize(byte bport, byte bbaudrate, byte binterrupt); byte TxPacket(byte bid, byte binstruction, byte bparameterlength); 32

34 byte RxPacket(byte brxlength); void PrintBuffer(byte *bpprintbuffer, byte blength); // --- Gloval Variable Number --- volatile byte gbprxinterruptbuffer[256]; byte gbpparameter[128]; byte gbrxbufferreadpointer; byte gbprxbuffer[128]; byte gbptxbuffer[128]; volatile byte gbrxbufferwritepointer; int main(void) byte bcount,bid, btxpacketlength,brxpacketlength; PortInitialize(); //Port In/Out Direction Definition RS485_RXD; //Set RS485 Direction to Input State. SerialInitialize(SERIAL_PORT0,1,RX_INTERRUPT);//RS485 Initializing(RxInterrupt) SerialInitialize(SERIAL_PORT1,DEFAULT_BAUD_RATE,0); Initializing(None Interrupt) gbrxbufferreadpointer = gbrxbufferwritepointer = 0; RxBuffer Clearing. //RS232 //RS485 sei(); //Enable Interrupt -- Compiler Function TxDString("\r\n [The Example of Dynamixel Evaluation with ATmega128,GCC-AVR]"); //Dynamixel Communication Function Execution Step. // Step 1. Parameter Setting (gbpparameter[]). In case of no parameter instruction(ex. INST_PING), this step is not needed. // Step 2. TxPacket(ID,INSTRUCTION,LengthOfParameter); --Total TxPacket Length is returned // Step 3. RxPacket(ExpectedReturnPacketLength); -- Real RxPacket Length is returned // Step 4 PrintBuffer(BufferStartPointer,LengthForPrinting); bid = 1; TxDString("\r\n\n Example 1. Scanning Dynamixels(0~9). -- Any Key to Continue."); RxD8(); for(bcount = 0; bcount < 0x0A; bcount++) btxpacketlength = TxPacket(bCount,INST_PING,0); brxpacketlength = RxPacket(255); TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength); TxDString(", RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength); if(brxpacketlength == DEFAULT_RETURN_PACKET_SIZE) TxDString(" Found!! ID:");TxD8Hex(bCount); bid = bcount; TxDString("\r\n\n Example 2. Read Firmware Version. -- Any Key to Continue."); RxD8(); gbpparameter[0] = P_VERSION; //Address of Firmware Version gbpparameter[1] = 1; //Read Length btxpacketlength = TxPacket(bID,INST_READ,2); brxpacketlength = RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter [1]); TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength); TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength); if(brxpacketlength == DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]) TxDString("\r\n Return Error : ");TxD8Hex(gbpRxBuffer[4]); TxDString("\r\n Firmware Version : ");TxD8Hex(gbpRxBuffer[5]); TxDString("\r\n\n Example 3. LED ON -- Any Key to Continue."); RxD8(); gbpparameter[0] = P_LED; //Address of LED gbpparameter[1] = 1; //Writing Data btxpacketlength = TxPacket(bID,INST_WRITE,2); brxpacketlength = RxPacket(DEFAULT_RETURN_PACKET_SIZE); TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength); TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength); TxDString("\r\n\n Example 4. LED OFF -- Any Key to Continue."); RxD8(); gbpparameter[0] = P_LED; //Address of LED gbpparameter[1] = 0; //Writing Data btxpacketlength = TxPacket(bID,INST_WRITE,2); brxpacketlength = RxPacket(DEFAULT_RETURN_PACKET_SIZE); TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength); TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength); TxDString("\r\n\n Example 5. Read Control Table. -- Any Key to Continue."); RxD8(); gbpparameter[0] = 0; //Reading Address gbpparameter[1] = 49; //Read Length btxpacketlength = TxPacket(bID,INST_READ,2); brxpacketlength = RxPacket(DEFAULT_RETURN_PACKET_SIZE+gbpParameter [1]); TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength); TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength); if(brxpacketlength == DEFAULT_RETURN_PACKET_SIZE+gbpParameter[1]) TxDString("\r\n"); for(bcount = 0; bcount < 49; bcount++) TxD8('[');TxD8Hex(bCount);TxDString("]:"); TxD8Hex(gbpRxBuffer[bCount+5]);TxD8(' '); TxDString("\r\n\n Example 6. Go 0x200 with Speed 0x Any Key to Continue."); RxD8(); gbpparameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version gbpparameter[1] = 0x00; //Writing Data P_GOAL_POSITION_L gbpparameter[2] = 0x02; //Writing Data P_GOAL_POSITION_H gbpparameter[3] = 0x00; //Writing Data P_GOAL_SPEED_L gbpparameter[4] = 0x01; //Writing Data P_GOAL_SPEED_H btxpacketlength = TxPacket(bID,INST_WRITE,5); brxpacketlength = RxPacket(DEFAULT_RETURN_PACKET_SIZE); TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength); TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength); TxDString("\r\n\n Example 7. Go 0x00 with Speed 0x40 -- Any Key to Continue."); RxD8(); gbpparameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version gbpparameter[1] = 0x00; //Writing Data P_GOAL_POSITION_L gbpparameter[2] = 0x00; //Writing Data P_GOAL_POSITION_H gbpparameter[3] = 0x40; //Writing Data P_GOAL_SPEED_L gbpparameter[4] = 0x00; //Writing Data P_GOAL_SPEED_H btxpacketlength = TxPacket(bID,INST_WRITE,5); brxpacketlength = RxPacket(DEFAULT_RETURN_PACKET_SIZE); TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength); TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength); TxDString("\r\n\n Example 8. Go 0x3ff with Speed 0x3ff -- Any Key to Continue."); RxD8(); gbpparameter[0] = P_GOAL_POSITION_L; //Address of Firmware Version gbpparameter[1] = 0xff; //Writing Data P_GOAL_POSITION_L gbpparameter[2] = 0x03; //Writing Data P_GOAL_POSITION_H gbpparameter[3] = 0xff; //Writing Data P_GOAL_SPEED_L gbpparameter[4] = 0x03; //Writing Data P_GOAL_SPEED_H btxpacketlength = TxPacket(bID,INST_WRITE,5); brxpacketlength = RxPacket(DEFAULT_RETURN_PACKET_SIZE); TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength); TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength); TxDString("\r\n\n Example 9. Torque Off -- Any Key to Continue."); RxD8(); gbpparameter[0] = P_TORQUE_ENABLE; //Address of LED gbpparameter[1] = 0; //Writing Data btxpacketlength = TxPacket(bID,INST_WRITE,2); brxpacketlength = RxPacket(DEFAULT_RETURN_PACKET_SIZE); TxDString("\r\n TxD:"); PrintBuffer(gbpTxBuffer,bTxPacketLength); TxDString("\r\n RxD:"); PrintBuffer(gbpRxBuffer,bRxPacketLength); 33

35 TxDString("\r\n\n End. Push reset button for repeat"); while(1); void PortInitialize(void) DDRA = DDRB = DDRC = DDRD = DDRE = DDRF = 0; //Set all port to input direction first. PORTB = PORTC = PORTD = PORTE = PORTF = PORTG = 0x00; //PortData initialize to 0 cbi(sfior,2); //All Port Pull Up ready DDRE = (BIT_RS485_DIRECTION0 BIT_RS485_DIRECTION1); //set output the bit RS485direction DDRD = (BIT_ZIGBEE_RESET BIT_ENABLE_RXD_LINK_PC BIT_ENA BLE_RXD_LINK_ZIGBEE); PORTD &= ~_BV(BIT_LINK_PLUGIN); // no pull up PORTD = _BV(BIT_ZIGBEE_RESET); PORTD = _BV(BIT_ENABLE_RXD_LINK_PC); PORTD = _BV(BIT_ENABLE_RXD_LINK_ZIGBEE); /* TxPacket() send data to RS485. TxPacket() needs 3 parameter; ID of Dynamixel, Instruction byte, Length of parameters. TxPacket() return length of Return packet from Dynamixel. */ byte TxPacket(byte bid, byte binstruction, byte bparameterlength) byte bcount,bchecksum,bpacketlength; gbptxbuffer[0] = 0xff; gbptxbuffer[1] = 0xff; gbptxbuffer[2] = bid; gbptxbuffer[3] = bparameterlength+2; //Length(Paramter,Instruction,Checksum) gbptxbuffer[4] = binstruction; for(bcount = 0; bcount < bparameterlength; bcount++) gbptxbuffer[bcount+5] = gbpparameter[bcount]; bchecksum = 0; bpacketlength = bparameterlength+4+2; for(bcount = 2; bcount < bpacketlength-1; bcount++) //except 0xff,checksum bchecksum += gbptxbuffer[bcount]; gbptxbuffer[bcount] = ~bchecksum; //Writing Checksum with Bit Inversion RS485_TXD; for(bcount = 0; bcount < bpacketlength; bcount++) sbi(ucsr0a,6);//set_txd0_finish; TxD80(gbpTxBuffer[bCount]); while(!check_txd0_finish); //Wait until TXD Shift register empty RS485_RXD; return(bpacketlength); /* RxPacket() read data from buffer. RxPacket() need a Parameter; Total length of Return Packet. RxPacket() return Length of Return Packet. */ byte RxPacket(byte brxpacketlength) #define RX_TIMEOUT_COUNT2 3000L #define RX_TIMEOUT_COUNT1 (RX_TIMEOUT_COUNT2*10L) unsigned long ulcounter; byte bcount, blength, bchecksum; byte btimeout; btimeout = 0; for(bcount = 0; bcount < brxpacketlength; bcount++) ulcounter = 0; while(gbrxbufferreadpointer == gbrxbufferwritepointer) if(ulcounter++ > RX_TIMEOUT_COUNT1) btimeout = 1; break; if(btimeout) break; gbprxbuffer[bcount] = gbprxinterruptbuffer[gbrxbufferreadpointer++]; blength = bcount; bchecksum = 0; if(gbptxbuffer[2]!= BROADCASTING_ID) if(btimeout && brxpacketlength!= 255) TxDString("\r\n [Error:RxD Timeout]"); CLEAR_BUFFER; if(blength > 3) //checking is available. if(gbprxbuffer[0]!= 0xff gbprxbuffer[1]!= 0xff ) TxDString("\r\n [Error:Wrong Header]"); CLEAR_BUFFER; return 0; if(gbprxbuffer[2]!= gbptxbuffer[2] ) TxDString("\r\n [Error:TxID!= RxID]"); CLEAR_BUFFER; return 0; if(gbprxbuffer[3]!= blength-4) TxDString("\r\n [Error:Wrong Length]"); CLEAR_BUFFER; return 0; for(bcount = 2; bcount < blength; bcount++) bchecksum += gbprxbuffer[bcount]; if(bchecksum!= 0xff) TxDString("\r\n [Error:Wrong CheckSum]"); CLEAR_BUFFER; return 0; return blength; /* PrintBuffer() print data in Hex code. PrintBuffer() needs two parameter; name of Pointer(gbpTxBuffer, gbprxbuffer) */ void PrintBuffer(byte *bpprintbuffer, byte blength) byte bcount; for(bcount = 0; bcount < blength; bcount++) TxD8Hex(bpPrintBuffer[bCount]); TxD8(' '); 34

36 TxDString("(LEN:");TxD8Hex(bLength);TxD8(')'); TXD0_DATA = btxddata; /* /* Print value of Baud Rate. TXD81() send data to USART 1. */ */ void PrintBaudrate(void) void TxD81(byte btxddata) TxDString("\r\n RS232:");TxD32Dec(( L/8L)/((long)UBRR1L+1 L) ); TxDString(" BPS,"); while(!txd1_ready); TXD1_DATA = btxddata; TxDString(" RS485:");TxD32Dec(( L/8L)/((long)UBRR0L+1L) ); TxDString(" BPS"); /* TXD32Dex() change data to decimal number system */ void TxD32Dec(long llong) /*Hardware Dependent Item*/ #define TXD1_READY bit_is_set(ucsr1a,5) //(UCSR1A_Bit5) byte bcount, bprinted; long ltmp,ldigit; #define TXD1_DATA (UDR1) bprinted = 0; #define RXD1_READY bit_is_set(ucsr1a,7) if(llong < 0) #define RXD1_DATA (UDR1) llong = -llong; #define TXD0_READY bit_is_set(ucsr0a,5) TxD8('-'); #define TXD0_DATA (UDR0) #define RXD0_READY bit_is_set(ucsr0a,7) ldigit = L; #define RXD0_DATA (UDR0) for(bcount = 0; bcount < 9; bcount++) /* ltmp = (byte)(llong/ldigit); SerialInitialize() set Serial Port to initial state. if(ltmp) Vide Mega128 Data sheet about Setting bit of register. SerialInitialize() needs port, Baud rate, Interrupt value. TxD8(((byte)lTmp)+'0'); bprinted = 1; */ void SerialInitialize(byte bport, byte bbaudrate, byte binterrupt) else if(bprinted) TxD8(((byte)lTmp)+'0'); llong -= ((long)ltmp)*ldigit; if(bport == SERIAL_PORT0) ldigit = ldigit/10; UBRR0H = 0; UBRR0L = bbaudrate; ltmp = (byte)(llong/ldigit); UCSR0A = 0x02; UCSR0B = 0x18; /*if(ltmp)*/ TxD8(((byte)lTmp)+'0'); if(binterrupt&rx_interrupt) sbi(ucsr0b,7); // RxD interrupt enable UCSR0C = 0x06; UDR0 = 0xFF; sbi(ucsr0a,6);//set_txd0_finish; // Note. set 1, then 0 is read /* TxDString() prints data in ACSII code. else if(bport == SERIAL_PORT1) */ void TxDString(byte *bdata) UBRR1H = 0; UBRR1L = bbaudrate; UCSR1A = 0x02; UCSR1B = 0x18; while(*bdata) if(binterrupt&rx_interrupt) sbi(ucsr1b,7); // RxD interrupt enable UCSR1C = 0x06; UDR1 = 0xFF; TxD8(*bData++); sbi(ucsr1a,6);//set_txd1_finish; // Note. set 1, then 0 is read /* /* RxD81() read data from UART1. TxD8Hex() print data seperatly. RxD81() return Read data. ex> 0x1a -> '1' 'a'. */ */ byte RxD81(void) void TxD8Hex(byte bsentdata) while(!rxd1_ready); byte btmp; return(rxd1_data); btmp =((byte)(bsentdata>>4)&0x0f) + (byte)'0'; if(btmp > '9') btmp += 7; /* TxD8(bTmp); SIGNAL() UART0 Rx Interrupt - write data to buffer btmp =(byte)(bsentdata & 0x0f) + (byte)'0'; */ if(btmp > '9') btmp += 7; SIGNAL (SIG_UART0_RECV) TxD8(bTmp); gbprxinterruptbuffer[(gbrxbufferwritepointer++)] = RXD0_DATA; /* TxD80() send data to USART 0. */ void TxD80(byte btxddata) while(!txd0_ready); 35

37 Connector Female Connector Company Name : Molex Pin Number: 4 Model Number Molex Part Number Old Part Number Male Female Temperature range : -40 C to +105 C Contact Insertion Force-max : 14.7N (3.30 lb) Contact Retention Force-min : 14.7N (3.30 lb) or for more detail information Male Connector Pin No.1 36

38 Dimension CM-5 전용콘트롤박스. 30개의 제어가능 6개의 Push button(5개 : 선택용, 1개 reset용 ) Wireless장치를추가장착할수있다. 충전지수납공간 (AA X 8) 이있으며충전기능이있다.( 외부 SMPS연결시 ) CM-5 37