X-Git-Url: https://defiant.homedns.org/gitweb/?p=wt_open_manipulator.git;a=blobdiff_plain;f=include%2Fopen_manipulator_i2c.h;h=f46045c7af1cc68c67f8dd91d4be7ff6cdf5f49f;hp=da6d18ee911494cf8751589df2a94c32c9a8a440;hb=4c12857fc4730cb86439579dd2bb35261fd3a519;hpb=daa8e7ab9ac102a671c4cdd74f52004960f4fe45

diff --git a/include/open_manipulator_i2c.h b/include/open_manipulator_i2c.h
index da6d18e..f46045c 100644
--- a/include/open_manipulator_i2c.h
+++ b/include/open_manipulator_i2c.h
@@ -10,11 +10,13 @@
 #include <hardware_interface/joint_state_interface.h>
 #include <hardware_interface/robot_hw.h>
 #include "std_srvs/SetBool.h"
+#include "wt_open_manipulator/GetError.h"
 
 #define I2C_FILE "/dev/i2c-2"
 #define I2C_ADDR (0x60>>1)
 #define XM_OPERATING_MODE 11
 #define XM_TORQUE_ENABLE 64
+#define XM_HARDWARE_ERROR_STATUS 70
 #define XM_GOAL_CURRENT 102
 #define XM_PROFILE_ACCELERATION 108
 #define XM_PROFILE_VELOCITY 112
@@ -34,6 +36,7 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
 			uint8_t mode=255;
 			this->nh = nh;
 			torque_enable_service = nh.advertiseService("torque_enable", &WtOpenManipulatorI2C::cbTorqueEnable, this);
+			get_error_service = nh.advertiseService("get_error", &WtOpenManipulatorI2C::cbGetError, this);
 
 			// connect and register the joint state interface
 			hardware_interface::JointStateHandle state_handle_1("joint1", &pos[0], &vel[0], &eff[0]);
@@ -207,7 +210,7 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
 		double pos[5] = {0, 0, 0, 0, 0};
 		double vel[5] = {0, 0, 0, 0, 0};
 		double eff[5] = {0, 0, 0, 0, 0};
-		ros::ServiceServer torque_enable_service;
+		ros::ServiceServer torque_enable_service, get_error_service;
 		std::mutex comm_mutex;
 
 		int dynamixel_write(uint8_t *write_data, int write_num, uint8_t *read_data, int read_num) {
@@ -311,9 +314,31 @@ class WtOpenManipulatorI2C : public hardware_interface::RobotHW
 			return 255;
 		}
 
-		bool cbTorqueEnable(std_srvs::SetBool::Request  &req, std_srvs::SetBool::Response &res) {
+		bool cbTorqueEnable(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) {
 			ROS_INFO("Setting torque enable=%d", req.data);
 			res.success = torque_enable(req.data);
 			return true;
 		}
+
+		bool cbGetError(wt_open_manipulator::GetError::Request &req, wt_open_manipulator::GetError::Response &res) {
+			ROS_INFO("Reading Hardware Error Status");
+
+			for(int id=11, i=0; id<=15; id++, i++) {
+				wt_open_manipulator::HardwareError hw_error;
+				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);
+				hw_error.overHeating = value & (1<<2);
+				hw_error.motorEncoder = value & (1<<3);
+				hw_error.electricalShock = value & (1<<4);
+				hw_error.overload = value & (1<<5);
+				res.errors.push_back(hw_error);
+			}
+
+			return true;
+		}
 };