19 #include "../../include/dynamixel_workbench_toolbox/dynamixel_driver.h" 22 sync_write_handler_cnt_(0),
23 sync_read_handler_cnt_(0),
24 bulk_read_parameter_cnt_(0)
59 if (
tools_[num].getDynamixelCount() <
tools_[num].getDynamixelBuffer())
67 if (log != NULL) *log =
"[DynamixelDriver] Too many Dynamixels are connected (default buffer size is 16, the same series of Dynamixels)";
81 if (log != NULL) *log =
"[DynamixelDriver] Too many series are connected (MAX = 5 different series)";
85 if (log != NULL) *log =
"[DynamixelDriver] Failed to set the Tool";
95 if (
tools_[i].getID()[j] == id)
102 if (log != NULL) *log =
"[DynamixelDriver] Failed to get the Tool";
111 if (result ==
false)
return false;
114 if (result ==
false)
return false;
117 if (result ==
false)
return false;
124 return init(device_name, baud_rate, log);
133 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to open the port!";
137 if (log != NULL) *log =
"[DynamixelDriver] Failed to open the port!";
145 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to change the baudrate!";
149 if (log != NULL) *log =
"[DynamixelDriver] Failed to change the baudrate!";
159 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to set the protocol!";
163 if (log != NULL) *log =
"[DynamixelDriver] Failed to set the protocol!";
179 uint8_t factor =
getTool(
id, log);
190 uint8_t factor =
getTool(
id, log);
191 if (factor == 0xff)
return false;
195 if (
tools_[factor].getID()[i] == id)
204 uint8_t factor =
getTool(
id, log);
205 if (factor == 0xff)
return NULL;
214 uint8_t factor =
getTool(
id, log);
215 if (factor == 0xff)
return NULL;
218 if (control_item == NULL)
return NULL;
219 else return control_item;
226 uint8_t factor =
getTool(
id, log);
227 if (factor == 0xff)
return false;
234 uint8_t factor =
getTool(
id, log);
235 if (factor == 0xff)
return NULL;
263 uint16_t model_number = 0;
265 uint8_t get_end_num = end_num;
267 if (get_end_num > 253) get_end_num = 253;
272 if (result ==
false)
return false;
274 for (
id = start_num;
id <= get_end_num;
id++)
287 get_id[id_cnt++] = id;
294 *get_the_number_of_id = id_cnt;
299 if (result ==
false)
return false;
301 for (
id = start_num;
id <= get_end_num;
id++)
315 get_id[id_cnt++] = id;
322 *get_the_number_of_id = id_cnt;
331 return scan(get_id, get_the_number_of_id, 0, range, log);
339 uint16_t model_number = 0;
342 if (result ==
false)
return false;
356 if (get_model_number != NULL) *get_model_number = model_number;
361 if (result ==
false)
return false;
375 if (get_model_number != NULL) *get_model_number = model_number;
384 return ping(
id, NULL, log);
404 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to clear!";
417 if (log != NULL) *log =
"[DynamixelDriver] reboot functions is not available with the Dynamixel Protocol 1.0.";
423 #if defined(__OPENCR__) || defined(__OPENCM904__) 441 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to reboot!";
454 uint32_t new_baud_rate = 0;
458 if (model_name == NULL)
return false;
461 if (model_number == 0)
return false;
466 #if defined(__OPENCR__) || defined(__OPENCM904__) 484 if (!strncmp(model_name,
"AX", strlen(
"AX")) ||
485 !strncmp(model_name,
"MX-12W", strlen(
"MX-12W")))
486 new_baud_rate = 1000000;
488 new_baud_rate = 57600;
495 if (!strncmp(model_name,
"MX-28-2", strlen(
"MX-28-2")) ||
496 !strncmp(model_name,
"MX-64-2", strlen(
"MX-64-2")) ||
497 !strncmp(model_name,
"MX-106-2", strlen(
"MX-106-2")) ||
498 !strncmp(model_name,
"XL", strlen(
"XL")) ||
499 !strncmp(model_name,
"XM", strlen(
"XM")) ||
500 !strncmp(model_name,
"XH", strlen(
"XH")) ||
501 !strncmp(model_name,
"PRO", strlen(
"PRO"))||
502 !strncmp(model_name,
"RH", strlen(
"RH")))
505 if (result ==
false)
return false;
510 if (result ==
false)
return false;
516 result =
setTool(model_number, new_id, log);
517 if (result ==
false)
return false;
519 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to reset!";
525 #if defined(__OPENCR__) || defined(__OPENCM904__) 543 if (!strncmp(model_name,
"XL-320", strlen(
"XL-320")))
544 new_baud_rate = 1000000;
546 new_baud_rate = 57600;
554 if (result ==
false)
return false;
559 result =
setTool(model_number, new_id, log);
560 if (result ==
false)
return false;
562 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to reset!";
573 #if defined(__OPENCR__) || defined(__OPENCM904__) 597 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to write!";
610 uint8_t factor =
getTool(
id, log);
611 if (factor == 0xff)
return false;
614 if (control_item == NULL)
return false;
616 uint8_t data_1_byte = (uint8_t)data;
617 uint16_t data_2_byte = (uint16_t)data;
618 uint32_t data_4_byte = (uint32_t)data;
620 #if defined(__OPENCR__) || defined(__OPENCM904__) 673 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to write!";
684 #if defined(__OPENCR__) || defined(__OPENCM904__) 702 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to write!";
715 uint8_t factor =
getTool(
id, log);
716 if (factor == 0xff)
return false;
719 if (control_item == NULL)
return false;
721 #if defined(__OPENCR__) || defined(__OPENCM904__) 765 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to write!";
776 uint8_t data_read[length];
782 (uint8_t *)&data_read,
799 *data = data_read[0];
811 for (uint16_t index = 0; index < length; index++)
813 data[index] = (uint32_t)data_read[index];
817 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to read!";
830 uint8_t factor =
getTool(
id, log);
831 if (factor == 0xff)
return false;
834 if (control_item == NULL)
return false;
836 uint8_t data_1_byte = 0;
837 uint16_t data_2_byte = 0;
838 uint32_t data_4_byte = 0;
906 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to read!";
925 if (log != NULL) *log =
"[DynamixelDriver] Too many sync write handler are added (MAX = 5)";
938 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to add sync write handler";
946 uint8_t factor =
getTool(
id, log);
947 if (factor == 0xff)
return false;
950 if (control_item == NULL)
return false;
954 if (log != NULL) *log =
"[DynamixelDriver] Too many sync write handler are added (MAX = 5)";
967 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to add sync write handler";
976 uint8_t parameter[4] = {0, 0, 0, 0};
986 if (log != NULL) *log =
"groupSyncWrite addparam failed";
1003 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to sync write!";
1011 uint8_t parameter[4] = {0, 0, 0, 0};
1012 uint8_t multi_parameter[4*data_num_for_each_id];
1015 for (
int i = 0; i < id_num; i++)
1017 for (
int j = 0; j < data_num_for_each_id; j++)
1020 for (
int k = 0; k < 4; k++)
1022 multi_parameter[4*j+k] = parameter[k];
1029 if (log != NULL) *log =
"groupSyncWrite addparam failed";
1043 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to sync write!";
1051 if (log != NULL) *log =
"[DynamixelDriver] Too many sync read handler are added (MAX = 5)";
1064 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to add sync read handler";
1072 uint8_t factor =
getTool(
id, log);
1073 if (factor == 0xff)
return false;
1076 if (control_item == NULL)
return false;
1080 if (log != NULL) *log =
"[DynamixelDriver] Too many sync read handler are added (MAX = 5)";
1093 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to add sync read handler";
1109 if (log != NULL) *log =
"groupSyncWrite addparam failed";
1122 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to sync read!";
1131 for (
int i = 0; i < id_num; i++)
1136 if (log != NULL) *log =
"groupSyncWrite addparam failed";
1148 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to sync read!";
1165 if (log != NULL) *log =
"groupSyncRead getdata failed";
1177 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to get sync read data!";
1185 for (
int i = 0; i < id_num; i++)
1192 if (log != NULL) *log =
"groupSyncRead getdata failed";
1203 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to get sync read data!";
1211 for (
int i = 0; i < id_num; i++)
1218 if (log != NULL) *log =
"groupSyncRead getdata failed";
1229 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to get sync read data!";
1237 if (log != NULL) *log =
"[DynamixelDriver] Failed to load portHandler!";
1241 if (log != NULL) *log =
"[DynamixelDriver] Failed to load packetHandler!";
1247 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to init groupBulkWrite!";
1258 uint8_t parameter[4] = {0, 0, 0, 0};
1267 if (log != NULL) *log =
"groupBulkWrite addparam failed";
1271 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to add param for bulk write!";
1279 uint8_t parameter[4] = {0, 0, 0, 0};
1283 uint8_t factor =
getTool(
id, log);
1284 if (factor == 0xff)
return false;
1287 if (control_item == NULL)
return false;
1296 if (log != NULL) *log =
"groupBulkWrite addparam failed";
1300 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to add param for bulk write!";
1316 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to bulk write!";
1325 if (log != NULL) *log =
"[DynamixelDriver] Failed to load portHandler!";
1329 if (log != NULL) *log =
"[DynamixelDriver] Failed to load packetHandler!";
1335 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to init groupBulkRead!";
1352 if (log != NULL) *log =
"grouBulkRead addparam failed";
1365 if (log != NULL) *log =
"[DynamixelDriver] Too many bulk parameter are added (default buffer size is 10)";
1369 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to add param for bulk read!";
1379 uint8_t factor =
getTool(
id, log);
1380 if (factor == 0xff)
return false;
1383 if (control_item == NULL)
return false;
1390 if (log != NULL) *log =
"grouBulkRead addparam failed";
1403 if (log != NULL) *log =
"[DynamixelDriver] Too many bulk parameter are added (default buffer size is 10)";
1407 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to add param for bulk read!";
1422 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to bulk read!";
1437 if (log != NULL) *log =
"groupBulkRead getdata failed";
1448 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to get bulk read data!";
1456 for (
int i = 0; i < id_num; i++)
1463 if (log != NULL) *log =
"groupBulkRead getdata failed";
1474 if (log != NULL) *log =
"[DynamixelDriver] Succeeded to get bulk read data!";
SyncWriteHandler syncWriteHandler_[MAX_HANDLER_NUM]
virtual int read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error=0)=0
bool isAvailable(uint8_t id, uint16_t address, uint16_t data_length)
static PortHandler * getPortHandler(const char *port_name)
uint32_t getBaudrate(void)
bool addBulkWriteParam(uint8_t id, uint16_t address, uint16_t length, int32_t data, const char **log=NULL)
bool addBulkReadParam(uint8_t id, uint16_t address, uint16_t length, const char **log=NULL)
dynamixel::GroupBulkRead * groupBulkRead_
bool isAvailable(uint8_t id, uint16_t address, uint16_t data_length)
virtual bool openPort()=0
const ControlItem * control_item
bool getBulkReadData(int32_t *data, const char **log=NULL)
virtual int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data)=0
uint8_t getTheNumberOfSyncWriteHandler(void)
bool addParam(uint8_t id, uint8_t *data)
virtual int writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error=0)=0
#define MAX_BULK_PARAMETER
dynamixel::GroupSyncWrite * groupSyncWrite
bool setPacketHandler(float protocol_version, const char **log=NULL)
bool addParam(uint8_t id, uint16_t start_address, uint16_t data_length)
bool init(const char *device_name="/dev/ttyUSB0", uint32_t baud_rate=57600, const char **log=NULL)
bool addParam(uint8_t id)
dynamixel::PacketHandler * packetHandler_
uint32_t getData(uint8_t id, uint16_t address, uint16_t data_length)
const ControlItem * control_item
virtual int read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error=0)=0
bool syncWrite(uint8_t index, int32_t *data, const char **log=NULL)
virtual int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data)=0
bool readRegister(uint8_t id, uint16_t address, uint16_t length, uint32_t *data, const char **log=NULL)
const char * getModelName(uint8_t id, const char **log=NULL)
bool writeRegister(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, const char **log=NULL)
static const char * model_name
virtual int reboot(PortHandler *port, uint8_t id, uint8_t *error=0)=0
bool clearMultiTurn(uint8_t id, const char **log=NULL)
bool reset(uint8_t id, const char **log=NULL)
SyncReadHandler syncReadHandler_[MAX_HANDLER_NUM]
virtual void closePort()=0
virtual int readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error=0)=0
virtual const char * getRxPacketError(uint8_t error)=0
bool writeOnlyRegister(uint8_t id, uint16_t address, uint16_t length, uint8_t *data, const char **log=NULL)
bool addSyncWriteHandler(uint16_t address, uint16_t length, const char **log=NULL)
bool addSyncReadHandler(uint16_t address, uint16_t length, const char **log=NULL)
bool begin(const char *device_name="/dev/ttyUSB0", uint32_t baud_rate=57600, const char **log=NULL)
uint8_t sync_write_handler_cnt_
const ControlItem * getItemInfo(uint8_t id, const char *item_name, const char **log=NULL)
bool ping(uint8_t id, uint16_t *get_model_number, const char **log=NULL)
bool scan(uint8_t *get_id, uint8_t *get_the_number_of_id, uint8_t range=253, const char **log=NULL)
bool getSyncReadData(uint8_t index, int32_t *data, const char **log=NULL)
DynamixelTool tools_[MAX_DXL_SERIES_NUM]
virtual int write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error=0)=0
uint8_t bulk_read_parameter_cnt_
virtual int writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data)=0
virtual float getProtocolVersion()=0
static PacketHandler * getPacketHandler(float protocol_version=2.0)
uint8_t getTheNumberOfSyncReadHandler(void)
dynamixel::PortHandler * portHandler_
bool initBulkWrite(const char **log=NULL)
bool clearBulkReadParam(void)
virtual int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data)=0
const ModelInfo * getModelInfo(uint8_t id, const char **log=NULL)
dynamixel::GroupBulkWrite * groupBulkWrite_
uint32_t getData(uint8_t id, uint16_t address, uint16_t data_length)
bool setPortHandler(const char *device_name, const char **log=NULL)
virtual int write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error=0)=0
bool initBulkRead(const char **log=NULL)
uint8_t getTheNumberOfBulkReadParam(void)
dynamixel::GroupSyncRead * groupSyncRead
virtual int write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error=0)=0
#define MAX_DXL_SERIES_NUM
bool bulkWrite(const char **log=NULL)
const ControlItem * getControlTable(uint8_t id, const char **log=NULL)
bool reboot(uint8_t id, const char **log=NULL)
uint8_t getTheNumberOfControlItem(uint8_t id, const char **log=NULL)
bool bulkRead(const char **log=NULL)
virtual int read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error=0)=0
void getParam(int32_t data, uint8_t *param)
virtual int getBaudRate()=0
bool setTool(uint16_t model_number, uint8_t id, const char **log=NULL)
virtual int ping(PortHandler *port, uint8_t id, uint8_t *error=0)=0
uint8_t getTool(uint8_t id, const char **log=NULL)
bool syncRead(uint8_t index, const char **log=NULL)
virtual int factoryReset(PortHandler *port, uint8_t id, uint8_t option=0, uint8_t *error=0)=0
virtual bool setBaudRate(const int baudrate)=0
virtual const char * getTxRxResult(int result)=0
uint16_t getModelNumber(uint8_t id, const char **log=NULL)
uint8_t sync_read_handler_cnt_
float getProtocolVersion(void)
virtual int clearMultiTurn(PortHandler *port, uint8_t id, uint8_t *error=0)=0
bool setBaudrate(uint32_t baud_rate, const char **log=NULL)
BulkParameter bulk_read_param_[MAX_BULK_PARAMETER]
bool addParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data)