diff --git a/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml b/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml index ce53806..e60c2f9 100644 --- a/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml +++ b/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml @@ -16,14 +16,14 @@ joint_sensor_node: - 1.3744467859455343 right_sensor_directions: - -1.0 - - 1.0 + - -1.0 - -1.0 - 1.0 - -1.0 - 1.0 - -1.0 - 1.0 - right_auto_zero_on_first_sample: false + right_auto_zero_on_first_sample: true left_zero_offsets: - 2.492718780314167 - -0.7577865092155065 @@ -35,14 +35,14 @@ joint_sensor_node: - 1.2977477465512521 left_sensor_directions: - -1.0 - - 1.0 + - -1.0 - -1.0 - -1.0 - -1.0 - 1.0 - - 1.0 - -1.0 - left_auto_zero_on_first_sample: false + - -1.0 + left_auto_zero_on_first_sample: true right_calibrate_now: false left_calibrate_now: false calibration_yaml_path: /home/shen/openarm_test/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml diff --git a/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py b/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py index 0f5149b..87de9c2 100644 --- a/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py +++ b/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py @@ -537,10 +537,6 @@ class JointSensorNode(Node): if len(data) < 2: return None - # 通用格式:前2字节角度,默认 centi-rad - # raw = int.from_bytes(data[0:2], byteorder='big', signed=True) - # return self.raw_to_rad(raw) - def wrap_to_pi(self, angle_rad: float) -> float: # 归一化到 [-pi, pi),解决 0~2pi 到 ±pi 的过零映射问题 return (angle_rad + math.pi) % (2.0 * math.pi) - math.pi @@ -562,12 +558,6 @@ class JointSensorNode(Node): return prev + delta - def raw_to_rad(self, raw_value: int) -> float: - if self.raw_angle_unit == 'centi-deg': - return math.radians(raw_value / 100.0) - # 默认按 centi-rad 处理,适配你当前 0~6.28 弧度输出 - return raw_value / 100.0 - def parse_sensor_from_can(self, data: bytes): if len(data) == 0: return 0.0, 0.0 @@ -587,14 +577,6 @@ class JointSensorNode(Node): if len(data) < 8: return 0.0, 0.0 - # # 通用格式:前2字节为角度,后2字节为速度,默认按 centi-rad 解析 - # raw_deg = int.from_bytes(data[0:2], byteorder='big', signed=True) - # rad = self.wrap_to_pi(self.raw_to_rad(raw_deg)) - - # raw_vel = int.from_bytes(data[2:4], byteorder='big', signed=True) - # vel_rad = self.raw_to_rad(raw_vel) - - # return rad, vel_rad def main():