[Neuron][Servo][Encoder] correct the joint offset calib process

jsk-ros-pkg · Jun 24, 2023 · 1f3f6b4 · 1f3f6b4
1 parent fae7e1b
commit 1f3f6b4
Showing 1 changed file with 7 additions and 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
@@ -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;