[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
jsk-ros-pkg · Nov 27, 2024 · 469059f · 469059f
1 parent d9923e2
commit 469059f
Show file tree

Hide file tree

Showing 6 changed files with 63 additions and 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
@@ -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
@@ -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
@@ -27,6 +27,7 @@ void Servo::sendData()
 			CANServoData data(static_cast<int16_t>(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<uint8_t*>(&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
@@ -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
@@ -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<CANServoData*>(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
@@ -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;
 	};