From 49418eb249f7dc5ae744216cfeb8232a5a091977 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=B6=99=E3=80=80=E6=BC=A0=E5=B1=85=28Zhao=2C=20Moju=29?= Date: Sun, 19 Jan 2025 20:30:26 +0900 Subject: [PATCH] [Spinal][Neuron][Servo] enable current control for dynamixel motor (#527) * [Neuron][Dynamixel] remove uncessary codes * [Neuron][Dynamixel] enable to get model number in init phase * [Neuron][Dynamixel][WIP] enable sending goal current (torque) in the init phase * [Neuron][Dynamixel][WIP] use sync write for goal position regardless of RS485 TTL mix mode * [Neuron][Dynamixel] enable polling the operating mode * [Neuron][Dynamixel] refactor the current assignment * [Neuron][Dynamixel] enable to send goal current every iteration after sending the goal position. Also enable receiving the goal current from CAN * [Spinal][Servo][WIP] enable send goal current from rosserial to spinal, and finally to neuron * [Spinal][Neuron] modify the CAN initialize message protocol to support sending the initial goal current from neuron to spinal * [Servo Bridge] enable to pub/sub target torque via servo bridge * [Servo Config] refactor Servo.yaml for all robots: removing unnecessary topic name assignment, use default one * [Spinal][RQT] fix the import bug for python3 * [Servo][Spinal][Neuron] refactor the force servo off rule, which is determined by neuron but not spinal * [Neuron][Dynamixel] add original rule to force switching off servo according to overload error * [Neuron][Dynamixel] implement setup of pre-defined configuration * [Spinal][RQT] fix the wrong list assignment * [Spinal][CAN] set 1[ms] internal between motor pwn and servo target command. --------- Co-authored-by: Moju Zhao --- .../include/aerial_robot_model/servo_bridge.h | 36 +++- .../src/servo_bridge/servo_bridge.cpp | 148 +++++++++++--- .../neuron/neuronlib/CAN/can_constants.h | 1 + .../neuronlib/Initializer/initializer.cpp | 17 +- .../Servo/Dynamixel/dynamixel_serial.cpp | 174 +++++++++++++++-- .../Servo/Dynamixel/dynamixel_serial.h | 184 +++++++++++------- .../neuron/neuronlib/Servo/servo.cpp | 9 + .../CANDevice/initializer/can_initializer.cpp | 9 +- .../Hydrus_Lib/CANDevice/servo/can_servo.cpp | 7 + .../Hydrus_Lib/CANDevice/servo/can_servo.h | 4 +- .../lib/Hydrus_Lib/Spine/spine.cpp | 26 ++- .../mcu_project/lib/Hydrus_Lib/Spine/spine.h | 3 +- .../lib/Jsk_Lib/CAN/can_constants.h | 1 + robots/dragon/config/quad/Servo.yaml | 5 +- .../quad/default_mode_201907/Servo.yaml | 3 - .../old_model_tx2_rs_t265_201906/Servo.yaml | 3 - .../quad/old_model_tx2_zed_201810/Servo.yaml | 3 - .../quad/tilt_10deg_15inch_202106/Servo.yaml | 3 - .../config/quad/tilt_10deg_201907/Servo.yaml | 3 - .../config/quad/tilt_20deg_201907/Servo.yaml | 3 - robots/hydrus_xi/config/hex/Servo.yaml | 3 - robots/hydrus_xi/config/hex_branch/Servo.yaml | 3 - robots/hydrus_xi/config/quad/Servo.yaml | 3 - 23 files changed, 487 insertions(+), 164 deletions(-) diff --git a/aerial_robot_model/include/aerial_robot_model/servo_bridge.h b/aerial_robot_model/include/aerial_robot_model/servo_bridge.h index bf94aa6c9..4e68d6506 100644 --- a/aerial_robot_model/include/aerial_robot_model/servo_bridge.h +++ b/aerial_robot_model/include/aerial_robot_model/servo_bridge.h @@ -125,9 +125,15 @@ class SingleServoHandle inline void setCurrTorqueVal(const double& val) { - curr_torque_val_ = torque_scale_ * angle_sgn_ * val; + curr_torque_val_ = torque_scale_ * angle_sgn_ * val; } + inline void setTargetTorqueVal(const double& val) + { + target_torque_val_ = val; + } + + inline void setName(const string& name){ name_ = name; } inline void setId(const int& id){ id_ = id; } inline void setAngleSgn(const int& sgn){ angle_sgn_ = sgn; } @@ -164,6 +170,19 @@ class SingleServoHandle return curr_torque_val_; } + inline const double getTargetTorqueVal(int value_type) const + { + if(value_type == ValueType::BIT) + { + return target_torque_val_ * angle_sgn_ / torque_scale_; + } + else // Nm + { + return target_torque_val_; + } + } + + inline const string& getName(){return name_; } inline const int& getId() const {return id_; } inline const int& getAngleSgn() const {return angle_sgn_; } @@ -177,6 +196,7 @@ class SingleServoHandle double curr_angle_val_; // radian double target_angle_val_; // radian double curr_torque_val_; // Nm + double target_torque_val_; // Nm int angle_sgn_; int zero_point_offset_; double angle_scale_; @@ -206,11 +226,14 @@ class ServoBridge ros::Subscriber uav_info_sub_; map servo_states_subs_; map servo_ctrl_subs_; + map servo_torque_ctrl_subs_; + map servo_torque_subs_; map no_real_state_flags_; - map servo_ctrl_pubs_; - map servo_torque_ctrl_srvs_; - map servo_torque_ctrl_pubs_; - map > servo_ctrl_sim_pubs_; // TODO: should be actionlib, trajectory controller + map servo_target_pos_pubs_; + map servo_target_torque_pubs_; + map servo_enable_srvs_; + map servo_enable_pubs_; + map > servo_target_pos_sim_pubs_; // TODO: should be actionlib, trajectory controller map servos_handler_; double moving_check_rate_; @@ -222,7 +245,8 @@ class ServoBridge void servoStatesCallback(const spinal::ServoStatesConstPtr& state_msg, const std::string& servo_group_name); void servoCtrlCallback(const sensor_msgs::JointStateConstPtr& joints_ctrl_msg, const std::string& servo_group_name); - bool servoTorqueCtrlCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res, const std::string& servo_group_name); + void servoTorqueCtrlCallback(const sensor_msgs::JointStateConstPtr& joints_ctrl_msg, const std::string& servo_group_name); + bool servoEnableCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res, const std::string& servo_group_name); void uavInfoCallback(const spinal::UavInfoConstPtr& uav_msg); public: diff --git a/aerial_robot_model/src/servo_bridge/servo_bridge.cpp b/aerial_robot_model/src/servo_bridge/servo_bridge.cpp index bd3545834..4a947d173 100644 --- a/aerial_robot_model/src/servo_bridge/servo_bridge.cpp +++ b/aerial_robot_model/src/servo_bridge/servo_bridge.cpp @@ -62,17 +62,20 @@ ServoBridge::ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh),nhp_( /* common param and topics (between servo_bridge and spinal_ros_bridge) */ ros::NodeHandle nh_controller(nh_, "servo_controller"); - std::string state_sub_topic, ctrl_pub_topic, torque_pub_topic; + std::string state_sub_topic, pos_pub_topic, torque_pub_topic, servo_enable_pub_topic; nh_controller.param("state_sub_topic", state_sub_topic, std::string("servo/states")); - nh_controller.param("ctrl_pub_topic", ctrl_pub_topic, std::string("servo/target_states")); - nh_controller.param("torque_pub_topic", torque_pub_topic, std::string("servo/torque_enable")); + nh_controller.param("pos_pub_topic", pos_pub_topic, std::string("servo/target_states")); + nh_controller.param("torque_pub_topic", torque_pub_topic, std::string("servo/target_current")); + nh_controller.param("servo_enable_pub_topic", servo_enable_pub_topic, std::string("servo/torque_enable")); /* common subsriber: get servo states (i.e. joint angles) from real machine (spinal_ros_bridge) */ servo_states_subs_.insert(make_pair("common", nh_.subscribe(state_sub_topic, 10, boost::bind(&ServoBridge::servoStatesCallback, this, _1, "common")))); - /* common publisher: target servo state to real machine (spinal_ros_bridge) */ - servo_ctrl_pubs_.insert(make_pair("common", nh_.advertise(ctrl_pub_topic, 1))); + /* common publisher: target servo position to real machine (spinal_ros_bridge) */ + servo_target_pos_pubs_.insert(make_pair("common", nh_.advertise(pos_pub_topic, 1))); mujoco_control_input_pub_ = nh_.advertise("mujoco/ctrl_input", 1); - /* common publisher: torque on/off command */ - servo_torque_ctrl_pubs_.insert(make_pair("common", nh_.advertise(torque_pub_topic, 1))); + /* common publisher: target servo torque to real machine (spinal_ros_bridge) */ + servo_target_torque_pubs_.insert(make_pair("common", nh_.advertise(torque_pub_topic, 1))); + /* common publisher: servo on/off flag to real machine (spinal_ros_bridge) */ + servo_enable_pubs_.insert(make_pair("common", nh_.advertise(servo_enable_pub_topic, 1))); /* common publisher: joint profiles */ joint_profile_pub_ = nh_.advertise("joint_profiles",1); /* subscriber: uav info */ @@ -96,10 +99,12 @@ ServoBridge::ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh),nhp_( no_real_state_flags_.at(group_name) = servo_group_params.second["no_real_state"]; /* pub, sub, and srv */ - /* mandatory subscriber: target servo state from controller */ + /* mandatory subscriber: target servo command (e.g., position, torque) from controller */ servo_ctrl_subs_.insert(make_pair(group_name, nh_.subscribe(group_name + string("_ctrl"), 10, boost::bind(&ServoBridge::servoCtrlCallback, this, _1, group_name)))); + /* mandatory subscriber: only target servo troque command from controller */ + servo_torque_ctrl_subs_.insert(make_pair(group_name, nh_.subscribe(group_name + string("_torque_ctrl"), 10, boost::bind(&ServoBridge::servoTorqueCtrlCallback, this, _1, group_name)))); /* mandatory service: get torque enalbe/disable flag */ - servo_torque_ctrl_srvs_.insert(make_pair(group_name, nh_.advertiseService(servo_group_params.first + string("/torque_enable"), boost::bind(&ServoBridge::servoTorqueCtrlCallback, this, _1, _2, servo_group_params.first)))); + servo_enable_srvs_.insert(make_pair(group_name, nh_.advertiseService(servo_group_params.first + string("/torque_enable"), boost::bind(&ServoBridge::servoEnableCallback, this, _1, _2, servo_group_params.first)))); if(servo_group_params.second.hasMember("state_sub_topic")) { @@ -107,16 +112,22 @@ ServoBridge::ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh),nhp_( servo_states_subs_.insert(make_pair(group_name, nh_.subscribe((string)servo_group_params.second["state_sub_topic"], 10, boost::bind(&ServoBridge::servoStatesCallback, this, _1, servo_group_params.first)))); } - if(servo_group_params.second.hasMember("ctrl_pub_topic")) + if(servo_group_params.second.hasMember("pos_pub_topic")) { /* option: publish target servo state to real machine (spinal_ros_bridge) */ - servo_ctrl_pubs_.insert(make_pair(group_name, nh_.advertise(servo_group_params.second["ctrl_pub_topic"], 1))); + servo_target_pos_pubs_.insert(make_pair(group_name, nh_.advertise(servo_group_params.second["pos_pub_topic"], 1))); } if(servo_group_params.second.hasMember("torque_pub_topic")) { /* option: torque on/off */ - servo_torque_ctrl_pubs_.insert(make_pair(group_name, nh_.advertise((string)servo_group_params.second["torque_pub_topic"], 1))); + servo_target_torque_pubs_.insert(make_pair(group_name, nh_.advertise(servo_group_params.second["torque_pub_topic"], 1))); + } + + if(servo_group_params.second.hasMember("servo_enable_pub_topic")) + { + /* option: torque on/off */ + servo_enable_pubs_.insert(make_pair(group_name, nh_.advertise((string)servo_group_params.second["servo_enable_pub_topic"], 1))); } /* servo handler */ @@ -202,15 +213,15 @@ ServoBridge::ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp): nh_(nh),nhp_( ROS_ERROR("Failed to call service %s", controller_starter.getService().c_str()); /* init the servo command publisher to the controller */ - servo_ctrl_sim_pubs_[servo_group_params.first].push_back(nh_.advertise(load_srv.request.name + string("/command"), 1)); + servo_target_pos_sim_pubs_[servo_group_params.first].push_back(nh_.advertise(load_srv.request.name + string("/command"), 1)); // wait for the publisher initialization - while(servo_ctrl_sim_pubs_[servo_group_params.first].back().getNumSubscribers() == 0 && ros::ok()) + while(servo_target_pos_sim_pubs_[servo_group_params.first].back().getNumSubscribers() == 0 && ros::ok()) ros::Duration(0.1).sleep(); /* set the initial angle */ std_msgs::Float64 msg; nh_.getParam(string("servo_controller/") + servo_group_params.first + string("/") + servo_params.first + string("/simulation/init_value"), msg.data); - servo_ctrl_sim_pubs_[servo_group_params.first].back().publish(msg); + servo_target_pos_sim_pubs_[servo_group_params.first].back().publish(msg); } } } @@ -303,6 +314,7 @@ void ServoBridge::servoStatesCallback(const spinal::ServoStatesConstPtr& state_m void ServoBridge::servoCtrlCallback(const sensor_msgs::JointStateConstPtr& servo_ctrl_msg, const string& servo_group_name) { spinal::ServoControlCmd target_angle_msg; + spinal::ServoControlCmd target_torque_msg; sensor_msgs::JointState mujoco_control_input_msg; if(servo_ctrl_msg->name.size() > 0) @@ -333,11 +345,20 @@ void ServoBridge::servoCtrlCallback(const sensor_msgs::JointStateConstPtr& servo target_angle_msg.index.push_back((*servo_handler)->getId()); target_angle_msg.angles.push_back((*servo_handler)->getTargetAngleVal(ValueType::BIT)); + // process torque command if necessary + if(servo_ctrl_msg->effort.size() == servo_ctrl_msg->name.size()) + { + double torque = servo_ctrl_msg->effort.at(i); + (*servo_handler)->setTargetTorqueVal(torque); + target_torque_msg.index.push_back((*servo_handler)->getId()); + target_torque_msg.angles.push_back((*servo_handler)->getTargetTorqueVal(ValueType::BIT)); + } + if(simulation_mode_) { std_msgs::Float64 msg; msg.data = servo_ctrl_msg->position[i]; - servo_ctrl_sim_pubs_[servo_group_name].at(distance(servos_handler_[servo_group_name].begin(), servo_handler)).publish(msg); + servo_target_pos_sim_pubs_[servo_group_name].at(distance(servos_handler_[servo_group_name].begin(), servo_handler)).publish(msg); } } } @@ -346,7 +367,7 @@ void ServoBridge::servoCtrlCallback(const sensor_msgs::JointStateConstPtr& servo if(servo_ctrl_msg->position.size() != servos_handler_[servo_group_name].size()) { - ROS_ERROR("[servo bridge, servo control control]: the joint num from rosparam %d is not equal with ros msgs %d", + ROS_ERROR("[servo bridge, servo ctrl control]: the joint num from rosparam %d is not equal with ros msgs %d", (int)servos_handler_[servo_group_name].size(), (int)servo_ctrl_msg->position.size()); return; } @@ -359,6 +380,15 @@ void ServoBridge::servoCtrlCallback(const sensor_msgs::JointStateConstPtr& servo target_angle_msg.index.push_back(servo_handler->getId()); target_angle_msg.angles.push_back(servo_handler->getTargetAngleVal(ValueType::BIT)); + // process torque command if necessary + if(servo_ctrl_msg->effort.size() == servo_ctrl_msg->position.size()) + { + double torque = servo_ctrl_msg->effort.at(i); + servo_handler->setTargetTorqueVal(torque); + target_torque_msg.index.push_back(servo_handler->getId()); + target_torque_msg.angles.push_back(servo_handler->getTargetTorqueVal(ValueType::BIT)); + } + mujoco_control_input_msg.name.push_back(servo_handler->getName()); mujoco_control_input_msg.position.push_back(servo_ctrl_msg->position.at(i)); @@ -366,20 +396,88 @@ void ServoBridge::servoCtrlCallback(const sensor_msgs::JointStateConstPtr& servo { std_msgs::Float64 msg; msg.data = servo_ctrl_msg->position[i]; - servo_ctrl_sim_pubs_[servo_group_name].at(i).publish(msg); + servo_target_pos_sim_pubs_[servo_group_name].at(i).publish(msg); } } } mujoco_control_input_pub_.publish(mujoco_control_input_msg); - if (servo_ctrl_pubs_.find(servo_group_name) != servo_ctrl_pubs_.end()) - servo_ctrl_pubs_[servo_group_name].publish(target_angle_msg); + if (servo_target_pos_pubs_.find(servo_group_name) != servo_target_pos_pubs_.end()) + servo_target_pos_pubs_[servo_group_name].publish(target_angle_msg); + else + servo_target_pos_pubs_["common"].publish(target_angle_msg); + + + // process torque command if necessary + if (target_torque_msg.index.size() == 0) return; + if (servo_target_torque_pubs_.find(servo_group_name) != servo_target_torque_pubs_.end()) + servo_target_torque_pubs_[servo_group_name].publish(target_torque_msg); + else + servo_target_torque_pubs_["common"].publish(target_torque_msg); +} + +void ServoBridge::servoTorqueCtrlCallback(const sensor_msgs::JointStateConstPtr& servo_ctrl_msg, const string& servo_group_name) +{ + spinal::ServoControlCmd target_torque_msg; + + if(servo_ctrl_msg->name.size() > 0) + { + for(int i = 0; i < servo_ctrl_msg->name.size(); i++) + {/* servo name is assigned */ + + if(servo_ctrl_msg->effort.size() != servo_ctrl_msg->name.size()) + { + ROS_ERROR("[servo bridge, servo torque control]: the servo effort (torque) num and name num are different in ros msgs [%d vs %d]", + (int)servo_ctrl_msg->effort.size(), (int)servo_ctrl_msg->name.size()); + return; + } + + // use servo_name to search the servo_handler + auto servo_handler = find_if(servos_handler_[servo_group_name].begin(), servos_handler_[servo_group_name].end(), + [&](SingleServoHandlePtr s) {return servo_ctrl_msg->name.at(i) == s->getName();}); + + if(servo_handler == servos_handler_[servo_group_name].end()) + { + ROS_ERROR("[servo bridge, servo control callback]: no matching servo handler for %s", servo_ctrl_msg->name.at(i).c_str()); + return; + } + + double torque = servo_ctrl_msg->effort.at(i); + (*servo_handler)->setTargetTorqueVal(torque); + target_torque_msg.index.push_back((*servo_handler)->getId()); + target_torque_msg.angles.push_back((*servo_handler)->getTargetTorqueVal(ValueType::BIT)); + } + } + else + { /* for fast tranmission: no searching process, in the predefine order */ + + if(servo_ctrl_msg->position.size() != servos_handler_[servo_group_name].size()) + { + ROS_ERROR("[servo bridge, servo control control]: the joint num from rosparam %d is not equal with ros msgs %d", + (int)servos_handler_[servo_group_name].size(), (int)servo_ctrl_msg->position.size()); + return; + } + + for(int i = 0; i < servo_ctrl_msg->position.size(); i++) + { + /* use the kinematics order (e.g. joint1 ~ joint N, gimbal_roll -> gimbal_pitch) */ + SingleServoHandlePtr servo_handler = servos_handler_[servo_group_name].at(i); + double torque = servo_ctrl_msg->effort.at(i); + + servo_handler->setTargetTorqueVal(torque); + target_torque_msg.index.push_back(servo_handler->getId()); + target_torque_msg.angles.push_back(servo_handler->getTargetTorqueVal(ValueType::BIT)); + } + } + + if (servo_target_torque_pubs_.find(servo_group_name) != servo_target_torque_pubs_.end()) + servo_target_torque_pubs_[servo_group_name].publish(target_torque_msg); else - servo_ctrl_pubs_["common"].publish(target_angle_msg); + servo_target_torque_pubs_["common"].publish(target_torque_msg); } -bool ServoBridge::servoTorqueCtrlCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res, const std::string& servo_group_name) +bool ServoBridge::servoEnableCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res, const std::string& servo_group_name) { spinal::ServoTorqueCmd torque_msg; for(auto servo_handler: servos_handler_[servo_group_name]) @@ -388,10 +486,10 @@ bool ServoBridge::servoTorqueCtrlCallback(std_srvs::SetBool::Request &req, std_s torque_msg.torque_enable.push_back(req.data); } - if (servo_torque_ctrl_pubs_.find(servo_group_name) != servo_torque_ctrl_pubs_.end()) - servo_torque_ctrl_pubs_[servo_group_name].publish(torque_msg); + if (servo_enable_pubs_.find(servo_group_name) != servo_enable_pubs_.end()) + servo_enable_pubs_[servo_group_name].publish(torque_msg); else - servo_torque_ctrl_pubs_["common"].publish(torque_msg); + servo_enable_pubs_["common"].publish(torque_msg); return true; } diff --git a/aerial_robot_nerve/neuron/neuronlib/CAN/can_constants.h b/aerial_robot_nerve/neuron/neuronlib/CAN/can_constants.h index 5b4733625..9175a4ca8 100644 --- a/aerial_robot_nerve/neuron/neuronlib/CAN/can_constants.h +++ b/aerial_robot_nerve/neuron/neuronlib/CAN/can_constants.h @@ -30,6 +30,7 @@ namespace CAN { constexpr uint8_t MESSAGEID_RECEIVE_PWM_0_5 = 0; constexpr uint8_t MESSAGEID_RECEIVE_PWM_6_11 = 1; constexpr uint8_t MESSAGEID_RECEIVE_SERVO_ANGLE = 0; + constexpr uint8_t MESSAGEID_RECEIVE_SERVO_CURRENT = 1; constexpr uint8_t MESSAGEID_RECEIVE_SERVO_CONFIG = 15; constexpr uint8_t MESSAGEID_SEND_SERVO_LIST[4] = {0, 1, 2, 3}; constexpr uint8_t MESSAGEID_RECEIVE_ENUM_REQUEST = 0; diff --git a/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp b/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp index 47d7549c5..8c86b611b 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp +++ b/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp @@ -39,15 +39,16 @@ void Initializer::sendBoardConfig() data[5] = s.current_limit_ & 0xFF; data[6] = (s.current_limit_ >> 8) & 0xFF; data[7] = (s.send_data_flag_ ? 1 : 0); + data[7] += ((s.external_encoder_flag_ ? 1 : 0) << 1); sendMessage(CAN::MESSAGEID_SEND_INITIAL_CONFIG_2, m_slave_id, 8, data, 1); - data[0] = i; - data[1] = s.hardware_error_status_; - data[2] = (s.external_encoder_flag_ ? 1 : 0); - data[3] = s.joint_resolution_ & 0xFF; - data[4] = (s.joint_resolution_ >> 8) & 0xFF; - data[5] = s.servo_resolution_ & 0xFF; - data[6] = (s.servo_resolution_ >> 8) & 0xFF; - data[7] = 0; + data[0] = i; + data[1] = s.hardware_error_status_; + data[2] = s.goal_current_ & 0xFF; + data[3] = (s.goal_current_ >> 8) & 0xFF; + data[4] = s.joint_resolution_ & 0xFF; + data[5] = (s.joint_resolution_ >> 8) & 0xFF; + data[6] = s.servo_resolution_ & 0xFF; + data[7] = (s.servo_resolution_ >> 8) & 0xFF; sendMessage(CAN::MESSAGEID_SEND_INITIAL_CONFIG_3, m_slave_id, 8, data, 1); } } diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp b/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp index 4839fee9c..3553ebc62 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp @@ -6,7 +6,7 @@ void DynamixelSerial::init(UART_HandleTypeDef* huart, I2C_HandleTypeDef* hi2c, o huart_ = huart; mutex_ = mutex; servo_num_ = 0; - set_pos_tick_ = 0; + set_command_tick_ = 0; get_pos_tick_ = 0; get_load_tick_ = 0; get_temp_tick_ = 0; @@ -45,6 +45,11 @@ void DynamixelSerial::init(UART_HandleTypeDef* huart, I2C_HandleTypeDef* hi2c, o } cmdSyncWriteLed(); + // pre-define configuration + cmdSyncWriteShutdownBit(); + cmdSyncWriteReturnDelayTime(); + cmdSyncWriteTemperatureLimit(); + for (int i = 0; i < MAX_SERVO_NUM; i++) { Flashmemory::addValue(&(servo_[i].p_gain_), 2); Flashmemory::addValue(&(servo_[i].i_gain_), 2); @@ -68,6 +73,8 @@ void DynamixelSerial::init(UART_HandleTypeDef* huart, I2C_HandleTypeDef* hi2c, o getCurrentLimit(); getPositionGains(); getProfileVelocity(); + getModelNumber(); + getOperatingMode(); //initialize encoder: only can support one encoder @@ -85,6 +92,16 @@ void DynamixelSerial::init(UART_HandleTypeDef* huart, I2C_HandleTypeDef* hi2c, o if(servo_[i].external_encoder_flag_) encoder_handler_.setOffset(servo_[i].joint_offset_); } } + + // set initial goal current if necessary + for (unsigned int i = 0; i < servo_num_; i++) { + uint8_t operating_mode = servo_[i].operating_mode_; + uint16_t current_limit = servo_[i].current_limit_; + if (operating_mode == CURRENT_BASE_POSITION_CONTROL_MODE) { + servo_[i].goal_current_ = current_limit * 0.8; // workaround: set 80% of the overload threshold + cmdWriteGoalCurrent(i); + } + } } void DynamixelSerial::ping() @@ -189,17 +206,11 @@ void DynamixelSerial::update() uint32_t current_time = HAL_GetTick(); /* send position command to servo */ - if(current_time >= set_pos_tick_ + SET_POS_DU && SET_POS_DU > 0) { - if (set_pos_tick_ == 0) set_pos_tick_ = current_time + SET_POS_OFFSET; // init - else set_pos_tick_ = current_time; + if(current_time >= set_command_tick_ + SET_COMMAND_DU && SET_COMMAND_DU > 0) { + if (set_command_tick_ == 0) set_command_tick_ = current_time + SET_COMMAND_OFFSET; // init + else set_command_tick_ = current_time; - if (ttl_rs485_mixed_ != 0) { - for (unsigned int i = 0; i < servo_num_; ++i) { - instruction_buffer_.push(std::make_pair(INST_SET_GOAL_POS, i)); - } - } else { - instruction_buffer_.push(std::make_pair(INST_SET_GOAL_POS, 0)); - } + instruction_buffer_.push(std::make_pair(INST_SET_GOAL_COMMAND, 0)); // broadcast both target pos and current } /* read servo position(angle) */ @@ -217,7 +228,6 @@ void DynamixelSerial::update() } /* read servo load */ - if(current_time >= get_load_tick_ + GET_LOAD_DU && GET_LOAD_DU > 0) { if (get_load_tick_ == 0) get_load_tick_ = current_time + GET_LOAD_OFFSET; // init else get_load_tick_ = current_time; @@ -232,7 +242,6 @@ void DynamixelSerial::update() } /* read servo temperature */ - if(current_time >= get_temp_tick_ + GET_TEMP_DU && GET_TEMP_DU > 0) { if (get_temp_tick_ == 0) get_temp_tick_ = current_time + GET_TEMP_OFFSET; // init else get_temp_tick_ = current_time; @@ -273,9 +282,20 @@ void DynamixelSerial::update() instruction_buffer_.push(std::make_pair(INST_GET_HARDWARE_ERROR_STATUS, 0)); } - // check the latest error status + // check the latest error status with original rule for (unsigned int i = 0; i < servo_num_; i++) { if (servo_[i].hardware_error_status_ != 0) { + + // ingnore overload error for current based position control + if (servo_[i].hardware_error_status_ == (1 << OVERLOAD_ERROR)) { + if (servo_[i].operating_mode_ == CURRENT_BASE_POSITION_CONTROL_MODE) { + if (servo_[i].goal_current_ < servo_[i].current_limit_) { + servo_[i].force_servo_off_ = false; + continue; + } + } + } + servo_[i].force_servo_off_ = true; if (servo_[i].torque_enable_) { @@ -301,8 +321,9 @@ void DynamixelSerial::update() /* set command */ switch (instruction.first) { - case INST_SET_GOAL_POS: /* send angle command to servo */ + case INST_SET_GOAL_COMMAND: /* send angle and current command to servo */ cmdSyncWriteGoalPosition(); + cmdSyncWriteGoalCurrent(); break; case INST_SET_TORQUE: /* send torque enable flag */ cmdWriteTorqueEnable(servo_index); @@ -562,7 +583,7 @@ int8_t DynamixelSerial::readStatusPacket(uint8_t status_packet_instruction) } break; case READ_ERROR: - if ((rx_data & 0x7F) == ERROR_NO_ERROR) { + if ((rx_data & 0x7F) == NO_ERROR) { status_stage++; } else { return -1; @@ -677,6 +698,16 @@ int8_t DynamixelSerial::readStatusPacket(uint8_t status_packet_instruction) s->moving_ = parameters[0]; } return 0; + case INST_GET_MODEL_NUMBER: + if (s != servo_.end()) { + s->model_number_ = ((parameters[1] << 8) & 0xFF00) | (parameters[0] & 0xFF); + } + return 0; + case INST_GET_OPERATING_MODE: + if (s != servo_.end()) { + s->operating_mode_ = parameters[0]; + } + return 0; case INST_GET_HOMING_OFFSET: if (s != servo_.end()) { s->homing_offset_ = ((parameters[3] << 24) & 0xFF000000) | ((parameters[2] << 16) & 0xFF0000) | ((parameters[1] << 8) & 0xFF00) | (parameters[0] & 0xFF); @@ -794,6 +825,16 @@ void DynamixelSerial::cmdReadMoving(uint8_t servo_index) cmdRead(servo_[servo_index].id_, CTRL_MOVING, MOVING_BYTE_LEN); } +void DynamixelSerial::cmdReadModelNumber(uint8_t servo_index) +{ + cmdRead(servo_[servo_index].id_, CTRL_MODEL_NUMBER, MODEL_NUMBER_BYTE_LEN); +} + +void DynamixelSerial::cmdReadOperatingMode(uint8_t servo_index) +{ + cmdRead(servo_[servo_index].id_, CTRL_OPERATING_MODE, OPERATING_MODE_BYTE_LEN); +} + void DynamixelSerial::cmdReadPositionGains(uint8_t servo_index) { cmdRead(servo_[servo_index].id_, CTRL_POSITION_D_GAIN, POSITION_GAINS_BYTE_LEN); @@ -872,12 +913,19 @@ void DynamixelSerial::cmdWriteStatusReturnLevel(uint8_t id, uint8_t set) void DynamixelSerial::cmdWriteTorqueEnable(uint8_t servo_index) { - uint8_t parameter = (uint8_t)((servo_[servo_index].torque_enable_) ? 1 : 0); - cmdWrite(servo_[servo_index].id_, CTRL_TORQUE_ENABLE, ¶meter, TORQUE_ENABLE_BYTE_LEN); } +void DynamixelSerial::cmdWriteGoalCurrent(uint8_t servo_index) +{ + int16_t cur = servo_[servo_index].goal_current_; + uint8_t parameters[GOAL_CURRENT_BYTE_LEN]; + parameters[0] = (uint8_t)(cur & 0xFF); + parameters[1] = (uint8_t)((cur >> 8) & 0xFF); + cmdWrite(servo_[servo_index].id_, CTRL_GOAL_CURRENT, parameters, GOAL_CURRENT_BYTE_LEN); +} + void DynamixelSerial::cmdSyncReadCurrentLimit(bool send_all) { cmdSyncRead(CTRL_CURRENT_LIMIT, CURRENT_LIMIT_BYTE_LEN, send_all); @@ -898,6 +946,16 @@ void DynamixelSerial::cmdSyncReadMoving(bool send_all) cmdSyncRead(CTRL_MOVING, MOVING_BYTE_LEN, send_all); } +void DynamixelSerial::cmdSyncReadModelNumber(bool send_all) +{ + cmdSyncRead(CTRL_MODEL_NUMBER, MODEL_NUMBER_BYTE_LEN, send_all); +} + +void DynamixelSerial::cmdSyncReadOperatingMode(bool send_all) +{ + cmdSyncRead(CTRL_OPERATING_MODE, OPERATING_MODE_BYTE_LEN, send_all); +} + void DynamixelSerial::cmdSyncReadPositionGains(bool send_all) { cmdSyncRead(CTRL_POSITION_D_GAIN, POSITION_GAINS_BYTE_LEN, send_all); @@ -938,6 +996,24 @@ void DynamixelSerial::cmdSyncWriteGoalPosition() cmdSyncWrite(CTRL_GOAL_POSITION, parameters, GOAL_POSITION_BYTE_LEN); } +void DynamixelSerial::cmdSyncWriteGoalCurrent() +{ + uint8_t parameters[INSTRUCTION_PACKET_SIZE]; + + for (unsigned int i = 0; i < servo_num_; i++) { + // uint8_t operating_mode = servo_[i].operating_mode; + // if (operating_mode != EXTENDED_POSITION_CONTROL_MODE && + // operating_mode != CURRENT_BASE_POSITION_CONTROL_MODE) { + // continue + // } + int16_t goal_current = servo_[i].goal_current_; + parameters[i * 2 + 0] = (uint8_t)(goal_current & 0xFF); + parameters[i * 2 + 1] = (uint8_t)((goal_current >> 8) & 0xFF); + } + + cmdSyncWrite(CTRL_GOAL_CURRENT, parameters, GOAL_CURRENT_BYTE_LEN); +} + void DynamixelSerial::cmdSyncWriteLed() { uint8_t parameters[INSTRUCTION_PACKET_SIZE]; @@ -988,12 +1064,74 @@ void DynamixelSerial::cmdSyncWriteTorqueEnable() cmdSyncWrite(CTRL_TORQUE_ENABLE, parameters, TORQUE_ENABLE_BYTE_LEN); } +void DynamixelSerial::cmdSyncWriteShutdownBit() +{ + uint8_t parameters[INSTRUCTION_PACKET_SIZE]; + + for (unsigned int i = 0; i < servo_num_; i++) { + parameters[i] = SHUTDOWN_BIT; + } + + cmdSyncWrite(CTRL_SHUTDOWN, parameters, SHUTDOWN_BYTE_LEN); +} + +void DynamixelSerial::cmdSyncWriteReturnDelayTime() +{ + uint8_t parameters[INSTRUCTION_PACKET_SIZE]; + + for (unsigned int i = 0; i < servo_num_; i++) { + parameters[i] = RETURN_DELAY_TIME; + } + + cmdSyncWrite(CTRL_RETURN_DELAY_TIME, parameters, RETURN_DELAY_TIME_BYTE_LEN); +} + +void DynamixelSerial::cmdSyncWriteTemperatureLimit() +{ + uint8_t parameters[INSTRUCTION_PACKET_SIZE]; + + for (unsigned int i = 0; i < servo_num_; i++) { + parameters[i] = TEMPERATURE_LIMIT; + } + + cmdSyncWrite(CTRL_TEMPERATURE_LIMIT, parameters, TEMPERATURE_LIMIT_BYTE_LEN); +} void DynamixelSerial::setStatusReturnLevel() { cmdWriteStatusReturnLevel(BROADCAST_ID, READ); } +void DynamixelSerial::getModelNumber() +{ + if (ttl_rs485_mixed_ != 0) { + for (unsigned int i = 0; i < servo_num_; ++i) { + cmdReadModelNumber(i); + readStatusPacket(INST_GET_MODEL_NUMBER); + } + } else { + cmdSyncReadModelNumber(); + for (unsigned int i = 0; i < servo_num_; i++) { + readStatusPacket(INST_GET_MODEL_NUMBER); + } + } +} + +void DynamixelSerial::getOperatingMode() +{ + if (ttl_rs485_mixed_ != 0) { + for (unsigned int i = 0; i < servo_num_; ++i) { + cmdReadOperatingMode(i); + readStatusPacket(INST_GET_OPERATING_MODE); + } + } else { + cmdSyncReadOperatingMode(); + for (unsigned int i = 0; i < servo_num_; i++) { + readStatusPacket(INST_GET_OPERATING_MODE); + } + } +} + void DynamixelSerial::getHomingOffset() { if (ttl_rs485_mixed_ != 0) { diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.h b/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.h index 87a7f77d7..495b2df17 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.h +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.h @@ -19,29 +19,50 @@ #include "Encoder/mag_encoder.h" #include "cmsis_os.h" -/* first should set the baudrate to 1000000*/ -/* uncomment following macro, and set the uart baudrate to 57143(M - * X28), then the buadrate will change */ -/* then comment following macro, change the baudrate back to 1000000 */ - -/* second, should set some setting to the servo */ -/* 1. the responce delay: which is very important for MX28AR, - * uncomment the INIT_SETTING marcro, and comment again after the setting - * we observed that the RX of uart will turn to 0, after the tx process - * therefore, we set the delay of rx after rx to be 0 => no 0V after rx - * 2. the voltage: we set the voltage limit according to the battery(4s: 17V) - */ - -//#define SET_BAUD -//#define SET_ID -//#define PING_TEST -//#define LED_TEST -//#define INIT_SETTING -//#define SERVO_TEST -//#define WHEEL_TEST -//#define SET_HOMING_OFFSET //######################################################################### + +//################ define - Dynamixel Model table ############################ +#define XC330_M077 0x4A6 +#define XC330_M288 0x4B0 +#define XC330_M181 0x4B0 +#define XC330_T181 0x4BA +#define XC330_T288 0x4C4 +#define XL430_W250 0x4CE +#define DXL430_W250 0x442 // 2XL430_W250 +#define XC430_W150 0x42E +#define XC430_W250 0x438 +#define DXC430_W250 0x488 // 2XC430_W250 +#define XM430_W210 0x406 +#define XH430_W210 0x3F2 +#define XH430_V210 0x41A +#define XD430_T210 0x3F3 +#define XM430_W350 0x3FC +#define XH430_W350 0x3E8 +#define XH430_V350 0x410 +#define XD430_T350 0x3E9 +#define XW430_T200 0x500 +#define XW430_T333 0x4F6 +#define XM540_W150 0x46A +#define XH540_W150 0x456 +#define XH540_V150 0x47E +#define XM540_W270 0x460 +#define XH540_W270 0x44C +#define XH540_V270 0x474 +#define XW540_T140 0x49C +#define XW540_T260 0x492 +#define MX_28 0x1E +#define MX_64 0x137 +#define MX_106 0x141 + +//################ define - Dynamixel operating mode table ###################### +#define CURRENT_CONTROL_MODE 0 +#define VELOCITY_CONTROL_MODE 1 +#define POSITION_CONTROL_MODE 3 +#define EXTENDED_POSITION_CONTROL_MODE 4 +#define CURRENT_BASE_POSITION_CONTROL_MODE 5 +#define PWM_CONTROL_MODE 16 + //################ define - Dynamixel Hex control table ###################### #define CTRL_MODEL_NUMBER 0x00 @@ -111,30 +132,36 @@ #define COMMAND_BULK_WRITE 0x93 //################ Instruction packet lengths ############################# -#define PING_LEN 3 -#define READ_INST_LEN 7 +#define PING_LEN 3 +#define READ_INST_LEN 7 #define HOMING_OFFSET_BYTE_LEN 4 #define SET_STATUS_RETURN_LEVEL_LEN 3 -#define REBOOT_LEN 3 +#define REBOOT_LEN 3 #define TORQUE_ENABLE_BYTE_LEN 1 -#define LED_BYTE_LEN 1 -#define STATUS_RETURN_LEVEL_BYTE_LEN 1 +#define LED_BYTE_LEN 1 +#define STATUS_RETURN_LEVEL_BYTE_LEN 1 #define GOAL_POSITION_BYTE_LEN 4 +#define GOAL_CURRENT_BYTE_LEN 2 #define PRESENT_POSITION_BYTE_LEN 4 #define PRESENT_CURRENT_BYTE_LEN 2 -#define PRESENT_TEMPERATURE_BYTE_LEN 1 -#define MOVING_BYTE_LEN 1 -#define HARDWARE_ERROR_STATUS_BYTE_LEN 1 +#define PRESENT_TEMPERATURE_BYTE_LEN 1 +#define MOVING_BYTE_LEN 1 +#define OPERATING_MODE_BYTE_LEN 1 +#define MODEL_NUMBER_BYTE_LEN 2 +#define HARDWARE_ERROR_STATUS_BYTE_LEN 1 #define POSITION_GAINS_BYTE_LEN 6 #define PROFILE_VELOCITY_BYTE_LEN 4 #define CURRENT_LIMIT_BYTE_LEN 2 +#define SHUTDOWN_BYTE_LEN 1 +#define RETURN_DELAY_TIME_BYTE_LEN 1 +#define TEMPERATURE_LIMIT_BYTE_LEN 1 //######################################################################### //############################ Specials ################################### #define NONE 0x00 #define READ 0x01 -#define ALL 0x02 +#define ALL 0x02 #define BROADCAST_ID 0xFE @@ -151,65 +178,70 @@ #define PING_TRIAL_NUM 100 //read status packet -#define READ_HEADER0 0 -#define READ_HEADER1 1 -#define READ_HEADER2 2 -#define READ_HEADER3 3 -#define READ_SERVOID 4 -#define READ_LENL 5 -#define READ_LENH 6 -#define READ_INSTRUCTION 7 -#define READ_ERROR 8 -#define READ_PARAMETER 9 -#define READ_CHECKSUML 10 -#define READ_CHECKSUMH 11 +#define READ_HEADER0 0 +#define READ_HEADER1 1 +#define READ_HEADER2 2 +#define READ_HEADER3 3 +#define READ_SERVOID 4 +#define READ_LENL 5 +#define READ_LENH 6 +#define READ_INSTRUCTION 7 +#define READ_ERROR 8 +#define READ_PARAMETER 9 +#define READ_CHECKSUML 10 +#define READ_CHECKSUMH 11 #define STATUS_PACKET_INSTRUCTION 0x55 +#define NO_ERROR 0 //error -#define ERROR_NO_ERROR 0 -#define ERROR_RESULT_FAIL 1 -#define ERROR_INSTRUCTION_ERROR 2 -#define ERROR_CRC_ERROR 3 -#define ERROR_DATA_RANGE_ERROR 4 -#define ERROR_DATA_LENGTH_ERROR 5 -#define ERROR_DATA_LIMIT_ERROR 6 -#define ERROR_ACCESS_ERROR 7 +#define INPUT_VOLTAGE_ERROR 0 +#define OVERHEATING_ERROR 2 +#define MOTOR_ENCODER_ERROR 3 +#define ELECTRICAL_SHOCK_ERROR 4 +#define OVERLOAD_ERROR 5 //addintional error status for external encoder -#define RESOLUTION_RATIO_ERROR 6 -#define ENCODER_CONNECT_ERROR 7 - +#define RESOLUTION_RATIO_ERROR 6 +#define ENCODER_CONNECT_ERROR 7 //for instruction buffer #define INST_GET_CURRENT_LIMIT 0 -#define INST_GET_HARDWARE_ERROR_STATUS 1 +#define INST_GET_HARDWARE_ERROR_STATUS 1 #define INST_GET_HOMING_OFFSET 2 #define INST_GET_POSITION_GAINS 3 #define INST_GET_PRESENT_CURRENT 4 #define INST_GET_PRESENT_MOVING 5 #define INST_GET_PRESENT_POS 6 -#define INST_GET_PRESENT_TEMPERATURE 7 +#define INST_GET_PRESENT_TEMPERATURE 7 #define INST_GET_PROFILE_VELOCITY 8 -#define INST_PING 9 +#define INST_PING 9 #define INST_SET_CURRENT_LIMIT 10 -#define INST_SET_GOAL_POS 11 +#define INST_SET_GOAL_COMMAND 11 #define INST_SET_HOMING_OFFSET 12 #define INST_SET_POSITION_GAINS 13 #define INST_SET_PROFILE_VELOCITY 14 -#define INST_SET_TORQUE 15 +#define INST_SET_TORQUE 15 +#define INST_GET_MODEL_NUMBER 16 +#define INST_GET_OPERATING_MODE 17 //instruction frequency: 0 means no process -#define SET_POS_DU 20 //[msec], 20ms => 50Hz -#define SET_POS_OFFSET 0 // offset from SET_POS +#define SET_COMMAND_DU 20 //[msec], 20ms => 50Hz +#define SET_COMMAND_OFFSET 0 // offset from SET_COMMAND #define GET_POS_DU 20 //[msec], 20ms => 50Hz -#define GET_POS_OFFSET 10 //offset from GET_POS +#define GET_POS_OFFSET 10 //offset from SET_COMMAND #define GET_LOAD_DU 200 //[msec], 200ms => 5Hz -#define GET_LOAD_OFFSET 0 //offset from GET_LOAD +#define GET_LOAD_OFFSET 0 //offset from SET_COMMAND #define GET_TEMP_DU 200 //[msec], 200ms => 5Hz -#define GET_TEMP_OFFSET 50 //offset from GET_TEMP +#define GET_TEMP_OFFSET 50 //offset from SET_COMMAND #define GET_MOVE_DU 200 //[msec], 200ms => 5Hz -#define GET_MOVE_OFFSET 100 //offset from GET_MOVE +#define GET_MOVE_OFFSET 100 //offset from SET_COMMAND #define GET_HARDWARE_ERROR_STATUS_DU 200 //[msec], 200ms => 5Hz -#define GET_HARDWARE_ERROR_STATUS_OFFSET 150 +#define GET_HARDWARE_ERROR_STATUS_OFFSET 150 //offset from SET_COMMAND + +// pre-defined configuration for dynamixel +#define TEMPERATURE_LIMIT 60 +#define RETURN_DELAY_TIME 25 +#define SHUTDOWN_BIT 1 << OVERHEATING_ERROR | 1 << ELECTRICAL_SHOCK_ERROR + /* please define the gpio which control the IO direction */ #define WE HAL_GPIO_WritePin(RS485EN_GPIO_Port, RS485EN_Pin, GPIO_PIN_SET); @@ -280,7 +312,9 @@ class ServoData { ServoData(){} ServoData(uint8_t id): id_(id), + goal_current_(0), internal_offset_(0), + operating_mode_(0), hardware_error_status_(0), torque_enable_(false), force_servo_off_(false), @@ -290,12 +324,16 @@ class ServoData { uint8_t id_; int32_t present_position_; int32_t goal_position_; + int16_t goal_current_; int32_t calib_value_; int32_t homing_offset_; int32_t internal_offset_; uint8_t present_temp_; int16_t present_current_; uint8_t moving_; + uint16_t model_number_; + uint8_t shutdown_bit_; + uint8_t operating_mode_; uint8_t hardware_error_status_; uint16_t p_gain_, i_gain_, d_gain_; uint16_t profile_velocity_; @@ -316,7 +354,8 @@ class ServoData { int32_t getPresentPosition() const {return present_position_;} void setGoalPosition(int32_t goal_position) {goal_position_ = resolution_ratio_ * goal_position - internal_offset_;} int32_t getGoalPosition() const {return goal_position_;} - + void setGoalCurrent(int16_t goal_current) {goal_current_ = goal_current;} + int16_t getGoalCurrent() const { return goal_current_;} bool operator==(const ServoData& r) const {return this->id_ == r.id_;} }; @@ -350,7 +389,7 @@ class DynamixelSerial unsigned int servo_num_; std::array servo_; uint16_t ttl_rs485_mixed_; - uint32_t set_pos_tick_; + uint32_t set_command_tick_; uint32_t get_pos_tick_; uint32_t get_load_tick_; uint32_t get_temp_tick_; @@ -375,6 +414,8 @@ class DynamixelSerial inline void cmdReadHardwareErrorStatus(uint8_t servo_index); inline void cmdReadHomingOffset(uint8_t servo_index); inline void cmdReadMoving(uint8_t servo_index); + inline void cmdReadModelNumber(uint8_t servo_index); + inline void cmdReadOperatingMode(uint8_t servo_index); inline void cmdReadPositionGains(uint8_t servo_index); inline void cmdReadPresentCurrent(uint8_t servo_index); inline void cmdReadPresentPosition(uint8_t servo_index); @@ -386,26 +427,35 @@ class DynamixelSerial inline void cmdWriteProfileVelocity(uint8_t servo_index); inline void cmdWriteStatusReturnLevel(uint8_t id, uint8_t set); inline void cmdWriteTorqueEnable(uint8_t servo_index); + inline void cmdWriteGoalCurrent(uint8_t servo_index); inline void cmdSyncReadCurrentLimit(bool send_all = true); inline void cmdSyncReadHardwareErrorStatus(bool send_all = true); inline void cmdSyncReadHomingOffset(bool send_all = true); inline void cmdSyncReadMoving(bool send_all = true); + inline void cmdSyncReadModelNumber(bool send_all = true); + inline void cmdSyncReadOperatingMode(bool send_all = true); inline void cmdSyncReadPositionGains(bool send_all = true); inline void cmdSyncReadPresentCurrent(bool send_all = true); inline void cmdSyncReadPresentPosition(bool send_all = true); inline void cmdSyncReadPresentTemperature(bool send_all = true); inline void cmdSyncReadProfileVelocity(bool send_all = true); inline void cmdSyncWriteGoalPosition(); + inline void cmdSyncWriteGoalCurrent(); inline void cmdSyncWriteLed(); inline void cmdSyncWritePositionGains(); inline void cmdSyncWriteProfileVelocity(); inline void cmdSyncWriteTorqueEnable(); + inline void cmdSyncWriteShutdownBit(); + inline void cmdSyncWriteReturnDelayTime(); + inline void cmdSyncWriteTemperatureLimit(); inline void setStatusReturnLevel(); inline void getHomingOffset(); inline void getCurrentLimit(); inline void getPositionGains(); inline void getProfileVelocity(); + inline void getModelNumber(); + inline void getOperatingMode(); uint16_t calcCRC16(uint16_t crc_accum, uint8_t *data_blk_ptr, int data_blk_size); }; diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp index 575c30478..491f1cb07 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp @@ -61,5 +61,14 @@ void Servo::receiveDataCallback(uint8_t message_id, uint32_t DLC, uint8_t* data) } break; } + case CAN::MESSAGEID_RECEIVE_SERVO_CURRENT: + { + for (unsigned int i = 0; i < servo_handler_.getServoNum(); i++) { + ServoData& s = servo_handler_.getServo()[i]; + int16_t goal_current = (int16_t)(((data[i * 2 + 1] << 8) & 0xFF00) | (data[i * 2] & 0xFF)); + s.setGoalCurrent(goal_current); + } + break; + } } } diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.cpp index 7c8816dc6..9f78c712c 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.cpp @@ -256,16 +256,17 @@ void CANInitializer::receiveDataCallback(uint8_t slave_id, uint8_t message_id, u neuron_[index].can_servo_.servo_[servo_index].goal_position_ = (data[2] << 8) | data[1]; neuron_[index].can_servo_.servo_[servo_index].profile_velocity_ = (data[4] << 8) | data[3]; neuron_[index].can_servo_.servo_[servo_index].current_limit_ = (data[6] << 8) | data[5]; - neuron_[index].can_servo_.servo_[servo_index].send_data_flag_ = data[7]; + neuron_[index].can_servo_.servo_[servo_index].send_data_flag_ = data[7] & 0x01; + neuron_[index].can_servo_.servo_[servo_index].external_encoder_flag_ = data[7] & 0x02; break; } case CAN::MESSAGEID_SEND_INITIAL_CONFIG_3: { uint8_t servo_index = data[0]; neuron_[index].can_servo_.servo_[servo_index].error_ = data[1]; - neuron_[index].can_servo_.servo_[servo_index].external_encoder_flag_ = data[2]; - neuron_[index].can_servo_.servo_[servo_index].joint_resolution_ = (data[4] << 8) | data[3]; - neuron_[index].can_servo_.servo_[servo_index].servo_resolution_ = (data[6] << 8) | data[5]; + neuron_[index].can_servo_.servo_[servo_index].goal_current_ = (data[3] << 8) | data[2]; + neuron_[index].can_servo_.servo_[servo_index].joint_resolution_ = (data[5] << 8) | data[4]; + neuron_[index].can_servo_.servo_[servo_index].servo_resolution_ = (data[7] << 8) | data[6]; break; } default: diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.cpp index d0989306c..4b5deff01 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.cpp @@ -21,6 +21,13 @@ void CANServo::sendData() target_angle[i] = ((servo_[i].torque_enable_ ? 1 : 0) << 15) | (servo_[i].goal_position_ & 0x7FFF); } sendMessage(CAN::MESSAGEID_RECEIVE_SERVO_ANGLE, m_slave_id, servo_.size() * 2, reinterpret_cast(target_angle), 0); + + int16_t target_current[4]; + for (unsigned int i = 0 ; i < servo_.size(); i++) { + target_current[i] = servo_[i].goal_current_; + } + sendMessage(CAN::MESSAGEID_RECEIVE_SERVO_CURRENT, m_slave_id, servo_.size() * 2, reinterpret_cast(target_current), 0); + } void CANServo::receiveDataCallback(uint8_t slave_id, uint8_t message_id, uint32_t DLC, uint8_t* data) diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.h b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.h index 6f9ff5ec0..a7704a9c0 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.h +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.h @@ -18,8 +18,9 @@ class Servo uint16_t p_gain_, i_gain_, d_gain_; int16_t present_position_; int16_t goal_position_; + int16_t goal_current_; uint16_t profile_velocity_; - uint16_t present_current_; + int16_t present_current_; uint16_t current_limit_; uint8_t present_temperature_; uint8_t moving_; @@ -55,6 +56,7 @@ class Servo bool getForceServoOffFlag() const {return force_servo_off_;} void setIndex(uint8_t index) {index_ = index;} void setGoalPosition(int16_t goal_position) {goal_position_ = goal_position;} + void setGoalCurrent(int16_t goal_current) {goal_current_ = goal_current;} void setTorqueEnable(bool torque_enable); }; diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.cpp index 8410f4ecf..45be37c4b 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.cpp @@ -32,13 +32,18 @@ namespace Spine spinal::ServoStates servo_state_msg_; spinal::ServoTorqueStates servo_torque_state_msg_; ros::Publisher servo_state_pub_("servo/states", &servo_state_msg_); + // merge torque_states to states ros::Publisher servo_torque_state_pub_("servo/torque_states", &servo_torque_state_msg_); #if SEND_GYRO hydrus::Gyro gyro_msg_; ros::Publisher gyro_pub_("hydrus_gyro", &gyro_msg_); #endif - ros::Subscriber servo_ctrl_sub_("servo/target_states", servoControlCallback); + // rename following subscriber. + // taget_states -> target_position + // torque_enable -> control_enable + ros::Subscriber servo_position_sub_("servo/target_states", servoPositionCallback); + ros::Subscriber servo_current_sub_("servo/target_current", servoCurrentCallback); ros::Subscriber servo_torque_ctrl_sub_("servo/torque_enable", servoTorqueControlCallback); ros::ServiceServer board_info_srv_("get_board_info", boardInfoCallback); @@ -85,7 +90,7 @@ namespace Spine res = board_info_res_; } - void servoControlCallback(const spinal::ServoControlCmd& control_msg) + void servoPositionCallback(const spinal::ServoControlCmd& control_msg) { if (!servo_control_flag_) return; if (control_msg.index_length != control_msg.angles_length) return; @@ -94,6 +99,16 @@ namespace Spine } } + void servoCurrentCallback(const spinal::ServoControlCmd& control_msg) + { + if (!servo_control_flag_) return; + if (control_msg.index_length != control_msg.angles_length) return; + for (unsigned int i = 0; i < control_msg.index_length; i++) { + servo_.at(control_msg.index[i]).get().setGoalCurrent(control_msg.angles[i]); + // TODO: change angles -> commands + } + } + void servoTorqueControlCallback(const spinal::ServoTorqueCmd& control_msg) { if (control_msg.index_length != control_msg.torque_enable_length) return; @@ -134,7 +149,8 @@ namespace Spine nh_->advertise(gyro_pub_); #endif - nh_->subscribe(servo_ctrl_sub_); + nh_->subscribe(servo_position_sub_); + nh_->subscribe(servo_current_sub_); nh_->subscribe(servo_torque_ctrl_sub_); nh_->advertiseService(board_info_srv_); @@ -221,8 +237,12 @@ namespace Spine if(HAL_GetTick() < can_tx_idle_start_time_ + CAN_TX_PAUSE_TIME) return; if(HAL_GetTick() % 2 == 0) { + // 500Hz can_motor_send_device_.sendData(); + } + else { if (slave_num_ != 0) { + // 500Hz neuron_.at(send_board_index).can_servo_.sendData(); send_board_index++; if (send_board_index == slave_num_) send_board_index = 0; diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.h b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.h index 85be79aaa..02fe2fc28 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.h +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.h @@ -60,7 +60,8 @@ namespace Spine int8_t getUavModel(); void useRTOS(osMailQId* handle); void setServoControlFlag(bool flag); - void servoControlCallback(const spinal::ServoControlCmd& control_msg); + void servoPositionCallback(const spinal::ServoControlCmd& control_msg); + void servoCurrentCallback(const spinal::ServoControlCmd& control_msg); void servoTorqueControlCallback(const spinal::ServoTorqueCmd& control_msg); void boardInfoCallback(const spinal::GetBoardInfo::Request& req, spinal::GetBoardInfo::Response& res); void boardConfigCallback(const spinal::SetBoardConfig::Request& req, spinal::SetBoardConfig::Response& res); diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_constants.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_constants.h index 3e3aa10cd..425e439bf 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_constants.h +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/CAN/can_constants.h @@ -31,6 +31,7 @@ namespace CAN { constexpr uint8_t MESSAGEID_RECEIVE_PWM_0_5 = 0; constexpr uint8_t MESSAGEID_RECEIVE_PWM_6_11 = 1; constexpr uint8_t MESSAGEID_RECEIVE_SERVO_ANGLE = 0; + constexpr uint8_t MESSAGEID_RECEIVE_SERVO_CURRENT = 1; constexpr uint8_t MESSAGEID_RECEIVE_SERVO_CONFIG = 15; constexpr uint8_t MESSAGEID_SEND_SERVO_LIST[4] = {0, 1, 2, 3}; constexpr uint8_t MESSAGEID_RECEIVE_ENUM_REQUEST = 0; diff --git a/robots/dragon/config/quad/Servo.yaml b/robots/dragon/config/quad/Servo.yaml index 2efddbaaa..7335a147b 100644 --- a/robots/dragon/config/quad/Servo.yaml +++ b/robots/dragon/config/quad/Servo.yaml @@ -1,13 +1,10 @@ servo_controller: - state_sub_topic: servo/states - ctrl_pub_topic: servo/target_states - torque_pub_topic: servo/torque_enable joints: angle_sgn: 1 angle_scale: 0.00076699 zero_point_offset: 2047 - torque_scale: 1.0 # TODO please update the scale + torque_scale: 0.01 # for low pass filter filter_flag: true diff --git a/robots/hydrus/config/quad/default_mode_201907/Servo.yaml b/robots/hydrus/config/quad/default_mode_201907/Servo.yaml index 7a5793ed9..8cbe6c2fb 100644 --- a/robots/hydrus/config/quad/default_mode_201907/Servo.yaml +++ b/robots/hydrus/config/quad/default_mode_201907/Servo.yaml @@ -1,7 +1,4 @@ servo_controller: - state_sub_topic: servo/states - ctrl_pub_topic: servo/target_states - torque_pub_topic: servo/torque_enable joints: angle_sgn: 1 diff --git a/robots/hydrus/config/quad/old_model_tx2_rs_t265_201906/Servo.yaml b/robots/hydrus/config/quad/old_model_tx2_rs_t265_201906/Servo.yaml index 9e1bef575..522ad8073 100644 --- a/robots/hydrus/config/quad/old_model_tx2_rs_t265_201906/Servo.yaml +++ b/robots/hydrus/config/quad/old_model_tx2_rs_t265_201906/Servo.yaml @@ -1,7 +1,4 @@ servo_controller: - state_sub_topic: servo/states - ctrl_pub_topic: servo/target_states - torque_pub_topic: servo/torque_enable joints: angle_sgn: 1 diff --git a/robots/hydrus/config/quad/old_model_tx2_zed_201810/Servo.yaml b/robots/hydrus/config/quad/old_model_tx2_zed_201810/Servo.yaml index d2f848fba..deea48d70 100644 --- a/robots/hydrus/config/quad/old_model_tx2_zed_201810/Servo.yaml +++ b/robots/hydrus/config/quad/old_model_tx2_zed_201810/Servo.yaml @@ -1,7 +1,4 @@ servo_controller: - state_sub_topic: servo/states - ctrl_pub_topic: servo/target_states - torque_pub_topic: servo/torque_enable joints: angle_sgn: 1 diff --git a/robots/hydrus/config/quad/tilt_10deg_15inch_202106/Servo.yaml b/robots/hydrus/config/quad/tilt_10deg_15inch_202106/Servo.yaml index 545a72cda..51156e774 100644 --- a/robots/hydrus/config/quad/tilt_10deg_15inch_202106/Servo.yaml +++ b/robots/hydrus/config/quad/tilt_10deg_15inch_202106/Servo.yaml @@ -1,7 +1,4 @@ servo_controller: - state_sub_topic: servo/states - ctrl_pub_topic: servo/target_states - torque_pub_topic: servo/torque_enable joints: angle_sgn: 1 diff --git a/robots/hydrus/config/quad/tilt_10deg_201907/Servo.yaml b/robots/hydrus/config/quad/tilt_10deg_201907/Servo.yaml index 545a72cda..51156e774 100644 --- a/robots/hydrus/config/quad/tilt_10deg_201907/Servo.yaml +++ b/robots/hydrus/config/quad/tilt_10deg_201907/Servo.yaml @@ -1,7 +1,4 @@ servo_controller: - state_sub_topic: servo/states - ctrl_pub_topic: servo/target_states - torque_pub_topic: servo/torque_enable joints: angle_sgn: 1 diff --git a/robots/hydrus/config/quad/tilt_20deg_201907/Servo.yaml b/robots/hydrus/config/quad/tilt_20deg_201907/Servo.yaml index 545a72cda..51156e774 100644 --- a/robots/hydrus/config/quad/tilt_20deg_201907/Servo.yaml +++ b/robots/hydrus/config/quad/tilt_20deg_201907/Servo.yaml @@ -1,7 +1,4 @@ servo_controller: - state_sub_topic: servo/states - ctrl_pub_topic: servo/target_states - torque_pub_topic: servo/torque_enable joints: angle_sgn: 1 diff --git a/robots/hydrus_xi/config/hex/Servo.yaml b/robots/hydrus_xi/config/hex/Servo.yaml index 7b86eda4c..c78947cd1 100644 --- a/robots/hydrus_xi/config/hex/Servo.yaml +++ b/robots/hydrus_xi/config/hex/Servo.yaml @@ -1,7 +1,4 @@ servo_controller: - state_sub_topic: servo/states - ctrl_pub_topic: servo/target_states - torque_pub_topic: servo/torque_enable joints: angle_sgn: -1 diff --git a/robots/hydrus_xi/config/hex_branch/Servo.yaml b/robots/hydrus_xi/config/hex_branch/Servo.yaml index 20f56d8f3..d11acbcc2 100644 --- a/robots/hydrus_xi/config/hex_branch/Servo.yaml +++ b/robots/hydrus_xi/config/hex_branch/Servo.yaml @@ -1,7 +1,4 @@ servo_controller: - state_sub_topic: servo/states - ctrl_pub_topic: servo/target_states - torque_pub_topic: servo/torque_enable joints: angle_sgn: -1 diff --git a/robots/hydrus_xi/config/quad/Servo.yaml b/robots/hydrus_xi/config/quad/Servo.yaml index 91948e184..fa3c6634d 100644 --- a/robots/hydrus_xi/config/quad/Servo.yaml +++ b/robots/hydrus_xi/config/quad/Servo.yaml @@ -1,7 +1,4 @@ servo_controller: - state_sub_topic: servo/states - ctrl_pub_topic: servo/target_states - torque_pub_topic: servo/torque_enable joints: angle_sgn: 1