From 469059f6320ca8cad8947f77021762713050ba5d 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: Wed, 27 Nov 2024 14:34:42 +0900 Subject: [PATCH] [Spinal][Neuron][Servo] refactor the torque off rule (#636) * [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 --- .../Servo/Dynamixel/dynamixel_serial.cpp | 26 +++++++++++++++++++ .../Servo/Dynamixel/dynamixel_serial.h | 10 ++++++- .../neuron/neuronlib/Servo/servo.cpp | 11 +++++--- .../neuron/neuronlib/Servo/servo.h | 9 ++++--- .../Hydrus_Lib/CANDevice/servo/can_servo.cpp | 14 +++++++--- .../Hydrus_Lib/CANDevice/servo/can_servo.h | 8 +++--- 6 files changed, 63 insertions(+), 15 deletions(-) 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 56ea7ed65..77f097134 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp @@ -272,6 +272,32 @@ void DynamixelSerial::update() } else { instruction_buffer_.push(std::make_pair(INST_GET_HARDWARE_ERROR_STATUS, 0)); } + + // 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_) { + servo_[i].torque_enable_= false; + setTorque(i); // servo off + } + } + else { + servo_[i].force_servo_off_ = false; + } + } } 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 50aeff4e3..87a7f77d7 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.h +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.h @@ -278,7 +278,14 @@ class RingBuffer class ServoData { public: ServoData(){} - ServoData(uint8_t id): id_(id), torque_enable_(false), first_get_pos_flag_(true), internal_offset_(0){} + ServoData(uint8_t id): + id_(id), + internal_offset_(0), + hardware_error_status_(0), + torque_enable_(false), + force_servo_off_(false), + first_get_pos_flag_(true) + {} uint8_t id_; int32_t present_position_; @@ -301,6 +308,7 @@ class ServoData { float resolution_ratio_; bool led_; bool torque_enable_; + bool force_servo_off_; bool first_get_pos_flag_; void updateHomingOffset() { homing_offset_ = calib_value_ - present_position_;} diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp index 5477e84e1..116b85126 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp @@ -27,6 +27,7 @@ void Servo::sendData() CANServoData data(static_cast(s.getPresentPosition()), s.present_temp_, s.moving_, + s.force_servo_off_, s.present_current_, s.hardware_error_status_); sendMessage(CAN::MESSAGEID_SEND_SERVO_LIST[i], m_slave_id, 8, reinterpret_cast(&data), 1); @@ -51,10 +52,12 @@ void Servo::receiveDataCallback(uint8_t message_id, uint32_t DLC, uint8_t* data) } s.setGoalPosition(goal_pos); bool torque_enable = (((data[i * 2 + 1] >> 7) & 0x01) != 0) ? true : false; - if (s.torque_enable_ != torque_enable) { - s.torque_enable_ = torque_enable; - servo_handler_.setTorque(i); - } + if (!s.force_servo_off_) { + if (s.torque_enable_ != torque_enable) { + s.torque_enable_ = torque_enable; + servo_handler_.setTorque(i); + } + } } break; } diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h index 9d8162bd4..29cba4257 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h @@ -31,11 +31,14 @@ class Servo : public CANDevice struct CANServoData{ int16_t angle; uint8_t temperature; - uint8_t moving; + uint8_t status; int16_t current; uint8_t error; - CANServoData(uint16_t angle, uint8_t temperature, uint8_t moving, int16_t current, uint8_t error) - :angle(angle), temperature(temperature), moving(moving), current(current), error(error){} + CANServoData(uint16_t angle, uint8_t temperature, uint8_t moving, bool force_servo_disable, int16_t current, uint8_t error) + :angle(angle), temperature(temperature), current(current), error(error) + { + status = (force_servo_disable? 1 : 0) | moving << 1; + } }; bool connect_; 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 bfaf9b0a1..d0989306c 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 @@ -7,10 +7,11 @@ #include "can_servo.h" -bool Servo::getTorqueEnable() +void Servo::setTorqueEnable(bool torque_enable) { - if(error_ != 0) torque_enable_ = false; - return torque_enable_; + if (force_servo_off_) return; + + torque_enable_ = torque_enable; } void CANServo::sendData() @@ -27,7 +28,12 @@ void CANServo::receiveDataCallback(uint8_t slave_id, uint8_t message_id, uint32_ CANServoData servo_data = *(reinterpret_cast(data)); servo_[message_id].present_position_ = servo_data.angle; servo_[message_id].present_temperature_ = servo_data.temperature; - servo_[message_id].moving_ = servo_data.moving; + servo_[message_id].moving_ = (servo_data.status >> 1) & 0x01 ; + servo_[message_id].force_servo_off_ = ((servo_data.status & 0x01) != 0)? true: false; servo_[message_id].present_current_ = servo_data.current; servo_[message_id].error_ = servo_data.error; + + if (servo_[message_id].force_servo_off_) { + servo_[message_id].torque_enable_ = false; + } } 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 060dbcb77..6f9ff5ec0 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 @@ -26,6 +26,7 @@ class Servo uint8_t error_; bool send_data_flag_; bool torque_enable_; + bool force_servo_off_; bool external_encoder_flag_; uint16_t joint_resolution_; uint16_t servo_resolution_; @@ -50,10 +51,11 @@ class Servo uint8_t getPresentTemperature() const {return present_temperature_;} uint8_t getMoving() const {return moving_;} uint8_t getError() const {return error_;} - bool getTorqueEnable(); + bool getTorqueEnable() const {return torque_enable_;} + 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 setTorqueEnable(bool torque_enable) {torque_enable_ = torque_enable;} + void setTorqueEnable(bool torque_enable); }; class CANServo : public CANDevice @@ -73,7 +75,7 @@ class CANServo : public CANDevice struct CANServoData{ int16_t angle; uint8_t temperature; - uint8_t moving; + uint8_t status; int16_t current; uint8_t error; };