cheng
103 lines
3.0 KiB
Python
103 lines
3.0 KiB
Python
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}")
|
|
|