init

gripper_control/hardware.py

@@ -0,0 +1,102 @@
+from .filters import clamp
+from .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):
+        config = self.config
+        if config.dry_run:
+            print(
+                f"[dry-run] {label}: temp_move "
+                f"position={position}, speed={config.speed}, force={force_pct}, "
+                f"accel={config.accel}, decel={config.decel}"
+            )
+            return
+
+        self.motor.temp_move(
+            position_mm=int(position),
+            speed_pct=int(config.speed),
+            force_pct=int(force_pct),
+            accel=int(config.accel),
+            decel=int(config.decel),
+            trigger=True,
+        )
+
+    def open(self, force_pct, label="open"):
+        self.move(self.config.open_pos, force_pct, label)
+
+    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
+        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}")
+