00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #if defined(__linux__)
00020 #include "protocol2_packet_handler.h"
00021 #elif defined(__APPLE__)
00022 #include "protocol2_packet_handler.h"
00023 #elif defined(_WIN32) || defined(_WIN64)
00024 #define WINDLLEXPORT
00025 #include "protocol2_packet_handler.h"
00026 #elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__)
00027 #include "../../include/dynamixel_sdk/protocol2_packet_handler.h"
00028 #endif
00029
00030 #include <stdio.h>
00031 #include <string.h>
00032 #include <stdlib.h>
00033
00034 #define TXPACKET_MAX_LEN (4*1024)
00035 #define RXPACKET_MAX_LEN (4*1024)
00036
00038 #define PKT_HEADER0 0
00039 #define PKT_HEADER1 1
00040 #define PKT_HEADER2 2
00041 #define PKT_RESERVED 3
00042 #define PKT_ID 4
00043 #define PKT_LENGTH_L 5
00044 #define PKT_LENGTH_H 6
00045 #define PKT_INSTRUCTION 7
00046 #define PKT_ERROR 8
00047 #define PKT_PARAMETER0 8
00048
00050 #define ERRNUM_RESULT_FAIL 1 // Failed to process the instruction packet.
00051 #define ERRNUM_INSTRUCTION 2 // Instruction error
00052 #define ERRNUM_CRC 3 // CRC check error
00053 #define ERRNUM_DATA_RANGE 4 // Data range error
00054 #define ERRNUM_DATA_LENGTH 5 // Data length error
00055 #define ERRNUM_DATA_LIMIT 6 // Data limit error
00056 #define ERRNUM_ACCESS 7 // Access error
00057
00058 #define ERRBIT_ALERT 128 //When the device has a problem, this bit is set to 1. Check "Device Status Check" value.
00059
00060 using namespace dynamixel;
00061
00062 Protocol2PacketHandler *Protocol2PacketHandler::unique_instance_ = new Protocol2PacketHandler();
00063
00064 Protocol2PacketHandler::Protocol2PacketHandler() { }
00065
00066 const char *Protocol2PacketHandler::getTxRxResult(int result)
00067 {
00068 switch(result)
00069 {
00070 case COMM_SUCCESS:
00071 return "[TxRxResult] Communication success.";
00072
00073 case COMM_PORT_BUSY:
00074 return "[TxRxResult] Port is in use!";
00075
00076 case COMM_TX_FAIL:
00077 return "[TxRxResult] Failed transmit instruction packet!";
00078
00079 case COMM_RX_FAIL:
00080 return "[TxRxResult] Failed get status packet from device!";
00081
00082 case COMM_TX_ERROR:
00083 return "[TxRxResult] Incorrect instruction packet!";
00084
00085 case COMM_RX_WAITING:
00086 return "[TxRxResult] Now recieving status packet!";
00087
00088 case COMM_RX_TIMEOUT:
00089 return "[TxRxResult] There is no status packet!";
00090
00091 case COMM_RX_CORRUPT:
00092 return "[TxRxResult] Incorrect status packet!";
00093
00094 case COMM_NOT_AVAILABLE:
00095 return "[TxRxResult] Protocol does not support This function!";
00096
00097 default:
00098 return "";
00099 }
00100 }
00101
00102 const char *Protocol2PacketHandler::getRxPacketError(uint8_t error)
00103 {
00104 if (error & ERRBIT_ALERT)
00105 return "[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!";
00106
00107 int not_alert_error = error & ~ERRBIT_ALERT;
00108
00109 switch(not_alert_error)
00110 {
00111 case 0:
00112 return "";
00113
00114 case ERRNUM_RESULT_FAIL:
00115 return "[RxPacketError] Failed to process the instruction packet!";
00116
00117 case ERRNUM_INSTRUCTION:
00118 return "[RxPacketError] Undefined instruction or incorrect instruction!";
00119
00120 case ERRNUM_CRC:
00121 return "[RxPacketError] CRC doesn't match!";
00122
00123 case ERRNUM_DATA_RANGE:
00124 return "[RxPacketError] The data value is out of range!";
00125
00126 case ERRNUM_DATA_LENGTH:
00127 return "[RxPacketError] The data length does not match as expected!";
00128
00129 case ERRNUM_DATA_LIMIT:
00130 return "[RxPacketError] The data value exceeds the limit value!";
00131
00132 case ERRNUM_ACCESS:
00133 return "[RxPacketError] Writing or Reading is not available to target address!";
00134
00135 default:
00136 return "[RxPacketError] Unknown error code!";
00137 }
00138 }
00139
00140 unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size)
00141 {
00142 uint16_t i;
00143 uint16_t crc_table[256] = {0x0000,
00144 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
00145 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027,
00146 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D,
00147 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B,
00148 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9,
00149 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF,
00150 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5,
00151 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093,
00152 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
00153 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197,
00154 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE,
00155 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB,
00156 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9,
00157 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F,
00158 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176,
00159 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123,
00160 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
00161 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104,
00162 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D,
00163 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B,
00164 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A,
00165 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C,
00166 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5,
00167 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3,
00168 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
00169 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7,
00170 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E,
00171 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B,
00172 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9,
00173 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC,
00174 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5,
00175 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243,
00176 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
00177 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264,
00178 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E,
00179 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208,
00180 0x820D, 0x8207, 0x0202 };
00181
00182 for (uint16_t j = 0; j < data_blk_size; j++)
00183 {
00184 i = ((uint16_t)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF;
00185 crc_accum = (crc_accum << 8) ^ crc_table[i];
00186 }
00187
00188 return crc_accum;
00189 }
00190
00191 void Protocol2PacketHandler::addStuffing(uint8_t *packet)
00192 {
00193 int i = 0, index = 0;
00194 int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]);
00195 int packet_length_out = packet_length_in;
00196 uint8_t temp[TXPACKET_MAX_LEN] = {0};
00197
00198 for (uint16_t s = PKT_HEADER0; s <= PKT_LENGTH_H; s++)
00199 temp[s] = packet[s];
00200
00201 index = PKT_INSTRUCTION;
00202 for (i = 0; i < packet_length_in - 2; i++)
00203 {
00204 temp[index++] = packet[i+PKT_INSTRUCTION];
00205 if (packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF)
00206 {
00207 temp[index++] = 0xFD;
00208 packet_length_out++;
00209 }
00210 }
00211 temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-2];
00212 temp[index++] = packet[PKT_INSTRUCTION+packet_length_in-1];
00213
00214
00216 if (packet_length_in != packet_length_out)
00217 packet = (uint8_t *)realloc(packet, index * sizeof(uint8_t));
00218
00220
00221 for (uint16_t s = 0; s < index; s++)
00222 packet[s] = temp[s];
00223
00224 packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out);
00225 packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out);
00226 }
00227
00228 void Protocol2PacketHandler::removeStuffing(uint8_t *packet)
00229 {
00230 int i = 0, index = 0;
00231 int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]);
00232 int packet_length_out = packet_length_in;
00233
00234 index = PKT_INSTRUCTION;
00235 for (i = 0; i < packet_length_in - 2; i++)
00236 {
00237 if (packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION+1] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF)
00238 {
00239 packet_length_out--;
00240 i++;
00241 }
00242 packet[index++] = packet[i+PKT_INSTRUCTION];
00243 }
00244 packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-2];
00245 packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-1];
00246
00247 packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out);
00248 packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out);
00249 }
00250
00251 int Protocol2PacketHandler::txPacket(PortHandler *port, uint8_t *txpacket)
00252 {
00253 uint16_t total_packet_length = 0;
00254 uint16_t written_packet_length = 0;
00255
00256 if (port->is_using_)
00257 return COMM_PORT_BUSY;
00258 port->is_using_ = true;
00259
00260
00261 addStuffing(txpacket);
00262
00263
00264 total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7;
00265
00266 if (total_packet_length > TXPACKET_MAX_LEN)
00267 {
00268 port->is_using_ = false;
00269 return COMM_TX_ERROR;
00270 }
00271
00272
00273 txpacket[PKT_HEADER0] = 0xFF;
00274 txpacket[PKT_HEADER1] = 0xFF;
00275 txpacket[PKT_HEADER2] = 0xFD;
00276 txpacket[PKT_RESERVED] = 0x00;
00277
00278
00279 uint16_t crc = updateCRC(0, txpacket, total_packet_length - 2);
00280 txpacket[total_packet_length - 2] = DXL_LOBYTE(crc);
00281 txpacket[total_packet_length - 1] = DXL_HIBYTE(crc);
00282
00283
00284 port->clearPort();
00285 written_packet_length = port->writePort(txpacket, total_packet_length);
00286 if (total_packet_length != written_packet_length)
00287 {
00288 port->is_using_ = false;
00289 return COMM_TX_FAIL;
00290 }
00291
00292 return COMM_SUCCESS;
00293 }
00294
00295 int Protocol2PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket)
00296 {
00297 int result = COMM_TX_FAIL;
00298
00299 uint16_t rx_length = 0;
00300 uint16_t wait_length = 11;
00301
00302 while(true)
00303 {
00304 rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length);
00305 if (rx_length >= wait_length)
00306 {
00307 uint16_t idx = 0;
00308
00309
00310 for (idx = 0; idx < (rx_length - 3); idx++)
00311 {
00312 if ((rxpacket[idx] == 0xFF) && (rxpacket[idx+1] == 0xFF) && (rxpacket[idx+2] == 0xFD) && (rxpacket[idx+3] != 0xFD))
00313 break;
00314 }
00315
00316 if (idx == 0)
00317 {
00318 if (rxpacket[PKT_RESERVED] != 0x00 ||
00319 rxpacket[PKT_ID] > 0xFC ||
00320 DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) > RXPACKET_MAX_LEN ||
00321 rxpacket[PKT_INSTRUCTION] != 0x55)
00322 {
00323
00324 for (uint16_t s = 0; s < rx_length - 1; s++)
00325 rxpacket[s] = rxpacket[1 + s];
00326
00327 rx_length -= 1;
00328 continue;
00329 }
00330
00331
00332 if (wait_length != DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1)
00333 {
00334 wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1;
00335 continue;
00336 }
00337
00338 if (rx_length < wait_length)
00339 {
00340
00341 if (port->isPacketTimeout() == true)
00342 {
00343 if (rx_length == 0)
00344 {
00345 result = COMM_RX_TIMEOUT;
00346 }
00347 else
00348 {
00349 result = COMM_RX_CORRUPT;
00350 }
00351 break;
00352 }
00353 else
00354 {
00355 continue;
00356 }
00357 }
00358
00359
00360 uint16_t crc = DXL_MAKEWORD(rxpacket[wait_length-2], rxpacket[wait_length-1]);
00361 if (updateCRC(0, rxpacket, wait_length - 2) == crc)
00362 {
00363 result = COMM_SUCCESS;
00364 }
00365 else
00366 {
00367 result = COMM_RX_CORRUPT;
00368 }
00369 break;
00370 }
00371 else
00372 {
00373
00374 for (uint16_t s = 0; s < rx_length - idx; s++)
00375 rxpacket[s] = rxpacket[idx + s];
00376
00377 rx_length -= idx;
00378 }
00379 }
00380 else
00381 {
00382
00383 if (port->isPacketTimeout() == true)
00384 {
00385 if (rx_length == 0)
00386 {
00387 result = COMM_RX_TIMEOUT;
00388 }
00389 else
00390 {
00391 result = COMM_RX_CORRUPT;
00392 }
00393 break;
00394 }
00395 }
00396 }
00397 port->is_using_ = false;
00398
00399 if (result == COMM_SUCCESS)
00400 removeStuffing(rxpacket);
00401
00402 return result;
00403 }
00404
00405
00406 int Protocol2PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error)
00407 {
00408 int result = COMM_TX_FAIL;
00409
00410
00411 result = txPacket(port, txpacket);
00412 if (result != COMM_SUCCESS)
00413 return result;
00414
00415
00416 if (txpacket[PKT_INSTRUCTION] == INST_BULK_READ || txpacket[PKT_INSTRUCTION] == INST_SYNC_READ)
00417 result = COMM_NOT_AVAILABLE;
00418
00419
00420
00421 if (txpacket[PKT_ID] == BROADCAST_ID || txpacket[PKT_INSTRUCTION] == INST_ACTION)
00422 {
00423 port->is_using_ = false;
00424 return result;
00425 }
00426
00427
00428 if (txpacket[PKT_INSTRUCTION] == INST_READ)
00429 {
00430 port->setPacketTimeout((uint16_t)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0+2], txpacket[PKT_PARAMETER0+3]) + 11));
00431 }
00432 else
00433 {
00434 port->setPacketTimeout((uint16_t)11);
00435
00436 }
00437
00438
00439 do {
00440 result = rxPacket(port, rxpacket);
00441 } while (result == COMM_SUCCESS && txpacket[PKT_ID] != rxpacket[PKT_ID]);
00442
00443 if (result == COMM_SUCCESS && txpacket[PKT_ID] == rxpacket[PKT_ID])
00444 {
00445 if (error != 0)
00446 *error = (uint8_t)rxpacket[PKT_ERROR];
00447 }
00448
00449 return result;
00450 }
00451
00452 int Protocol2PacketHandler::ping(PortHandler *port, uint8_t id, uint8_t *error)
00453 {
00454 return ping(port, id, 0, error);
00455 }
00456
00457 int Protocol2PacketHandler::ping(PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error)
00458 {
00459 int result = COMM_TX_FAIL;
00460
00461 uint8_t txpacket[10] = {0};
00462 uint8_t rxpacket[14] = {0};
00463
00464 if (id >= BROADCAST_ID)
00465 return COMM_NOT_AVAILABLE;
00466
00467 txpacket[PKT_ID] = id;
00468 txpacket[PKT_LENGTH_L] = 3;
00469 txpacket[PKT_LENGTH_H] = 0;
00470 txpacket[PKT_INSTRUCTION] = INST_PING;
00471
00472 result = txRxPacket(port, txpacket, rxpacket, error);
00473 if (result == COMM_SUCCESS && model_number != 0)
00474 *model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0+1], rxpacket[PKT_PARAMETER0+2]);
00475
00476 return result;
00477 }
00478
00479 int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vector<uint8_t> &id_list)
00480 {
00481 const int STATUS_LENGTH = 14;
00482 int result = COMM_TX_FAIL;
00483
00484 id_list.clear();
00485
00486 uint16_t rx_length = 0;
00487 uint16_t wait_length = STATUS_LENGTH * MAX_ID;
00488
00489 uint8_t txpacket[10] = {0};
00490 uint8_t rxpacket[STATUS_LENGTH * MAX_ID] = {0};
00491
00492 txpacket[PKT_ID] = BROADCAST_ID;
00493 txpacket[PKT_LENGTH_L] = 3;
00494 txpacket[PKT_LENGTH_H] = 0;
00495 txpacket[PKT_INSTRUCTION] = INST_PING;
00496
00497 result = txPacket(port, txpacket);
00498 if (result != COMM_SUCCESS)
00499 {
00500 port->is_using_ = false;
00501 return result;
00502 }
00503
00504
00505 port->setPacketTimeout((uint16_t)(wait_length * 30));
00506
00507 while(1)
00508 {
00509 rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length);
00510 if (port->isPacketTimeout() == true)
00511 break;
00512 }
00513
00514 port->is_using_ = false;
00515
00516 if (rx_length == 0)
00517 return COMM_RX_TIMEOUT;
00518
00519 while(1)
00520 {
00521 if (rx_length < STATUS_LENGTH)
00522 return COMM_RX_CORRUPT;
00523
00524 uint16_t idx = 0;
00525
00526
00527 for (idx = 0; idx < (rx_length - 2); idx++)
00528 {
00529 if (rxpacket[idx] == 0xFF && rxpacket[idx+1] == 0xFF && rxpacket[idx+2] == 0xFD)
00530 break;
00531 }
00532
00533 if (idx == 0)
00534 {
00535
00536 uint16_t crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH-2], rxpacket[STATUS_LENGTH-1]);
00537
00538 if (updateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc)
00539 {
00540 result = COMM_SUCCESS;
00541
00542 id_list.push_back(rxpacket[PKT_ID]);
00543
00544 for (uint16_t s = 0; s < rx_length - STATUS_LENGTH; s++)
00545 rxpacket[s] = rxpacket[STATUS_LENGTH + s];
00546 rx_length -= STATUS_LENGTH;
00547
00548 if (rx_length == 0)
00549 return result;
00550 }
00551 else
00552 {
00553 result = COMM_RX_CORRUPT;
00554
00555
00556 for (uint16_t s = 0; s < rx_length - 3; s++)
00557 rxpacket[s] = rxpacket[3 + s];
00558 rx_length -= 3;
00559 }
00560 }
00561 else
00562 {
00563
00564 for (uint16_t s = 0; s < rx_length - idx; s++)
00565 rxpacket[s] = rxpacket[idx + s];
00566 rx_length -= idx;
00567 }
00568 }
00569
00570 return result;
00571 }
00572
00573 int Protocol2PacketHandler::action(PortHandler *port, uint8_t id)
00574 {
00575 uint8_t txpacket[10] = {0};
00576
00577 txpacket[PKT_ID] = id;
00578 txpacket[PKT_LENGTH_L] = 3;
00579 txpacket[PKT_LENGTH_H] = 0;
00580 txpacket[PKT_INSTRUCTION] = INST_ACTION;
00581
00582 return txRxPacket(port, txpacket, 0);
00583 }
00584
00585 int Protocol2PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error)
00586 {
00587 uint8_t txpacket[10] = {0};
00588 uint8_t rxpacket[11] = {0};
00589
00590 txpacket[PKT_ID] = id;
00591 txpacket[PKT_LENGTH_L] = 3;
00592 txpacket[PKT_LENGTH_H] = 0;
00593 txpacket[PKT_INSTRUCTION] = INST_REBOOT;
00594
00595 return txRxPacket(port, txpacket, rxpacket, error);
00596 }
00597
00598 int Protocol2PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error)
00599 {
00600 uint8_t txpacket[11] = {0};
00601 uint8_t rxpacket[11] = {0};
00602
00603 txpacket[PKT_ID] = id;
00604 txpacket[PKT_LENGTH_L] = 4;
00605 txpacket[PKT_LENGTH_H] = 0;
00606 txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET;
00607 txpacket[PKT_PARAMETER0] = option;
00608
00609 return txRxPacket(port, txpacket, rxpacket, error);
00610 }
00611
00612 int Protocol2PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length)
00613 {
00614 int result = COMM_TX_FAIL;
00615
00616 uint8_t txpacket[14] = {0};
00617
00618 if (id >= BROADCAST_ID)
00619 return COMM_NOT_AVAILABLE;
00620
00621 txpacket[PKT_ID] = id;
00622 txpacket[PKT_LENGTH_L] = 7;
00623 txpacket[PKT_LENGTH_H] = 0;
00624 txpacket[PKT_INSTRUCTION] = INST_READ;
00625 txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address);
00626 txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address);
00627 txpacket[PKT_PARAMETER0+2] = (uint8_t)DXL_LOBYTE(length);
00628 txpacket[PKT_PARAMETER0+3] = (uint8_t)DXL_HIBYTE(length);
00629
00630 result = txPacket(port, txpacket);
00631
00632
00633 if (result == COMM_SUCCESS)
00634 port->setPacketTimeout((uint16_t)(length + 11));
00635
00636 return result;
00637 }
00638
00639 int Protocol2PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error)
00640 {
00641 int result = COMM_TX_FAIL;
00642 uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);
00643
00644
00645
00646 do {
00647 result = rxPacket(port, rxpacket);
00648 } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id);
00649
00650 if (result == COMM_SUCCESS && rxpacket[PKT_ID] == id)
00651 {
00652 if (error != 0)
00653 *error = (uint8_t)rxpacket[PKT_ERROR];
00654
00655 for (uint16_t s = 0; s < length; s++)
00656 {
00657 data[s] = rxpacket[PKT_PARAMETER0 + 1 + s];
00658 }
00659
00660 }
00661
00662 free(rxpacket);
00663
00664 return result;
00665 }
00666
00667 int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error)
00668 {
00669 int result = COMM_TX_FAIL;
00670
00671 uint8_t txpacket[14] = {0};
00672 uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);
00673
00674
00675 if (id >= BROADCAST_ID)
00676 return COMM_NOT_AVAILABLE;
00677
00678 txpacket[PKT_ID] = id;
00679 txpacket[PKT_LENGTH_L] = 7;
00680 txpacket[PKT_LENGTH_H] = 0;
00681 txpacket[PKT_INSTRUCTION] = INST_READ;
00682 txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address);
00683 txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address);
00684 txpacket[PKT_PARAMETER0+2] = (uint8_t)DXL_LOBYTE(length);
00685 txpacket[PKT_PARAMETER0+3] = (uint8_t)DXL_HIBYTE(length);
00686
00687 result = txRxPacket(port, txpacket, rxpacket, error);
00688 if (result == COMM_SUCCESS)
00689 {
00690 if (error != 0)
00691 *error = (uint8_t)rxpacket[PKT_ERROR];
00692
00693 for (uint16_t s = 0; s < length; s++)
00694 {
00695 data[s] = rxpacket[PKT_PARAMETER0 + 1 + s];
00696 }
00697
00698 }
00699
00700 free(rxpacket);
00701
00702 return result;
00703 }
00704
00705 int Protocol2PacketHandler::read1ByteTx(PortHandler *port, uint8_t id, uint16_t address)
00706 {
00707 return readTx(port, id, address, 1);
00708 }
00709 int Protocol2PacketHandler::read1ByteRx(PortHandler *port, uint8_t id, uint8_t *data, uint8_t *error)
00710 {
00711 uint8_t data_read[1] = {0};
00712 int result = readRx(port, id, 1, data_read, error);
00713 if (result == COMM_SUCCESS)
00714 *data = data_read[0];
00715 return result;
00716 }
00717 int Protocol2PacketHandler::read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error)
00718 {
00719 uint8_t data_read[1] = {0};
00720 int result = readTxRx(port, id, address, 1, data_read, error);
00721 if (result == COMM_SUCCESS)
00722 *data = data_read[0];
00723 return result;
00724 }
00725
00726 int Protocol2PacketHandler::read2ByteTx(PortHandler *port, uint8_t id, uint16_t address)
00727 {
00728 return readTx(port, id, address, 2);
00729 }
00730 int Protocol2PacketHandler::read2ByteRx(PortHandler *port, uint8_t id, uint16_t *data, uint8_t *error)
00731 {
00732 uint8_t data_read[2] = {0};
00733 int result = readRx(port, id, 2, data_read, error);
00734 if (result == COMM_SUCCESS)
00735 *data = DXL_MAKEWORD(data_read[0], data_read[1]);
00736 return result;
00737 }
00738 int Protocol2PacketHandler::read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error)
00739 {
00740 uint8_t data_read[2] = {0};
00741 int result = readTxRx(port, id, address, 2, data_read, error);
00742 if (result == COMM_SUCCESS)
00743 *data = DXL_MAKEWORD(data_read[0], data_read[1]);
00744 return result;
00745 }
00746
00747 int Protocol2PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, uint16_t address)
00748 {
00749 return readTx(port, id, address, 4);
00750 }
00751 int Protocol2PacketHandler::read4ByteRx(PortHandler *port, uint8_t id, uint32_t *data, uint8_t *error)
00752 {
00753 uint8_t data_read[4] = {0};
00754 int result = readRx(port, id, 4, data_read, error);
00755 if (result == COMM_SUCCESS)
00756 *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3]));
00757 return result;
00758 }
00759 int Protocol2PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error)
00760 {
00761 uint8_t data_read[4] = {0};
00762 int result = readTxRx(port, id, address, 4, data_read, error);
00763 if (result == COMM_SUCCESS)
00764 *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3]));
00765 return result;
00766 }
00767
00768
00769 int Protocol2PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
00770 {
00771 int result = COMM_TX_FAIL;
00772
00773 uint8_t *txpacket = (uint8_t *)malloc(length+12);
00774
00775
00776 txpacket[PKT_ID] = id;
00777 txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5);
00778 txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5);
00779 txpacket[PKT_INSTRUCTION] = INST_WRITE;
00780 txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address);
00781 txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address);
00782
00783 for (uint16_t s = 0; s < length; s++)
00784 txpacket[PKT_PARAMETER0+2+s] = data[s];
00785
00786
00787 result = txPacket(port, txpacket);
00788 port->is_using_ = false;
00789
00790 free(txpacket);
00791
00792 return result;
00793 }
00794
00795 int Protocol2PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error)
00796 {
00797 int result = COMM_TX_FAIL;
00798
00799 uint8_t *txpacket = (uint8_t *)malloc(length + 12);
00800
00801 uint8_t rxpacket[11] = {0};
00802
00803 txpacket[PKT_ID] = id;
00804 txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5);
00805 txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5);
00806 txpacket[PKT_INSTRUCTION] = INST_WRITE;
00807 txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address);
00808 txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address);
00809
00810 for (uint16_t s = 0; s < length; s++)
00811 txpacket[PKT_PARAMETER0+2+s] = data[s];
00812
00813
00814 result = txRxPacket(port, txpacket, rxpacket, error);
00815
00816 free(txpacket);
00817
00818 return result;
00819 }
00820
00821 int Protocol2PacketHandler::write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data)
00822 {
00823 uint8_t data_write[1] = { data };
00824 return writeTxOnly(port, id, address, 1, data_write);
00825 }
00826 int Protocol2PacketHandler::write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error)
00827 {
00828 uint8_t data_write[1] = { data };
00829 return writeTxRx(port, id, address, 1, data_write, error);
00830 }
00831
00832 int Protocol2PacketHandler::write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data)
00833 {
00834 uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) };
00835 return writeTxOnly(port, id, address, 2, data_write);
00836 }
00837 int Protocol2PacketHandler::write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error)
00838 {
00839 uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) };
00840 return writeTxRx(port, id, address, 2, data_write, error);
00841 }
00842
00843 int Protocol2PacketHandler::write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data)
00844 {
00845 uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) };
00846 return writeTxOnly(port, id, address, 4, data_write);
00847 }
00848 int Protocol2PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error)
00849 {
00850 uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) };
00851 return writeTxRx(port, id, address, 4, data_write, error);
00852 }
00853
00854 int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data)
00855 {
00856 int result = COMM_TX_FAIL;
00857
00858 uint8_t *txpacket = (uint8_t *)malloc(length + 12);
00859
00860
00861 txpacket[PKT_ID] = id;
00862 txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5);
00863 txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5);
00864 txpacket[PKT_INSTRUCTION] = INST_REG_WRITE;
00865 txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address);
00866 txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address);
00867
00868 for (uint16_t s = 0; s < length; s++)
00869 txpacket[PKT_PARAMETER0+2+s] = data[s];
00870
00871
00872 result = txPacket(port, txpacket);
00873 port->is_using_ = false;
00874
00875 free(txpacket);
00876
00877 return result;
00878 }
00879
00880 int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error)
00881 {
00882 int result = COMM_TX_FAIL;
00883
00884 uint8_t *txpacket = (uint8_t *)malloc(length + 12);
00885
00886 uint8_t rxpacket[11] = {0};
00887
00888 txpacket[PKT_ID] = id;
00889 txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5);
00890 txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5);
00891 txpacket[PKT_INSTRUCTION] = INST_REG_WRITE;
00892 txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address);
00893 txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address);
00894
00895 for (uint16_t s = 0; s < length; s++)
00896 txpacket[PKT_PARAMETER0+2+s] = data[s];
00897
00898
00899 result = txRxPacket(port, txpacket, rxpacket, error);
00900
00901 free(txpacket);
00902
00903 return result;
00904 }
00905
00906 int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length)
00907 {
00908 int result = COMM_TX_FAIL;
00909
00910 uint8_t *txpacket = (uint8_t *)malloc(param_length + 14);
00911
00912
00913 txpacket[PKT_ID] = BROADCAST_ID;
00914 txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7);
00915 txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7);
00916 txpacket[PKT_INSTRUCTION] = INST_SYNC_READ;
00917 txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address);
00918 txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address);
00919 txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length);
00920 txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length);
00921
00922 for (uint16_t s = 0; s < param_length; s++)
00923 txpacket[PKT_PARAMETER0+4+s] = param[s];
00924
00925
00926 result = txPacket(port, txpacket);
00927 if (result == COMM_SUCCESS)
00928 port->setPacketTimeout((uint16_t)((11 + data_length) * param_length));
00929
00930 free(txpacket);
00931 return result;
00932 }
00933
00934 int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length)
00935 {
00936 int result = COMM_TX_FAIL;
00937
00938 uint8_t *txpacket = (uint8_t *)malloc(param_length + 14);
00939
00940
00941
00942 txpacket[PKT_ID] = BROADCAST_ID;
00943 txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7);
00944 txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7);
00945 txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE;
00946 txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address);
00947 txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address);
00948 txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length);
00949 txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length);
00950
00951 for (uint16_t s = 0; s < param_length; s++)
00952 txpacket[PKT_PARAMETER0+4+s] = param[s];
00953
00954
00955 result = txRxPacket(port, txpacket, 0, 0);
00956
00957 free(txpacket);
00958
00959 return result;
00960 }
00961
00962 int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length)
00963 {
00964 int result = COMM_TX_FAIL;
00965
00966 uint8_t *txpacket = (uint8_t *)malloc(param_length + 10);
00967
00968
00969
00970 txpacket[PKT_ID] = BROADCAST_ID;
00971 txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3);
00972 txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3);
00973 txpacket[PKT_INSTRUCTION] = INST_BULK_READ;
00974
00975 for (uint16_t s = 0; s < param_length; s++)
00976 txpacket[PKT_PARAMETER0+s] = param[s];
00977
00978
00979 result = txPacket(port, txpacket);
00980 if (result == COMM_SUCCESS)
00981 {
00982 int wait_length = 0;
00983 for (uint16_t i = 0; i < param_length; i += 5)
00984 wait_length += DXL_MAKEWORD(param[i+3], param[i+4]) + 10;
00985 port->setPacketTimeout((uint16_t)wait_length);
00986 }
00987
00988 free(txpacket);
00989
00990 return result;
00991 }
00992
00993 int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length)
00994 {
00995 int result = COMM_TX_FAIL;
00996
00997 uint8_t *txpacket = (uint8_t *)malloc(param_length + 10);
00998
00999
01000
01001 txpacket[PKT_ID] = BROADCAST_ID;
01002 txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3);
01003 txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3);
01004 txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE;
01005
01006 for (uint16_t s = 0; s < param_length; s++)
01007 txpacket[PKT_PARAMETER0+s] = param[s];
01008
01009
01010 result = txRxPacket(port, txpacket, 0, 0);
01011
01012 free(txpacket);
01013
01014 return result;
01015 }