User s Manual 2005-02-23 (2 nd Edition) Closer to Real, Dynamixel DX-113, DX-116, DX-117
목차 1. 요약 1-1. DX-113, 116, 117의개요와특징 Page 2 2-2. 주요사양요약 Page 3 2. Dynamixel 기동 2-1. 기계부조립방법 Page 4 2-2. Connector 조립 Page 4 2-3. Dynamixel의배선연결 Page 5 3. Communication Protocol 3-1. Communication 개요 Page 8 3-2. Instruction Packet Page 9 3-3. Status Packet Page 9 3-4. Control Table Page 11 4. Instruction Set과그사용예 4-1. WRITE DATA Page 18 4-2. READ DATA Page 19 4-3. REG WRITE와 ACTION Page 19 4-4. PING Page 20 4-5. RESET Page 21 4-6. SYNC WRITE Page 22 ` 5. Example Page 23 ` Appendix Page 29 1
1. Dynamixel 1-1. 의개요와특징 Dynamixel Dynamixel은감속기, Control Unit 및 Network기능까지일체형으로구성되어있는 Module형 Smart Actuator이다. Dynamixel은 Compact size임에도불구하고큰 Torque를낼수있고, 강한외력에견딜수있는특수한재질로만들어졌다. 또한내부온도변화나공급전압의변화등내부상황을스스로인식하고처리할수있는기능을갖고있다. Dynamixel중 DX series는 DX-113, DX-116, DX-117 세가지종류가있으며다음과같은장점이있다. 정밀제어 1024 단계의 resolution 으로 position 과 speed 를제어할수있다. Compliance Driving 위치제어에있어서탄력의정도를설정할수있다. Feedback 위치각, 현재속도는물론, 구동중인 Load 의크기까지 Feedback 해줄수있다. Alarm System 내부온도, Torque, 공급전압등이사용자가지정한동작범위를벗어났을때이를알려줄뿐아니라 (Alarming) 스스로적절히처리할수있는기능이있다. Communication Daisy chain 으로연결되어배선이간단하며통신속도는 1M BPS 까지지원된다. 고성능 Motor DX-116,117의경우 Swiss Maxon Motor의가장최상급제품군인 RE MAX Series Coreless DC Motor를채용, 출력 torque가크며, 가속능력도매우우수하다. Distributed Control 한번의명령 Packet으로구동 Schedule을설정할수있기때문에메인프로세서는매우적은 Resource로여러개의 Dynamixel을 Control할수있다. Engineering Plastic Main Body 에 Engineering Plastic 이사용되어큰 Torque 에도견딜수있다. Metal Gear 모든기어가금속으로제작되어, 내구성이높다. Axis Bearing 기어의최종축에 Bearing을사용하였으므로축이강한외력을받을경우도효율이감소하지않는다. Status LED ERROR 상황을 LED 를통하여사용자에게알려준다. 2
1-2. 주요사양요약 DX-116 DX-117 DX-113 Weight(g) 66 66 58 Gear Reduction Ratio 142.5 192.6 192.6 Input Voltage 12 16 12 16 12 Final Max Holding Torque(kgf.cm) 21.38 28.50 28.89 38.52 10.20 Sec/60degree 0.127 0.095 0.172 0.129 0.150 Resolution 0.35 Operating Angle 300 Voltage 12V~16V(Recommended voltage: 14.4V) DX113 : 12V Max. Current 1200mA Operating Temp. -5 ~ +85 Command Signal Digital Packet Protocol Type Half duplex Asynchronous Serial Communication (8bit,1stop,No Parity) Link (Physical) RS 485 Multi Drop(daisy chain type Connector) ID 254 ID (0~253) Communication Speed 7343bps ~ 1 Mbps Feedback Position, Temperature, Load, Input Voltage, etc. Material Full Metal Gear, Engineering Plastic Body Motor Swiss MAXON Motor(DX-116, DX-117). DX-113은 Cored motor사용 3
2. Dynamixel 의설치 2-1. 기계부조립방법 Dynamixel은기계부조립방법은다음과같다. Nut(8EA) Screw for mount(8ea) Horn Screw for Horn 8 조의 Nut 및 Screw 는 Dynamixel 을다른장치에고정시킬때만사용한다. 2-2. Connector 조립아래그림순서대로 Connector를조립한다. Wire Former를사용해서터미널을선에압착시킨다. Wire Former를사용할수없을때는터미널과선을납땜하여동작중에선이빠지지않도록한다. 4
23 2-3. Dynamixel 의배선연결 Pin Assignment Connector의 Pin배열을다음과같으며두개의 Connector는 Dynamixel 내부에서도통되어있다. Pin 1 : GND Pin 2 : +12V~18V Pin 3 : D+ (RS485 Signal) Pin 4 : D- (RS485 Signal) Pin 1 2 3 4 Pin 4 3 4 1 Wire Link 아래그림과같이같은 Pin Number 끼리연결한다. 1Main Controller Main Controller 4RS232 Dynamixel을구동할 Main Controller는 RS485를지원해야한다. Main Controller는직접자작한보드를사용해도좋으나 Dynamixel전용 Controller인 CM-2 Board를사용하기를추천한다. PC LINK CM-2 Board 를경유하여 PC 로 Dynamixel 을제어할수있다. Level RS485 Level PC CM-2 Dynamixels 5
Stand Alone CM-2 Board 는 Dynamixel 로구성된로봇에직접장착되어사용될수도있다. CM-2 Board on Robot 보다자세한 CM-2 Board 의활용은해당 Manual 을참조하기바란다. UART 와의연결 Dynamixel을제어하기위해서는 Main Controller UART의신호레벨을 TTL Level에서 RS485 Level로변환시켜줘야한다. 다음은그권장회로도이다. 전원은 Main Controller의 Molex4P Connector의 Pin1,Pin2를통하여 Dynamixel로공급되어진다. 위의회로도에서 TTL Level 의 TxD 와 RxD 는 DIRECTION485 의 Level 에따라다음과같 6
이 Data 신호의방향이결정된다. DIRECTION485 Level =High인경우 : TxD의신호가 D+,D-로출력 DIRECTION485 Level=Low인경우 : D+,D-의신호가 RxD로입력 RS485 Dynamixel의통신규약인 RS485(IEEE485) 는하나의 Node에여러개의 Terminal이연결되어있는 Multi-Drop Link방식이다. 때문에 RS485 Network에서는여러군데에서동시에 Data를송신하지않도록 Protocol이운영되어야한다. Main Controller [RS485 Multi Drop Link] Note 배선시엔 Pin배열이틀리지않도록각별히주의한다. 배선이끝나면전류소비량을확인한다. Standby상태에서한개의 Dynamixel의소비전류는 50mA이하이다. 연결확인 배선을통하여 Dynamixel에전원이올바르게공급되었다면 Dynamixel의 LED가두번깜박인다. 점검 위의과정을성공하지못했을경우 Connector의 Pin배열을확인한다. 전원공급장치의전압및전류허용량을확인한다. 7
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 통신을한다. 8
3-2. Instruction Packet Instruction Packet은메인컨트롤러가 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 연산자이다. 3-3. Status Packet Status Packet은 Dynamixel이 Instruction Packet을전송받은후그응답으로 Main Controller에게 return하는 Packet이다. Status Packet의구조는다음과같다. OXFF 0XFF ID LENGTH ERROR PARAMETER1 PARAMETER2 PARAMETER N CHECK SUM 9
Packet 을이루는각 byte 들의의미는다음과같다. 0XFF 0XFF 맨앞에위치한두개의 0XFF 는 Packet 의시작을알리는신호이다. ID Packet 을 Return 하는 Dynamixel 의 ID 이다. LENGTH Status Packet 의길이로서, 그값은 Parameter 개수 (N) + 2 이다. ERROR 그다음에위치한 ERROR 는 Dynamixel 이동작중에발생된 Error 상태를나타내며, 각 Bit 별의미는다음표와같다. Bit 명칭 내용 Bit 7 0 - 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 연산자이다. 10
3-4. Control Table EEPROM Area RAM Area Address Item Access Initial Value 0(0X00) Model Number(L) RD 116(0x74) 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 34(0x22) 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) 11
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. DX-116의경우값은 0X0074(116) 이며, DX-117은 0x0075(117), DX-113은 0X0071(113) 이다. Address 0x02 Firmware Version. Address 0x03 ID. Dynamixel을식별하기위한고유번호이다. Link된 Dynamixel들은서로다른 ID가할당되어야한다. Address 0x04 Baud Rate. 통신 Speed를결정한다. 산출공식은다음과같다. Speed(BPS) = 2000000/(Address4+1) 주요 Baud Rate별 Data Value Adress4 설정 BPS 목표 BPS 오차 1 1000000.0 1000000.0 0.000% 3 500000.0 500000.0 0.000% 4 400000.0 400000.0 0.000% 7 250000.0 250000.0 0.000% 9 200000.0 200000.0 0.000% 16 117647.1 115200.0-2.124% 34 57142.9 57600.0 0.794% 103 19230.8 19200.0-0.160% 207 9615.4 9600.0-0.160% 참고 UART 는 Baud Rate 오차가 3% 이내이면통신에지장이없다. 12
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 의값에상관없이 13
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 영역이다. 14
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) 15
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~11 10 9 8 7 6 5 4 3 2 1 0 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까지설정할수있다. 16
Range 각 Data들은유효한범위가정해져있다. 이를벗어난 Write명령이전송될경우 Error가 return된다. 아래표에사용자가 Write할수있는 Data의길이, 그리고범위를정리하였다. 16bit Data는 (L) 과 (H), 두 byte로표시된다. 이두 byte는하나의 Instruction Packet으로동시에 Write되어야한다. Write Address Writing Item Length (bytes) 3(0X03) ID 1 0 253(0xfd) 4(0X04) Baud Rate 1 0 254(0xfe) 5(0X05) Return Delay Time 1 0 254(0xfe) 6(0X06) CW Angle Limit 2 0 1023(0x3ff) 8(0X08) CCW Angle Limit 2 0 1023(0x3ff) 11(0X0B) the Highest Limit Temperature 1 0 150(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 2 0 1023(0x3ff) 16(0X10) Status Return Level 1 0 2 17(0X11) Alarm LED 1 0 127(0x7f) 18(0X12) Alarm Shutdown 1 0 127(0x7f) 19(0X13) (Reserved) 1 0 1 24(0X18) Torque Enable 1 0 1 25(0X19) LED 1 0 1 26(0X1A) CW Compliance Margin 1 0 254(0xfe) 27(0X1B) CCW Compliance Margin 1 0 254(0xfe) 28(0X1C) CW Compliance Slope 1 1 254(0xfe) 29(0X1D) CCW Compliance Slope 1 1 254(0xfe) 30(0X1E) Goal Position 2 0 1023(0x3ff) 32(0X20) Moving Speed 2 0 1023(0x3ff) 34(0X22) Torque Limit 2 0 1023(0x3ff) 44(0X2C) Registered Instruction 1 0 1 47(0X2F) Lock 1 1 1 48(0X30) Punch 2 0 1023(0x3ff) Min [Control Table Data Range and Length for Writing] Max 17
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) 로전송하기로한다. 18
Instruction Packet : 0XFF 0XFF 0XFE 0X04 0X03 0X03 0X01 0XF6` ID LENGTH INSTRUCTION PARAMETERS CHECKSUM Broadcast ID(0XFE) 로전송되었으므로 Status Packet 은 return 되지않는다. 4-2. 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) 이다. 4-3. REG_WRITE 과 ACTION 4-3-1. REG_WRITE Function REG_WRITE 명령은 WRITE_DATA 명령과기능은유사하나, 명령이수행되는시점이다 19
르다. 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 4-3-2. ACTION Function Length Instruction Parameter REG_WRITE로등록된 WRITE 작업을수행하라는명령 0X02 0X05 NONE ACTION 명령은여러개의 Dynamixel을동시에정확히움직여야하는경우유용하다. 여러개의구동장치를통신에의해제어할때, 맨처음명령을전달받는구동장치와맨마지막에명령을전달받는구동장치는구동시점에약간의시간차이가발생한다. Dynamixel에서는 ACTION Instruction을사용하여이러한문제를해결하였다. Broadcasting 두개이상의 Dynamixel에 ACTION 명령을전송할경우 Broadcast ID(0XFE) 를사용하여야하는데, 이때 Packet은 Return되지않는점에유의한다. 4-4. PING Function Length Instruction Parameter 아무것도지시하지않는다. 단지 Status Packet을받고자할때나특정 ID를갖는 Dynamixel의존재를확인하기위해사용된다. 0X02 0X01 NONE 20
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한다. 4-5. 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 로바뀌어있음을유의한다. 21
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되지않는다. 22
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 01 04 02 00 03 F5 (LEN:008) <-[Dynamixel]:FF FF 01 05 00 74 00 08 7D (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 01 04 03 03 00 F4 (LEN:008) <-[Dynamixel]:FF FF 01 02 00 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 00 04 03 04 01 F3 (LEN:008) <-[Dynamixel]:FF FF 00 02 00 FD (LEN:006) Status Packet Result NO ERROR Example 9 ID 가 0 인 Dynamixel 의 Return Delay Time 을 4uSec 로재설정한다. Reurn Delay Time Value 1 이 2uSec 에해당한다. 23
Instruction Packet Communication Instruction = WRITE_DATA, Address = 0x05, DATA = 0x02 ->[Dynamixel]:FF FF 00 04 03 05 02 F1 (LEN:008) <-[Dynamixel]:FF FF 00 02 00 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 00 05 03 08 FF 01 EF (LEN:009) <-[Dynamixel]:FF FF 00 02 00 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 00 04 03 0B 50 9D (LEN:008) <-[Dynamixel]:FF FF 00 02 00 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 00 05 03 0C 64 AA DD (LEN:009) <-[Dynamixel]:FF FF 00 02 00 FD (LEN:006) Status Packet Result NO ERROR 24
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 00 05 03 0E FF 01 E9 (LEN:009) <-[Dynamixel]:FF FF 00 02 00 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 00 04 03 10 00 E8 (LEN:008) <-[Dynamixel]:FF FF 00 02 00 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 00 05 03 11 04 04 DE (LEN:009) <-[Dynamixel]:FF FF 00 02 00 FD (LEN:006) Status Packet Result NO ERROR 25
Example 16 ID 가 0 인 Dynamixel 의 LED 를켜고, Torque 를 Enable 시킨다. Instruction Packet Communication Instruction = WRITE_DATA, Address = 0x18, DATA = 0x01, 0x01 ->[Dynamixel]:FF FF 00 05 03 18 01 01 DD (LEN:009) <-[Dynamixel]:FF FF 00 02 00 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 Output Torque CCW A B C D CW Angle(Position) A : CCW Compliance Slope(Address0x1D) = 0x40( 약 18.8 ) B : CCW Compliance Margin(Address0x1B) = 0x01 ( 약 0.29 ) 26
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 00 07 03 1A 01 01 40 40 59 (LEN:011) <-[Dynamixel]:FF FF 00 02 00 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 00 07 03 1E 00 02 00 02 D3 (LEN:011) <-[Dynamixel]:FF FF 00 02 00 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 00 05 04 1E 00 00 D8 (LEN:009) <-[Dynamixel]:FF FF 00 02 00 FD (LEN:006) ->[Dynamixel]:FF FF 01 05 04 1E FF 03 D5 (LEN:009) <-[Dynamixel]:FF FF 01 02 00 FC (LEN:006) ->[Dynamixel]:FF FF FE 02 05 FA (LEN:006) <-[Dynamixel]: //No return packet against broadcasting ID Status Packet Result NO ERROR 27
Example 20 Instruction Packet Communication ID가 0인 Dynamixel을 Address0x18 ~ Address0x23의값이외에는변경할수없도록한다. Address 0x2F(Lock) 을 1로설정한다. Instruction = WRITE_DATA, Address = 0x2F, DATA = 0x01 ->[Dynamixel]:FF FF 00 04 03 2F 01 C8 (LEN:008) <-[Dynamixel]:FF FF 00 02 00 FD (LEN:006) Status Packet Result NO ERROR Lock이되면전원을제거해야만 unlock할수있다. Lock된상태에서다른데이터를 Access하면 Error가 Return된다. ->[Dynamixel]:FF FF 00 05 03 30 40 00 87 (LEN:009) <-[Dynamixel]:FF FF 00 02 08 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 00 05 03 30 40 00 87 (LEN:009) <-[Dynamixel]:FF FF 00 02 00 FD (LEN:006) Status Packet Result NO ERROR 28
Appendix RS-485 RS-485는 Serial 통신에서사용되는물리규약의하나로여러개의 client가하나의 line에 BUS형태로연결되어운영되는방식이다. 때문에송신과수신이동시에발생할수없으며, 하나의 client가송신하는동안그외의다른모든 Client는입력상태이어야한다. Dynamixel을제어하는 Main Controller는 RS485통신방향을입력으로설정하고있다가 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 구간안에서 RS485 Direction을입력상태로전환해야한다. 485 Direction RS-485에서는송신이끝나는 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 29
485Direction 을전환하는시점에서는 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 PORT_485_DIRECTION = 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 PORT_485_DIRECTION = RX_DIRECTION; //485 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 이다. 30
C Language Example : Dinamixel access with Atmega128 /* * The Example of Dynamixel Evaluation with Atmega128 * Date : 2004.7.20 */ #define ENABLE_BIT_DEFINITIONS //#include <io.h> #include <inttypes.h> #include <avr/io.h> #include <avr/interrupt.h> #include <avr/signal.h> 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 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 #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 #define RS485_TXD PORTE = _BV(PE2); //_485_DIRECTION = 1 #define RS485_RXD PORTE &= ~_BV(PE2);//PORT_485_DIRECTION = 0 //#define PORT_485_DIRECTION PORTE.P2 //Bit2 of PortE is linked to MAX485 direction pin. //#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 LED_M0_ON cbi(porte,3) //PORTE_Bit3 #define LED_M1_ON cbi(porte,4) //PORTE_Bit4 #define LED_M2_ON cbi(porte,6) //PORTE_Bit6 #define LED_E0_ON cbi(porte,7) //PORTE_Bit7 #define LED_E1_ON cbi(portb,0) //PORTB_Bit0 #define LED_M0_OFF sbi(porte,3) //PORTE_Bit3 #define LED_M1_OFF sbi(porte,4) //PORTE_Bit4 #define LED_M2_OFF sbi(porte,6) //PORTE_Bit6 #define LED_E0_OFF sbi(porte,7) //PORTE_Bit7 #define LED_E1_OFF sbi(portb,0) //PORTB_Bit0 #define BIT_LED_M0 0x08 //Port E #define BIT_LED_M1 0x10 //Port E #define BIT_LED_M2 0x40 //Port E #define BIT_LED_E0 0x80 //Port E #define BIT_LED_E1 0x01 //Port B #define BIT_RS485_DIRECTION 0x04 //Port E 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); byte RxPacket(byte brxlength); void PrintBuffer(byte *bpprintbuffer, byte blength); // --- Gloval Variable Number --- 31
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. //RS485 Initializing(RxInterrupt) SerialInitialize(SERIAL_PORT0,DEFAULT_BAUD_RATE,RX_INTERRUPT); //RS232 Initializing(None Interrupt) SerialInitialize(SERIAL_PORT1,DEFAULT_BAUD_RATE,0); //RS485 RxBuffer Clearing. gbrxbufferreadpointer = gbrxbufferwritepointer = 0; 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); RxPacketLength=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 0x100 -- 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); TxDString("\r\n\n End. Push reset button for repeat"); while(1); 32
/* About Register and value of bits, vide Mega128 Data Sheet. */ 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 //Set 5 LED port and RS485Direction port to output DDRB = (BIT_LED_E1); DDRE = (BIT_RS485_DIRECTION BIT_LED_M0 BIT_LED_M1 BIT_LED_M2 BIT_LED_E0); //TurnOff LED LED_M0_OFF; LED_M1_OFF;LED_M2_OFF;LED_E0_OFF;LED_E1_OFF; /* 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); #define RX_WAIT_TIMEOUT 0xf000 #define RX_TIMEOUT_COUNT2 3000L #define RX_TIMEOUT_COUNT1 (RX_TIMEOUT_COUNT2*10L) /* RxPacket() read data from buffer. RxPacket() need a Parameter; Total length of Return Packet. RxPacket() return Length of Return Packet. */ byte RxPacket(byte brxpacketlength) 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(' '); TxDString("(LEN:");TxD8Hex(bLength);TxD8(')'); /* 33
Print value of Baud Rate. */ void PrintBaudrate(void) TxDString("\r\n RS232:");TxD32Dec((16000000L/8L)/((long)UBRR1L+1L) ); TxDString(" BPS,"); TxDString(" RS485:");TxD32Dec((16000000L/8L)/((long)UBRR0L+1L) ); TxDString(" BPS"); /*Hardware Dependent Item*/ #define TXD1_READY #define TXD1_RESET #define TXD1_DATA #define RXD1_READY #define RXD1_RESET #define RXD1_DATA #define TXD0_READY #define TXD0_RESET #define TXD0_DATA #define RXD0_READY #define RXD0_RESET #define RXD0_DATA bit_is_set(ucsr1a,5) //(UCSR1A_Bit5) (UDR1) bit_is_set(ucsr1a,7) (UDR1) bit_is_set(ucsr0a,5) (UDR0) bit_is_set(ucsr0a,7) (UDR0) /* SerialInitialize() set Serial Port to initial state. Vide Mega128 Data sheet about Setting bit of register. SerialInitialize() needs port, Baud rate, Interrupt value. */ void SerialInitialize(byte bport, byte bbaudrate, byte binterrupt) if(bport == SERIAL_PORT0) UBRR0H = 0; UBRR0L = bbaudrate; UCSR0A = 0x02; UCSR0B = 0x18; 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 else if(bport == SERIAL_PORT1) UBRR1H = 0; UBRR1L = bbaudrate; UCSR1A = 0x02; UCSR1B = 0x18; if(binterrupt&rx_interrupt) sbi(ucsr1b,7); // RxD interrupt enable UCSR1C = 0x06; UDR1 = 0xFF; sbi(ucsr1a,6);//set_txd1_finish; // Note. set 1, then 0 is read /* TxD8Hex() print data seperatly. ex> 0x1a -> '1' 'a'. */ void TxD8Hex(byte bsentdata) byte btmp; btmp =((byte)(bsentdata>>4)&0x0f) + (byte)'0'; if(btmp > '9') btmp += 7; TxD8(bTmp); btmp =(byte)(bsentdata & 0x0f) + (byte)'0'; if(btmp > '9') btmp += 7; TxD8(bTmp); /* TxD80() send data to USART 0. */ void TxD80(byte btxddata) while(!txd0_ready); TXD0_DATA = btxddata; /* TXD81() send data to USART 1. */ void TxD81(byte btxddata) while(!txd1_ready); TXD1_DATA = btxddata; /* TXD32Dex() change data to decimal number system */ void TxD32Dec(long llong) byte bcount, bprinted; long ltmp,ldigit; bprinted = 0; if(llong < 0) llong = -llong; TxD8('-'); ldigit = 1000000000L; for(bcount = 0; bcount < 9; bcount++) ltmp = (byte)(llong/ldigit); if(ltmp) TxD8(((byte)lTmp)+'0'); bprinted = 1; else if(bprinted) TxD8(((byte)lTmp)+'0'); llong -= ((long)ltmp)*ldigit; ldigit = ldigit/10; ltmp = (byte)(llong/ldigit); /*if(ltmp)*/ TxD8(((byte)lTmp)+'0'); /* TxDString() prints data in ACSII code. */ void TxDString(byte *bdata) while(*bdata) TxD8(*bData++); /* RxD81() read data from Port 1. RxD81() return Read data. */ byte RxD81(void) while(!rxd1_ready); RXD1_RESET; return(rxd1_data); /* SIGNAL() UART0 Rx Interrupt - write data to buffer */ SIGNAL (SIG_UART0_RECV) gbprxinterruptbuffer[(gbrxbufferwritepointer++)] = RXD0_DATA; 34
C Language Example : Dinamixel access with Am188ER CPU #include <stdio.h> #include <stdlib.h> #include <conio.h> #include <dos.h> #include <mem.h> #define MCS80 #include "..\..\base.h" #include "..\lib188es.c" #define RS485_DIRECTION_BIT 0x2000 #define RS485_TXD (SET_PORT1(RS485_DIRECTION_BIT)) #define RS485_RXD (RESET_PORT1(RS485_DIRECTION_BIT)) #define BROADCASTING_ID 0xfe #define MEMORY_SPARE 10 Definition area #define ADDRESS_TORQUE_ENABLE 20 #define ADDRESS_OPERATING_MODE 19 #define ADDRESS_ID 3 #define ADDRESS_GOAL_POSITION 26 #define INST_PING 0x01 #define INST_READ 0x02 #define INST_WRITE 0x03 #define INST_SET_SCHEDULE 0x04 #define INST_DO_SCHEDULE 0x05 #define INST_RESET 0x06 #define DIGITAL_MODE 0 //Gloval variable number byte gbpinterruptrxbuffer[256+memory_spare]; //485 RxD Data Buffer byte gbrxbufferreadpointer,gbrxbufferwritepointer; //Pointers for access the gbpinterruptrxbuffer void static interrupt far Serial0Interrupt(void); void PrintBuffer(byte *bpprintbuffer, byte blength); byte TxPacket(byte *bptxbuffer, byte bid, byte binstruction, byte *bpparameter, byte bparameterlength); byte RxPacket(byte *bprxbuffer); void main(void) byte bid,bpacketlength; byte bptxbuffer[20+memory_spare]; byte bprxbuffer[256+memory_spare]; byte bpparameter[256+memory_spare]; Major Function CLI; //Disable Interrupt //PortInitialize(); InitPort(OUT, PDATA1, 0xe000); //Set Out(Port30,31:LED,Port29:485Direction) InitPort(IN, PDATA1, 0x0004); //Set In(Port2:Push SW) InitPort(NORMAL_USE, PDATA1, 0x00c0); // RS485_RXD; //Set 485 Direction Select Port to 0 //UartInitialize(); outpw(sp0baud,5); //.2MBPS = 16MHz/16/5 outpw(sp1baud,17); //57600 outpw(sp0sts,0); outpw(sp1sts,0); CPU dependent Initialize 35
//InterruptInitialize(); SetInterrupt(INUM_SERIAL0,Serial0Interrupt,INT_ENABLE INT_RX, 7/*Priority*/); //Memory Initialize gbrxbufferreadpointer = gbrxbufferwritepointer = 0; STI; //Interrupt Enable /* * * Example For Driving Dynamixel DX-116 * */ TxDString("\r\n\n Dynamixel Driving Sample Program"); //Set ID to 3 bpparameter[0] = ADDRESS_ID; bpparameter[1] = 3; bpacketlength = TxPacket(bpTxBuffer, BROADCASTING_ID, INST_WRITE, bpparameter, 2/*Length of Parameter*/); bid = 3; TxDString("\r\n ->[Dynamixel]: "); PrintBuffer(bpTxBuffer,bPacketLength); bpacketlength = RxPacket(bpRxBuffer); TxDString("\r\n <-[Dynamixel]: "); PrintBuffer(bpRxBuffer,bPacketLength); //Set Motor Torque Enable bpparameter[0] = ADDRESS_TORQUE_ENABLE; bpparameter[1] = 1; bpacketlength = TxPacket(bpTxBuffer, bid, INST_WRITE, bpparameter, 2/*Length of Parameter*/); TxDString("\r\n ->[Dynamixel]: "); PrintBuffer(bpTxBuffer,bPacketLength); bpacketlength = RxPacket(bpRxBuffer); TxDString("\r\n <-[Dynamixel]: "); PrintBuffer(bpRxBuffer,bPacketLength); //Move to Position 0x0100 <-> 0x300 while(1) bpparameter[0] = ADDRESS_GOAL_POSITION; bpparameter[1] = 0x00; bpparameter[2] = 0x01; bpacketlength = TxPacket(bpTxBuffer, bid, INST_WRITE, bpparameter, 3/*Length of Parameter*/); TxDString("\r\n ->[Dynamixel]: "); PrintBuffer(bpTxBuffer,bPacketLength); bpacketlength = RxPacket(bpRxBuffer); TxDString("\r\n <-[Dynamixel]: "); PrintBuffer(bpRxBuffer,bPacketLength); MiliSec(1000); bpparameter[0] = ADDRESS_GOAL_POSITION; bpparameter[1] = 0x00; bpparameter[2] = 0x03; bpacketlength = TxPacket(bpTxBuffer, bid, INST_WRITE, bpparameter, 3/*Length of Parameter*/); TxDString("\r\n ->[Dynamixel]: "); PrintBuffer(bpTxBuffer,bPacketLength); bpacketlength = RxPacket(bpRxBuffer); TxDString("\r\n <-[Dynamixel]: "); PrintBuffer(bpRxBuffer,bPacketLength); MiliSec(1000); //while(1); void static interrupt far Serial0Interrupt(void) //Serial RxD Interrupt routine STI; //Enable Interrupt gbpinterruptrxbuffer[gbrxbufferwritepointer++] = RXD_DATA0; //Reading Arrival Data outpw(eoi, 0x14); //End of Interrupt byte RxPacket(byte *bprxbuffer) 36
#define RX_TIMEOUT_COUNT2 10000L //10mSec #define RX_TIMEOUT_COUNT1 (RX_TIMEOUT_COUNT2*10L) //1Sec unsigned long ulcounter; byte bcount; ulcounter = 0; while(gbrxbufferreadpointer == gbrxbufferwritepointer) if(ulcounter++ > RX_TIMEOUT_COUNT1) return 0; bcount = 0; for(bcount = 0; bcount < 254; bcount++) //Maximum Data Length Limit : 255 ulcounter = 0; while(gbrxbufferreadpointer == gbrxbufferwritepointer) if(ulcounter++ > RX_TIMEOUT_COUNT2) return bcount; bprxbuffer[bcount] = gbpinterruptrxbuffer[gbrxbufferreadpointer++]; return bcount; byte TxPacket(byte *bptxbuffer, byte bid, byte binstruction, byte *bpparameter, byte bparameterlength) byte bcount,bchecksum,bpacketlength; bptxbuffer[0] = 0xff; bptxbuffer[1] = 0xff; bptxbuffer[2] = bid; bptxbuffer[3] = bparameterlength+2; //Length(Paramter,Instruction,Checksum) bptxbuffer[4] = binstruction; for(bcount = 0; bcount < bparameterlength; bcount++) bptxbuffer[bcount+5] = bpparameter[bcount]; bchecksum = 0; bpacketlength = bparameterlength+4+2; for(bcount = 2; bcount < bpacketlength-1; bcount++) //except 0xff,checksum bchecksum += bptxbuffer[bcount]; bptxbuffer[bcount] = ~bchecksum; //Writing Checksum with Bit Inversion RS485_TXD; //Change 485 Direction to Transmission for(bcount = 0; bcount < bpacketlength; bcount++) TxD80(bpTxBuffer[bCount]); while(!txd_finish0); //Wait until TXD Shift register empty RS485_RXD; return(bpacketlength); Should wait until last data bit transmission is completed. Note.: Shift register empty is differ from Tx Ready. Tx Ready just means you can load the data to CPU UART TxD Register. There can be several Tx Buffering registers as what kind of CPU void PrintBuffer(byte *bpprintbuffer, byte blength) byte bcount; 37
for(bcount = 0; bcount < blength; bcount++) TxD8Hex(bpPrintBuffer[bCount]); TxD8(' '); Result Set ID to 3 Motor Torque Enable 0xFE is BROADCAST_ID, so Dynamixel does not return status packet.(first 2 Instruction Packet) 38
Connector Female Connector Company Name : Molex Pin Number: 4 Model Number Molex Part Number Old Part Number Male 22-03-5045 5267-04 Female 50-37-5043 5264-04 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) www.molex.com or www.molex.co.jp for more detail information Male Connector Pin No.1 39
Dimension Motor Curve(No reduction gear state) 40
Optional Frame Appilcation Example OF116H OF116S OF116B Body to Body Mount 41
Full Option frame CM-2 Board 의소개 - The Custom-Built Controller for Dynamixel - Optional Part : Blue-tooth module, RS232 UART, and 6-button blue-tooth remocon. - Can be adapted multi-axis robot directly. 86 48 42
Dynamixel Application Example CYCLOIDⅡ 43