21 void swap(int32_t *array);
23 int main(
int argc,
char *argv[])
25 const char* port_name =
"/dev/ttyUSB0";
26 int baud_rate = 57600;
28 uint16_t model_number = 0;
29 uint8_t dxl_id[2] = {0, 0};
33 printf(
"Please set '-port_name', '-baud_rate', '-dynamixel_id_1', '-dynamixel_id_2' arguments for connected Dynamixels\n");
39 baud_rate = atoi(argv[2]);
40 dxl_id[0] = atoi(argv[3]);
41 dxl_id[1] = atoi(argv[4]);
49 result = dxl_wb.
init(port_name, baud_rate, &log);
53 printf(
"Failed to init\n");
58 printf(
"Succeed to init(%d)\n", baud_rate);
60 for (
int cnt = 0; cnt < 2; cnt++)
62 result = dxl_wb.
ping(dxl_id[cnt], &model_number, &log);
66 printf(
"Failed to ping\n");
70 printf(
"Succeeded to ping\n");
71 printf(
"id : %d, model_number : %d\n", dxl_id[cnt], model_number);
74 result = dxl_wb.
jointMode(dxl_id[cnt], 0, 0, &log);
78 printf(
"Failed to change joint mode\n");
82 printf(
"Succeed to change joint mode\n");
110 printf(
"Failed to add bulk read position param\n");
121 printf(
"Failed to add bulk read led param\n");
128 int32_t goal_position[2] = {0, 1023};
129 int32_t led[2] = {0, 1};
131 int32_t get_data[2] = {0, 0};
133 const uint8_t handler_index = 0;
137 result = dxl_wb.
addBulkWriteParam(dxl_id[0],
"Goal_Position", goal_position[0], &log);
141 printf(
"Failed to add bulk write position param\n");
152 printf(
"Failed to add bulk write led param\n");
163 printf(
"Failed to bulk write\n");
172 printf(
"Failed to bulk read\n");
182 printf(
"[ID %d]\tGoal Position : %d\tPresent Position : %d, [ID %d]\tLED : %d\n" 183 ,dxl_id[0], goal_position[0], get_data[0], dxl_id[1], get_data[1]);
186 }
while(abs(goal_position[0] - get_data[0]) > 15);
197 int32_t tmp = array[0];
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)
void swap(int32_t *array)
bool getBulkReadData(int32_t *data, const char **log=NULL)
bool init(const char *device_name="/dev/ttyUSB0", uint32_t baud_rate=57600, const char **log=NULL)
DynamixelWorkbench dxl_wb
bool ping(uint8_t id, uint16_t *get_model_number, const char **log=NULL)
bool initBulkWrite(const char **log=NULL)
bool initBulkRead(const char **log=NULL)
bool bulkWrite(const char **log=NULL)
bool bulkRead(const char **log=NULL)
int main(int argc, char *argv[])
bool jointMode(uint8_t id, int32_t velocity=0, int32_t acceleration=0, const char **log=NULL)