cheng
113 lines
3.3 KiB
Python
113 lines
3.3 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, 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
|
|
|
|
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
|
|
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}")
|