From 548a3f79f594a3b2c3c817bffd1e028b601bc150 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Thu, 19 Aug 2021 14:32:34 +0200 Subject: [PATCH] open_manipulator_i2c: offset on id 11 --- include/open_manipulator_i2c.h | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/include/open_manipulator_i2c.h b/include/open_manipulator_i2c.h index 4694242..3d041c7 100644 --- a/include/open_manipulator_i2c.h +++ b/include/open_manipulator_i2c.h @@ -15,6 +15,7 @@ #define I2C_FILE "/dev/i2c-2" #define I2C_ADDR (0x60>>1) #define XM_OPERATING_MODE 11 +#define XM_HOMING_OFFSET 20 #define XM_CURRENT_LIMIT 38 #define XM_SHUTDOWN 63 #define XM_TORQUE_ENABLE 64 @@ -36,8 +37,8 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW public: WtOpenManipulatorI2C(ros::NodeHandle nh) { uint8_t mode=255; + int32_t homing_offset=0; this->nh = nh; - int16_t current; torque_enable_service = nh.advertiseService("torque_enable", &WtOpenManipulatorI2C::cbTorqueEnable, this); get_error_service = nh.advertiseService("get_error", &WtOpenManipulatorI2C::cbGetError, this); @@ -68,6 +69,16 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW registerInterface(&jnt_pos_interface); // Settings + if (dynamixel_readdword(11, XM_HOMING_OFFSET, &homing_offset) != 1) { + ROS_ERROR("I2C dynamixel id=11 read homing offset error"); + exit(1); + } + if (homing_offset != 64) { + if (dynamixel_writedword(11, XM_HOMING_OFFSET, 64) != 1) { + ROS_ERROR("I2C dynamixel id=11 write homing offset error"); + exit(1); + } + } if (dynamixel_writeword(15, XM_GOAL_CURRENT, 600) != 1) { // 0..1193, 2.69mA/value ROS_ERROR("I2C dynamixel id=15 write goal current error"); exit(1); @@ -252,7 +263,7 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW } // wait at least the half duplex transmission time, assume 29 (=14+15) bytes send/received: 1/(115200.0/10/29) <= 2.6ms - usleep(3.2*1e3); + //usleep(3.2*1e3); if (read_num > 0) { if ((ret = ::read(file, read_buf, read_num+1)) != read_num+1) { perror("i2c read"); -- 2.39.2