init

gripper/hardware/__init__.py

@@ -0,0 +1,4 @@
+from .gripper_client import GripperClient
+
+
+__all__ = ["GripperClient"]

gripper/hardware/gripper_client.py

@@ -0,0 +1,119 @@
+from ..common.filters import clamp
+from ..common.paths import ensure_project_paths
+
+
+class GripperClient:
+    def __init__(self, config):
+        self.config = config
+        self.motor = None
+
+    def connect(self):
+        if self.config.dry_run:
+            return self
+
+        ensure_project_paths()
+        from changingtek_p_rtu_Servo import MotorController
+
+        self.motor = MotorController(
+            self.config.port,
+            self.config.slave_id,
+            baudrate=self.config.baudrate,
+            timeout=self.config.serial_timeout,
+        )
+        return self
+
+    def move(self, position, force_pct, label, speed_pct=None, accel=None, decel=None):
+        config = self.config
+        speed_pct = config.speed if speed_pct is None else speed_pct
+        accel = config.accel if accel is None else accel
+        decel = config.decel if decel is None else decel
+
+        if config.dry_run:
+            print(
+                f"[dry-run] {label}: temp_move "
+                f"position={position}, speed={speed_pct}, force={force_pct}, "
+                f"accel={accel}, decel={decel}"
+            )
+            return
+        if self.motor is None:
+            raise RuntimeError(
+                "gripper motor is not connected; check serial port, conda environment, "
+                "and changingtek_p_rtu_Servo.py"
+            )
+
+        self.motor.temp_move(
+            position_mm=int(position),
+            speed_pct=int(speed_pct),
+            force_pct=int(force_pct),
+            accel=int(accel),
+            decel=int(decel),
+            trigger=True,
+        )
+
+    def open(self, force_pct, label="open", speed_pct=None, accel=None, decel=None):
+        self.move(
+            self.config.open_pos,
+            force_pct,
+            label,
+            speed_pct=speed_pct,
+            accel=accel,
+            decel=decel,
+        )
+
+    def close(self, force_pct, label="close"):
+        self.move(self.config.close_pos, force_pct, label)
+
+    def set_force(self, force_pct):
+        config = self.config
+        force_pct = int(clamp(force_pct, config.force_min, config.force_max))
+        if config.dry_run:
+            print(f"[dry-run] set_target_force {force_pct}")
+            return force_pct
+
+        self.motor.set_target_force(force_pct)
+        if config.trigger_force_update:
+            self.motor.trigger_motion()
+        return force_pct
+
+    def read_position(self):
+        if self.config.dry_run:
+            return None
+        try:
+            return self.motor.read_real_position()
+        except Exception as exc:
+            print(f"read_real_position failed: {exc}")
+            return None
+
+    def hold_current_position(self, force_pct):
+        current_pos = self.read_position()
+        if current_pos is None:
+            current_pos = self.config.hold_fallback_pos
+
+        if current_pos is None:
+            self.set_force(force_pct)
+            self._stop_by_speed()
+            return None
+
+        self.move(int(current_pos), force_pct, "clamp-hold")
+        return int(current_pos)
+
+    def final_open_if_needed(self):
+        if not self.config.open_at_end:
+            return
+        if not self.config.dry_run and self.motor is None:
+            return
+        final_force = int(clamp(
+            self.config.initial_force,
+            self.config.force_min,
+            self.config.force_max,
+        ))
+        self.open(final_force, "open-at-end")
+
+    def _stop_by_speed(self):
+        if self.config.dry_run:
+            return
+        try:
+            self.motor.set_target_speed(0)
+            self.motor.trigger_motion()
+        except Exception as exc:
+            print(f"stop by speed=0 failed: {exc}")