From 1f3f6b45db177e83b618a12704e400662244935f Mon Sep 17 00:00:00 2001 From: Moju Zhao Date: Fri, 14 Apr 2023 18:47:38 +0900 Subject: [PATCH] [Neuron][Servo][Encoder] correct the joint offset calib process --- .../neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) 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 2ad30d8ef..59732d501 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp @@ -120,7 +120,13 @@ void DynamixelSerial::setRoundOffset(uint8_t servo_index, int32_t ref_value) void DynamixelSerial::setHomingOffset(uint8_t servo_index) { if(servo_[servo_index].external_encoder_flag_){ - servo_[servo_index].joint_offset_ = servo_[servo_index].calib_value_ + servo_[servo_index].joint_offset_ - servo_[servo_index].present_position_; + int32_t joint_offset = servo_[servo_index].calib_value_ + servo_[servo_index].joint_offset_ - servo_[servo_index].present_position_; + if(joint_offset >= 0){ + servo_[servo_index].joint_offset_ = joint_offset % 4096; + } + else { + servo_[servo_index].joint_offset_ = joint_offset % -4096; + } // servo_[servo_index].joint_offset_ = servo_[servo_index].calib_value_; // debug encoder_handler_.setOffset(servo_[servo_index].joint_offset_); servo_[servo_index].first_get_pos_flag_ = true;