open_manipulator_i2c: offset on id 11

diff --git a/include/open_manipulator_i2c.h b/include/open_manipulator_i2c.h
index 4694242dc97dd13a2e7d58cb4e33c42ea93c4670..3d041c7f49fdfdcd2d5c7696aa1051252690cd22 100644 (file)
--- 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");