From 89582ffe1d48ea27ffd85d133ce5e758c4dc62c4 Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Sun, 8 Aug 2021 10:53:21 +0200
Subject: [PATCH] open_manipulator: increase torque to gripper

---
 include/open_manipulator_i2c.h | 28 +++++++++++++++++++++++++---
 1 file changed, 25 insertions(+), 3 deletions(-)

diff --git a/include/open_manipulator_i2c.h b/include/open_manipulator_i2c.h
index f46045c..4694242 100644
--- a/include/open_manipulator_i2c.h
+++ b/include/open_manipulator_i2c.h
@@ -15,6 +15,8 @@
 #define I2C_FILE "/dev/i2c-2"
 #define I2C_ADDR (0x60>>1)
 #define XM_OPERATING_MODE 11
+#define XM_CURRENT_LIMIT 38
+#define XM_SHUTDOWN 63
 #define XM_TORQUE_ENABLE 64
 #define XM_HARDWARE_ERROR_STATUS 70
 #define XM_GOAL_CURRENT 102
@@ -35,6 +37,7 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
 		WtOpenManipulatorI2C(ros::NodeHandle nh) {
 			uint8_t mode=255;
 			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);
 
@@ -65,7 +68,7 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
 			registerInterface(&jnt_pos_interface);
 
 			// Settings
-			if (dynamixel_writeword(15, XM_GOAL_CURRENT, 200) != 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);
 			}
@@ -100,19 +103,27 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
 			int32_t value;
 			int16_t current;
 			auto t1 = std::chrono::high_resolution_clock::now();
+			int num_error = 0;
 
 			// arm
 			for(int i=0, id=11; id<=14; id++, i++) {
+				if (ignore_ids.count(i)  > 0) {
+					ROS_ERROR_THROTTLE(10, "Ignoring dynamixel %d", id);
+					continue;
+				}
+
 				if (dynamixel_readdword(id, XM_PRESENT_POSITION, &value) == 1) {
 					pos[i] = STEPS_TO_RADIAN*(value-2048);
 				} else {
 					ROS_ERROR("I2C dynamixel id=%d position read error", id);
+					num_error++;
 				}
 
 				if (dynamixel_readdword(id, XM_PRESENT_VELOCITY, &value) == 1) {
 					vel[i] = value*VELOCITY_STEPS_TO_RADS;
 				} else {
 					ROS_ERROR("I2C dynamixel id=%d velocity read error", id);
+					num_error++;
 				}
 
 #ifdef READ_EFFORT
@@ -120,8 +131,13 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
 					eff[i] = current*2.69e-3;
 				} else {
 					ROS_ERROR("I2C dynamixel id=%d current read error", id);
+					num_error++;
 				}
 #endif
+
+				if (num_error >= 3) {
+					ignore_ids.insert(i);
+				}
 			}
 
 			// gripper
@@ -212,6 +228,7 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
 		double eff[5] = {0, 0, 0, 0, 0};
 		ros::ServiceServer torque_enable_service, get_error_service;
 		std::mutex comm_mutex;
+		std::set<int> ignore_ids;
 
 		int dynamixel_write(uint8_t *write_data, int write_num, uint8_t *read_data, int read_num) {
 			int file;
@@ -289,6 +306,9 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
 				*value = read_data[2];
 				return read_data[0];
 			}
+			if (read_data[0] != 1) {
+				ROS_ERROR("I2C dynamixel id=%d alert value: %d", id, read_data[0]);
+			}
 			return 255;
 		}
 
@@ -307,10 +327,13 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
 			uint8_t write_data[4] = {id, cmd, 0x14, CMD_EOF};
 			uint8_t read_data[6];
 			int ret = dynamixel_write(write_data, 4, read_data, 6);
-			if (ret == 6 && read_data[1] == id) {
+			if (ret == 6 && read_data[0] == 1 && read_data[1] == id) {
 				*value = (read_data[2]<<24) | (read_data[3]<<16) | (read_data[4]<<8) | read_data[5];
 				return read_data[0];
 			}
+			if (read_data[0] != 1) {
+				ROS_ERROR("I2C dynamixel id=%d alert value: %d", id, read_data[0]);
+			}
 			return 255;
 		}
 
@@ -328,7 +351,6 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
 				uint8_t value;
 				if (dynamixel_readbyte(id, XM_HARDWARE_ERROR_STATUS, &value) != 1) {
 					ROS_ERROR("I2C dynamixel id=%d reading hardware error status", id);
-					return false;
 				}
 				hw_error.id = id;
 				hw_error.inputVoltage = value & (1<<0);
-- 
2.39.2