diff --git a/reBotArm_control_py/actuator/arm.py b/reBotArm_control_py/actuator/arm.py index 19130b3..3cfdc89 100644 --- a/reBotArm_control_py/actuator/arm.py +++ b/reBotArm_control_py/actuator/arm.py @@ -572,6 +572,30 @@ def _ensure_mode(self, name: str, mot: any, mode: Mode, print(f"[ensure_mode/{name}] 跳过: {e}") return False + def _write_register_f32_retry( + self, + name: str, + mot: any, + register: int, + value: float, + label: str, + retries: int = 4, + delay: float = 0.08, + ) -> bool: + last_exc: Exception | None = None + for attempt in range(1, retries + 1): + try: + mot.write_register_f32(register, value) + return True + except Exception as exc: + last_exc = exc + time.sleep(delay * attempt) + print( + f"[mode_pos_vel/{name}] write {label} failed after " + f"{retries} attempts: {last_exc}" + ) + return False + def mode_mit(self, kp: Optional[np.ndarray] = None, kd: Optional[np.ndarray] = None, stabilize_delay: float = 0.2) -> bool: @@ -612,14 +636,18 @@ def mode_pos_vel(self, vlim: Optional[np.ndarray] = None, ok = True for jc in self._joints: m = self._motor_map[jc.name] - try: - m.write_register_f32(25, jc.vel_kp) # KP_ASR — 速度环 Kp - m.write_register_f32(26, jc.vel_ki) # KI_ASR — 速度环 Ki - m.write_register_f32(27, jc.pos_kp) # KP_APR — 位置环 Kp - m.write_register_f32(28, jc.pos_ki) # KI_APR — 位置环 Ki - time.sleep(0.02) - except Exception as e: - print(f"[mode_pos_vel/{jc.name}] 写 PI 参数失败: {e}") + ok &= self._write_register_f32_retry( + jc.name, m, 25, jc.vel_kp, "vel_kp" + ) + ok &= self._write_register_f32_retry( + jc.name, m, 26, jc.vel_ki, "vel_ki" + ) + ok &= self._write_register_f32_retry( + jc.name, m, 27, jc.pos_kp, "pos_kp" + ) + ok &= self._write_register_f32_retry( + jc.name, m, 28, jc.pos_ki, "pos_ki" + ) if not self._ensure_mode(jc.name, m, Mode.POS_VEL, 1000): ok = False time.sleep(0.05)