#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_HOMING_OFFSET 20
+#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
#define XM_PROFILE_ACCELERATION 108
#define XM_PROFILE_VELOCITY 112
public:
WtOpenManipulatorI2C(ros::NodeHandle nh) {
uint8_t mode=255;
+ int32_t homing_offset=0;
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]);
registerInterface(&jnt_pos_interface);
// Settings
- if (dynamixel_writeword(15, XM_GOAL_CURRENT, 200) != 1) {
+ 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);
}
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
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
}
// gripper
- if (isnan(cmd[4]) || cmd[4] < -0.010 || cmd[4] > 0.010) {
+ if (isnan(cmd[4]) || cmd[4] < -0.010 || cmd[4] > 0.019) {
ROS_WARN("Desired angle for id=15 is NaN or outside joint limit, skipping write.");
} else {
value = cmd[4]/GRIPPER_RADIAN_TO_METER/STEPS_TO_RADIAN + 2048;
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;
+ std::set<int> ignore_ids;
int dynamixel_write(uint8_t *write_data, int write_num, uint8_t *read_data, int read_num) {
int file;
}
// 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");
*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;
}
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;
}
- 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);
+ }
+ 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;
+ }
};
projects / wt_open_manipulator.git / blobdiff