Skip to content

Instantly share code, notes, and snippets.

@JamesNewton

JamesNewton/dynamixel_raw.ino

Created October 27, 2025 03:54
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save JamesNewton/fdbc2a0fee6aa53d2f76c523899bedbe to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save JamesNewton/fdbc2a0fee6aa53d2f76c523899bedbe to your computer and use it in GitHub Desktop.
Download ZIP
Dynamixel Servo TX/RX Raw
Raw
dynamixel_raw.ino
/*
This code does direct communications in both directions with Dynamixel servos
It doesn't use any libraries and provides minimal ping, write, read of registers
Really useful more just to see how they work than anything else.
I wrote it because a friend needed it to reduce code size on a minimal controller.
In a real environment with a Dynamixel attached, it will monitor register 116.
In testing / debugginer here, it uses the serial port to display testing results.
Obviously, for a real connection, you need to implement the hardware and use an IO
pin to switch the TX pin from driven to High-Z and wire it to the RX pin.
See it run at:
https://wokwi.com/projects/445841596976644097
*/
#include <assert.h> // only used in test
#include <string.h> // only used in test for memcmp() / memcopy()
#define BAUD_RATE 115200
#define SERVO_ID 0x01
#define ADDRESS 116
#define LENGTH 4
#define STATUS_IN 0x55
#define READ_INST 0x02
#define WRITE_INST 0x03
#define test
#define debug
byte buf[100]; //max packet length?
int dlen = 0;
byte c;
enum class S { //states of the decoding. e.g. what it's expecting
H1, //First header byte: FF
H2, //Second header byte: FF
H3, //Third header byte: FD
H4, //"Reserved" should be 0
ID, //Servo id. Be sure to set SERVO_ID correctly
LL, //Lower byte of Length
LH, //High byte of Length
IN, //Instruction. Status responses should always be 55
ER, //Error code (0 is good)
DX, //Data. This will increment dlen and decrement slen
CL, //Low byte of CRC
CH, //High byte of CRC
XX //Success!!
};
S sstate = S::H1;
int slen = 0;
int err = 0;
unsigned short scrc = 0;
unsigned short crc_table[256] = {
0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202
};
//#define crc() { scrc = (scrc << 8) ^ crc_table[((unsigned short)(scrc >> 8) ^ c) & 0xFF]; }
void crc(byte c) {
scrc = (scrc << 8) ^ crc_table[((unsigned short)(scrc >> 8) ^ c) & 0xFF];
}
void decode_in(byte c) {
switch (sstate) {
case S::XX: sstate=S::H1; // break; fall through for restart from success
case S::H1: scrc=0; //reset crc
sstate=(0xff == c? S::H2: S::H1); crc(c); break; //header 1
case S::H2: sstate=(0xff == c? S::H3: S::H1); crc(c); break; //header 2
case S::H3: sstate=(0xfd == c? S::H4: S::H1); crc(c); break; //header 3
case S::H4: sstate=(0x00 == c? S::ID: S::H1); crc(c); break; //reserved
case S::ID: sstate=(SERVO_ID == c? S::LL: S::H1); crc(c); break; //only our addr
case S::LL: sstate=S::LH; slen = c; dlen = 0; crc(c); break; //low byte of length
case S::LH: sstate=S::IN; slen += (c << 8) - 4; crc(c); break; //without inst/crc
case S::IN: sstate=(STATUS_IN == c? S::ER: S::H1);crc(c); break; //only status packets
case S::ER: sstate=S::DX; err=c; crc(c); break; //capture the error code if any.
case S::DX: sstate=(!--slen? S::CL: S::DX); buf[dlen++]=c; crc(c); break; //data
case S::CL: sstate=(c==( scrc & 0xFF)? S::CH: S::H1); break; //crclow
case S::CH: sstate=(c==((scrc>>8)& 0xFF)? S::XX: S::H1); break; //crchigh   
default: sstate = S::H1; err=0; break; //recycle
}
#ifdef debug2
Serial.print(c, HEX);
Serial.print(" ");
Serial.print(scrc, HEX);
Serial.print(" ");
//Serial.print(sstate);
Serial.print(",");
Serial.flush();
#endif
}
int32_t decode_data(int offset=0, int len=dlen, byte * buf = buf) {
int32_t data=0; //max 32 bit value
for (int i = len+offset-1; i>=offset; i--) {
data *= 0x100;
data += buf[i];
}
return data;
}
//takes in the address, data, length of the data (default 4), instruction (default write)
void write(int addr, int32_t data, int len = 4, byte inst = WRITE_INST) {
int i;
if (len > 4) return; //this hack only supports writing 4 bytes max
int plen = 2+len+1+2; //addr + data + instruction + crc
unsigned char packet[] = {
0xff, 0xff, 0xfd, 0x00, //header
SERVO_ID, //ID
plen & 0x00ff, (plen >> 8) & 0x00ff, //length
inst, //instruction. 2=read, 3=write
addr & 0x00ff, (addr >> 8) & 0x00ff,
data & 0x00ff,
(data >> 8) & 0x00ff, //overwrite with CRC if len=1
(data >> 16) & 0x00ff, //overwrite with CRC if len=2
(data >> 24) & 0x00ff, //overwrite with CRC if len=3
0, 0 //CRC if len is 4
};
scrc = 0;
for (int i = 0; i<plen+5; i++) {
crc(packet[i]);
}
packet[plen+5]=scrc & 0x00ff;
packet[plen+6]=(scrc >> 8) & 0x00ff;
#ifdef debug
Serial.print(scrc, HEX);
Serial.print(" ");
printHex(packet, plen+7);
printHex(buf, plen+7);
Serial.flush();
#endif
#ifdef test
assert(memcmp(buf, packet, plen+7) == 0 && "Buffers should be equal");
#else
Serial.write(packet, plen+6); //+header+id+length bytes
#endif
}
void read(int addr, int len = 4) {
//note that the 2 byte len is the "data" after the addr
//it's the length of data requested.
write(addr, len, 2, READ_INST);
}
void ping() {
//Hack: the -2 len causes the address field to be overwritten by the crc
write(0, 0, -2, 1);
}
void setup(void) {
Serial.begin(BAUD_RATE);
#ifdef test
Serial.print("Testing\n");
//Example 5.3.3 Write 4 byte register at 116 with 512. Should produce:
byte e533[]={0xFF,0xFF,0xFD,0x00,0x01,0x09,0x00,0x03,0x74,0x00,0x00,0x02,0x00,0x00,0xCA,0x89};
memcpy(buf, e533, sizeof(e533));
write(116,512);
//Example 1. Ping of a XM430-W210 Model Number 1030(0x0406), Firmware Version 38(0x26)
byte e1[]={0xFF,0xFF,0xFD,0x00,0x01,0x03,0x00,0x01,0x19,0x4E};
memcpy(buf, e1, sizeof(e1));
ping();
//this should return from the servo:
byte r1[] = {0xFF, 0xFF, 0xFD, 0x00, 0x01, 0x07, 0x00, 0x55, 0x00, 0x06, 0x04, 0x26, 0x65, 0x5D};
decode_packet(r1, sizeof(r1));
#ifdef debug
Serial.print(dlen);
Serial.print(" bytes ");
Serial.print(decode_data(0,2));
Serial.print(" : ");
Serial.print(decode_data(2,1));
Serial.print(" : ");
Serial.flush();
#endif
assert(dlen==3);
assert(decode_data(0,2)==1030);
assert(decode_data(2,1)==38);
//Start read of 4 bytes from register 132. Should produce;
byte e2[]={0xFF,0xFF,0xFD,0x00,0x01,0x07,0x00,0x02,0x84,0x00,0x04,0x00,0x1D,0x15};
memcpy(buf, e2, sizeof(e2));
read(132);
byte r2[] = {0xFF, 0xFF, 0xFD, 0x00, 0x01, 0x08, 0x00, 0x55, 0x00, 0xA6, 0x00, 0x00, 0x00, 0x8C, 0xC0};
decode_packet(r2, sizeof(r2));
#ifdef debug
Serial.print(dlen);
Serial.print(" bytes ");
Serial.print(decode_data(0,2));
Serial.print(" : ");
Serial.print(decode_data(2,1));
Serial.print(" : ");
Serial.flush();
#endif
assert(dlen==4);
assert(decode_data()==166);
//for the second example
Serial.print("Passed!\n");
#endif
}
int decode_packet(byte *packet, int len) {
int i=0;
while (len--) {
decode_in(packet[i++]);
if (sstate == S::H1 || sstate == S::XX) {//done'
break;
}
}
if (sstate != S::XX) {
Serial.println(" ");
Serial.print("fail at ");
Serial.print(i-1);
Serial.print(" which is ");
Serial.print(packet[i-1], HEX);
Serial.print(" crc was ");
Serial.println(scrc, HEX);
Serial.flush();
return 0;
}
return dlen;
}
void printHex(const byte* data, size_t length) {
for (size_t i = 0; i < length; i++) {
if (data[i] < 16) {
Serial.print("0");
}
Serial.print(data[i], HEX);
Serial.print(" ");
}
Serial.println();
Serial.flush();
}
void loop() {
if (sstate == S::XX || sstate == S::H1) {
read(ADDRESS, LENGTH); //start a new request
}
int data = 0;
while (-1==Serial.peek());
c = Serial.read();
decode_in(c);
if (sstate == S::XX) {
data = decode_data(0, LENGTH);
}
while (true) {};
//display.setCursor(0,20);
//display.print(String(c,HEX));
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment