19 #if defined(__linux__) 22 #elif defined(__APPLE__) 25 #elif defined(_WIN32) || defined(_WIN64) 29 #elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) 30 #include "../../include/dynamixel_sdk/protocol2_packet_handler.h" 37 #define TXPACKET_MAX_LEN (1*1024) 38 #define RXPACKET_MAX_LEN (1*1024) 44 #define PKT_RESERVED 3 46 #define PKT_LENGTH_L 5 47 #define PKT_LENGTH_H 6 48 #define PKT_INSTRUCTION 7 50 #define PKT_PARAMETER0 8 53 #define ERRNUM_RESULT_FAIL 1 // Failed to process the instruction packet. 54 #define ERRNUM_INSTRUCTION 2 // Instruction error 55 #define ERRNUM_CRC 3 // CRC check error 56 #define ERRNUM_DATA_RANGE 4 // Data range error 57 #define ERRNUM_DATA_LENGTH 5 // Data length error 58 #define ERRNUM_DATA_LIMIT 6 // Data limit error 59 #define ERRNUM_ACCESS 7 // Access error 61 #define ERRBIT_ALERT 128 //When the device has a problem, this bit is set to 1. Check "Device Status Check" value. 74 return "[TxRxResult] Communication success.";
77 return "[TxRxResult] Port is in use!";
80 return "[TxRxResult] Failed transmit instruction packet!";
83 return "[TxRxResult] Failed get status packet from device!";
86 return "[TxRxResult] Incorrect instruction packet!";
89 return "[TxRxResult] Now recieving status packet!";
92 return "[TxRxResult] There is no status packet!";
95 return "[TxRxResult] Incorrect status packet!";
98 return "[TxRxResult] Protocol does not support This function!";
108 return "[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!";
110 int not_alert_error = error & ~ERRBIT_ALERT;
112 switch(not_alert_error)
118 return "[RxPacketError] Failed to process the instruction packet!";
121 return "[RxPacketError] Undefined instruction or incorrect instruction!";
124 return "[RxPacketError] CRC doesn't match!";
127 return "[RxPacketError] The data value is out of range!";
130 return "[RxPacketError] The data length does not match as expected!";
133 return "[RxPacketError] The data value exceeds the limit value!";
136 return "[RxPacketError] Writing or Reading is not available to target address!";
139 return "[RxPacketError] Unknown error code!";
146 static const uint16_t crc_table[256] = {0x0000,
147 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
148 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027,
149 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D,
150 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B,
151 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9,
152 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF,
153 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5,
154 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093,
155 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
156 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197,
157 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE,
158 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB,
159 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9,
160 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F,
161 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176,
162 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123,
163 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
164 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104,
165 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D,
166 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B,
167 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A,
168 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C,
169 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5,
170 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3,
171 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
172 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7,
173 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E,
174 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B,
175 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9,
176 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC,
177 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5,
178 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243,
179 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
180 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264,
181 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E,
182 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208,
183 0x820D, 0x8207, 0x0202 };
185 for (uint16_t j = 0; j < data_blk_size; j++)
187 i = ((uint16_t)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF;
188 crc_accum = (crc_accum << 8) ^ crc_table[i];
197 int packet_length_out = packet_length_in;
199 if (packet_length_in < 8)
203 uint16_t packet_length_before_crc = packet_length_in - 2;
204 for (uint16_t i = 3; i < packet_length_before_crc; i++)
207 if (packet_ptr[0] == 0xFF && packet_ptr[1] == 0xFF && packet_ptr[2] == 0xFD)
211 if (packet_length_in == packet_length_out)
214 uint16_t out_index = packet_length_out + 6 - 2;
215 uint16_t in_index = packet_length_in + 6 - 2;
216 while (out_index != in_index)
218 if (packet[in_index] == 0xFD && packet[in_index-1] == 0xFF && packet[in_index-2] == 0xFF)
220 packet[out_index--] = 0xFD;
221 if (out_index != in_index)
223 packet[out_index--] = packet[in_index--];
224 packet[out_index--] = packet[in_index--];
225 packet[out_index--] = packet[in_index--];
230 packet[out_index--] = packet[in_index--];
242 int i = 0, index = 0;
244 int packet_length_out = packet_length_in;
247 for (i = 0; i < packet_length_in - 2; i++)
265 uint16_t total_packet_length = 0;
266 uint16_t written_packet_length = 0;
291 uint16_t crc =
updateCRC(0, txpacket, total_packet_length - 2);
292 txpacket[total_packet_length - 2] =
DXL_LOBYTE(crc);
293 txpacket[total_packet_length - 1] =
DXL_HIBYTE(crc);
297 written_packet_length = port->
writePort(txpacket, total_packet_length);
298 if (total_packet_length != written_packet_length)
311 uint16_t rx_length = 0;
312 uint16_t wait_length = 11;
316 rx_length += port->
readPort(&rxpacket[rx_length], wait_length - rx_length);
317 if (rx_length >= wait_length)
322 for (idx = 0; idx < (rx_length - 3); idx++)
324 if ((rxpacket[idx] == 0xFF) && (rxpacket[idx+1] == 0xFF) && (rxpacket[idx+2] == 0xFD) && (rxpacket[idx+3] != 0xFD))
331 rxpacket[
PKT_ID] > 0xFC ||
336 for (uint16_t
s = 0;
s < rx_length - 1;
s++)
337 rxpacket[
s] = rxpacket[1 +
s];
344 if (wait_length !=
DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1)
346 wait_length =
DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1;
350 if (rx_length < wait_length)
372 uint16_t crc =
DXL_MAKEWORD(rxpacket[wait_length-2], rxpacket[wait_length-1]);
373 if (
updateCRC(0, rxpacket, wait_length - 2) == crc)
386 for (uint16_t
s = 0;
s < rx_length - idx;
s++)
387 rxpacket[
s] = rxpacket[idx +
s];
408 #if defined(__linux__) || defined(__APPLE__) 410 #elif defined(_WIN32) || defined(_WIN64) 460 if (result ==
COMM_SUCCESS && txpacket[PKT_ID] == rxpacket[PKT_ID])
471 return ping(port,
id, 0, error);
478 uint8_t txpacket[10] = {0};
479 uint8_t rxpacket[14] = {0};
489 result =
txRxPacket(port, txpacket, rxpacket, error);
498 const int STATUS_LENGTH = 14;
503 uint16_t rx_length = 0;
504 uint16_t wait_length = STATUS_LENGTH *
MAX_ID;
506 uint8_t txpacket[10] = {0};
507 uint8_t rxpacket[STATUS_LENGTH *
MAX_ID] = {0};
509 double tx_time_per_byte = (1000.0 / (double)port->
getBaudRate()) * 10.0;
525 port->
setPacketTimeout(((
double)wait_length * tx_time_per_byte) + (3.0 * (
double)MAX_ID) + 16.0);
529 rx_length += port->
readPort(&rxpacket[rx_length], wait_length - rx_length);
541 if (rx_length < STATUS_LENGTH)
547 for (idx = 0; idx < (rx_length - 2); idx++)
549 if (rxpacket[idx] == 0xFF && rxpacket[idx+1] == 0xFF && rxpacket[idx+2] == 0xFD)
556 uint16_t crc =
DXL_MAKEWORD(rxpacket[STATUS_LENGTH-2], rxpacket[STATUS_LENGTH-1]);
558 if (
updateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc)
562 id_list.push_back(rxpacket[
PKT_ID]);
564 for (uint16_t
s = 0;
s < rx_length - STATUS_LENGTH;
s++)
565 rxpacket[
s] = rxpacket[STATUS_LENGTH +
s];
566 rx_length -= STATUS_LENGTH;
576 for (uint16_t
s = 0;
s < rx_length - 3;
s++)
577 rxpacket[
s] = rxpacket[3 +
s];
584 for (uint16_t
s = 0;
s < rx_length - idx;
s++)
585 rxpacket[
s] = rxpacket[idx +
s];
595 uint8_t txpacket[10] = {0};
607 uint8_t txpacket[10] = {0};
608 uint8_t rxpacket[11] = {0};
615 return txRxPacket(port, txpacket, rxpacket, error);
620 uint8_t txpacket[15] = {0};
621 uint8_t rxpacket[11] = {0};
633 return txRxPacket(port, txpacket, rxpacket, error);
638 uint8_t txpacket[11] = {0};
639 uint8_t rxpacket[11] = {0};
647 return txRxPacket(port, txpacket, rxpacket, error);
654 uint8_t txpacket[14] = {0};
683 if (rxpacket == NULL)
695 for (uint16_t
s = 0;
s < length;
s++)
711 uint8_t txpacket[14] = {0};
715 if (rxpacket == NULL)
733 result =
txRxPacket(port, txpacket, rxpacket, error);
739 for (uint16_t
s = 0;
s < length;
s++)
753 return readTx(port,
id, address, 1);
757 uint8_t data_read[1] = {0};
758 int result =
readRx(port,
id, 1, data_read, error);
760 *data = data_read[0];
765 uint8_t data_read[1] = {0};
766 int result =
readTxRx(port,
id, address, 1, data_read, error);
768 *data = data_read[0];
774 return readTx(port,
id, address, 2);
778 uint8_t data_read[2] = {0};
779 int result =
readRx(port,
id, 2, data_read, error);
786 uint8_t data_read[2] = {0};
787 int result =
readTxRx(port,
id, address, 2, data_read, error);
795 return readTx(port,
id, address, 4);
799 uint8_t data_read[4] = {0};
800 int result =
readRx(port,
id, 4, data_read, error);
807 uint8_t data_read[4] = {0};
808 int result =
readTxRx(port,
id, address, 4, data_read, error);
819 uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3));
821 if (txpacket == NULL)
831 for (uint16_t
s = 0;
s < length;
s++)
847 uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3));
848 uint8_t rxpacket[11] = {0};
850 if (txpacket == NULL)
860 for (uint16_t
s = 0;
s < length;
s++)
864 result =
txRxPacket(port, txpacket, rxpacket, error);
873 uint8_t data_write[1] = { data };
874 return writeTxOnly(port,
id, address, 1, data_write);
878 uint8_t data_write[1] = { data };
879 return writeTxRx(port,
id, address, 1, data_write, error);
885 return writeTxOnly(port,
id, address, 2, data_write);
890 return writeTxRx(port,
id, address, 2, data_write, error);
896 return writeTxOnly(port,
id, address, 4, data_write);
901 return writeTxRx(port,
id, address, 4, data_write, error);
908 uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3));
910 if (txpacket == NULL)
920 for (uint16_t
s = 0;
s < length;
s++)
936 uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3));
937 uint8_t rxpacket[11] = {0};
939 if (txpacket == NULL)
949 for (uint16_t
s = 0;
s < length;
s++)
953 result =
txRxPacket(port, txpacket, rxpacket, error);
964 uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3));
967 if (txpacket == NULL)
979 for (uint16_t
s = 0;
s < param_length;
s++)
995 uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3));
998 if (txpacket == NULL)
1010 for (uint16_t
s = 0;
s < param_length;
s++)
1025 uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3));
1028 if (txpacket == NULL)
1036 for (uint16_t
s = 0;
s < param_length;
s++)
1043 int wait_length = 0;
1044 for (uint16_t i = 0; i < param_length; i += 5)
1045 wait_length +=
DXL_MAKEWORD(param[i+3], param[i+4]) + 10;
1058 uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3));
1061 if (txpacket == NULL)
1069 for (uint16_t
s = 0;
s < param_length;
s++)
#define DXL_MAKEDWORD(a, b)
int readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error=0)
The function that transmits INST_READ instruction packet, and read data from received packet The fun...
static Protocol2PacketHandler * unique_instance_
int read4ByteTx(PortHandler *port, uint8_t id, uint16_t address)
The function that calls Protocol2PacketHandler::readTx() function for reading 4 byte data The functi...
int clearMultiTurn(PortHandler *port, uint8_t id, uint8_t *error=0)
The function that reset multi-turn revolution information of Dynamixel The function makes an instruc...
virtual void clearPort()=0
The function that clears the port The function clears the port.
#define ERRNUM_INSTRUCTION
int bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length)
The function that transmits INST_BULK_WRITE instruction packet The function makes an instruction pac...
int ping(PortHandler *port, uint8_t id, uint8_t *error=0)
The function that pings Dynamixel but doesn't take its model number The function calls Protocol2Pack...
int read4ByteRx(PortHandler *port, uint8_t id, uint32_t *data, uint8_t *error=0)
The function that calls Protocol2PacketHandler::readRx() function and reads 4 byte data on the packet...
uint16_t updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size)
int regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynami...
int regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error=0)
The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynami...
#define DXL_MAKEWORD(a, b)
int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data)
The function that calls Protocol2PacketHandler::writeTxOnly() for writing 2 byte data The function c...
int syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length)
The function that transmits INST_SYNC_WRITE instruction packet The function makes an instruction pac...
int factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error=0)
The function that makes Dynamixel reset as it was produced in the factory The function makes an inst...
int writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
The function that transmits INST_WRITE instruction packet with the data for write The function makes...
const char * getTxRxResult(int result)
The function that gets description of communication result.
int write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error=0)
The function that calls Protocol2PacketHandler::writeTxRx() for writing 4 byte data and receives the ...
int rxPacket(PortHandler *port, uint8_t *rxpacket)
The function that receives packet (rxpacket) during designated time via PortHandler port The functio...
#define ERRNUM_DATA_LIMIT
int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data)
The function that calls Protocol2PacketHandler::writeTxOnly() for writing 4 byte data The function c...
int read2ByteRx(PortHandler *port, uint8_t id, uint16_t *data, uint8_t *error=0)
The function that calls Protocol2PacketHandler::readRx() function and reads 2 byte data on the packet...
int read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error=0)
The function that calls Protocol2PacketHandler::readTxRx() function for reading 2 byte data The func...
#define ERRNUM_DATA_RANGE
#define INST_FACTORY_RESET
int txPacket(PortHandler *port, uint8_t *txpacket)
The function that transmits the instruction packet txpacket via PortHandler port. The function clear...
int syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length)
The function that transmits INST_SYNC_READ instruction packet The function makes an instruction pack...
int write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error=0)
The function that calls Protocol2PacketHandler::writeTxRx() for writing 2 byte data and receives the ...
int read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error=0)
The function that calls Protocol2PacketHandler::readTxRx() function for reading 1 byte data The func...
int read2ByteTx(PortHandler *port, uint8_t id, uint16_t address)
The function that calls Protocol2PacketHandler::readTx() function for reading 2 byte data The functi...
int reboot(PortHandler *port, uint8_t id, uint8_t *error=0)
The function that makes Dynamixel reboot The function makes an instruction packet with INST_REBOOT...
The class for port control that inherits PortHandlerLinux, PortHandlerWindows, PortHandlerMac, or PortHandlerArduino.
int read1ByteRx(PortHandler *port, uint8_t id, uint8_t *data, uint8_t *error=0)
The function that calls Protocol2PacketHandler::readRx() function and reads 1 byte data on the packet...
void removeStuffing(uint8_t *packet)
#define ERRNUM_DATA_LENGTH
int readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length)
The function that transmits INST_READ instruction packet The function makes an instruction packet wi...
int read1ByteTx(PortHandler *port, uint8_t id, uint16_t address)
The function that calls Protocol2PacketHandler::readTx() function for reading 1 byte data The functi...
virtual int writePort(uint8_t *packet, int length)=0
The function that writes bytes on the port buffer The function writes bytes on the port buffer...
int writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error=0)
The function that transmits INST_WRITE instruction packet with the data for write, and receives the packet The function makes an instruction packet with INST_WRITE and the data for write, transmits and receives the packet with Protocol2PacketHandler::txRxPacket(), gets the error from the packet.
int write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error=0)
The function that calls Protocol2PacketHandler::writeTxRx() for writing 1 byte data and receives the ...
virtual bool isPacketTimeout()=0
The function that checks whether packet timeout is occurred The function checks whether current time...
int broadcastPing(PortHandler *port, std::vector< uint8_t > &id_list)
(Available only in Protocol 2.0) The function that pings all connected Dynamixel
int readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error=0)
The function that receives the packet and reads the data in the packet The function receives the pac...
The class for control Dynamixel by using Protocol2.0.
int bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length)
The function that transmits INST_BULK_READ instruction packet The function makes an instruction pack...
virtual int readPort(uint8_t *packet, int length)=0
The function that reads bytes from the port buffer The function gets bytes from the port buffer...
#define COMM_NOT_AVAILABLE
int action(PortHandler *port, uint8_t id)
The function that makes Dynamixels run as written in the Dynamixel register The function makes an in...
int txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error=0)
The function that transmits packet (txpacket) and receives packet (rxpacket) during designated time v...
const char * getRxPacketError(uint8_t error)
The function that gets description of hardware error.
virtual int getBaudRate()=0
The function that returns current baudrate set into the port handler The function returns current ba...
int read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error=0)
The function that calls Protocol2PacketHandler::readTxRx() function for reading 4 byte data The func...
bool is_using_
shows whether the port is in use
#define ERRNUM_RESULT_FAIL
virtual void setPacketTimeout(uint16_t packet_length)=0
The function that sets and starts stopwatch for watching packet timeout The function sets the stopwa...
int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data)
The function that calls Protocol2PacketHandler::writeTxOnly() for writing 1 byte data The function c...
void addStuffing(uint8_t *packet)