mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2025-12-17 04:51:10 +00:00
a599eb70e5
* 修改lh的json启动 * 修改lh的json启动 * 修改backend，做成sim的通用backend * 修改yaml的地址，3D模型适配网页生产环境 * 添加laiyu硬件连接 * 修改移液枪的状态判断方法， 修改移液枪的状态判断方法， 添加三轴的表定点与零点之间的转换 添加三轴真实移动的backend * 修改laiyu移液站 简化移动方法， 取消软件限制位置， 修改当值使用Z轴时也需要重新复位Z轴的问题 * 更新lh以及laiyu workshop 1,现在可以直接通过修改backend，适配其他的移液站，主类依旧使用LiquidHandler，不用重新编写 2,修改枪头判断标准，使用枪头自身判断而不是类的判断， 3,将归零参数用毫米计算，方便手动调整， 4,修改归零方式，上电使用机械归零，确定机械零点，手动归零设置工作区域零点方便计算，二者互不干涉 * 修改枪头动作 * 修改虚拟仿真方法 --------- Co-authored-by: zhangshixiang <@zhangshixiang> Co-authored-by: Junhan Chang <changjh@dp.tech>
1254 lines
51 KiB
Python
1254 lines
51 KiB
Python
#!/usr/bin/env python3
|
||
# -*- coding: utf-8 -*-
|
||
"""
|
||
XYZ三轴步进电机控制器
|
||
支持坐标系管理、限位开关回零、工作原点设定等功能
|
||
|
||
主要功能：
|
||
- 坐标系转换层（步数↔毫米）
|
||
- 限位开关回零功能
|
||
- 工作原点示教和保存
|
||
- 安全限位检查
|
||
- 运动控制接口
|
||
|
||
"""
|
||
|
||
import json
|
||
import os
|
||
from re import X
|
||
import time
|
||
from typing import Optional, Dict, Tuple, Union
|
||
from dataclasses import dataclass, field, asdict
|
||
from pathlib import Path
|
||
import logging
|
||
|
||
# 添加项目根目录到Python路径以解决模块导入问题
|
||
import sys
|
||
import os
|
||
|
||
# 无论如何都添加项目根目录到路径
|
||
current_file = os.path.abspath(__file__)
|
||
# 从 .../Uni-Lab-OS/unilabos/devices/LaiYu_Liquid/controllers/xyz_controller.py
|
||
# 向上5级到 .../Uni-Lab-OS
|
||
project_root = os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(current_file)))))
|
||
# 强制添加项目根目录到sys.path的开头
|
||
sys.path.insert(0, project_root)
|
||
|
||
# 导入原有的驱动
|
||
from unilabos.devices.liquid_handling.laiyu.drivers.xyz_stepper_driver import XYZStepperController, MotorAxis, MotorStatus
|
||
|
||
logger = logging.getLogger(__name__)
|
||
|
||
|
||
@dataclass
|
||
class MachineConfig:
|
||
"""机械配置参数"""
|
||
# 步距配置 (基于16384步/圈的步进电机)
|
||
steps_per_mm_x: float = 204.8 # X轴步距 (16384步/圈 ÷ 80mm导程)
|
||
steps_per_mm_y: float = 204.8 # Y轴步距 (16384步/圈 ÷ 80mm导程)
|
||
steps_per_mm_z: float = 3276.8 # Z轴步距 (16384步/圈 ÷ 5mm导程)
|
||
|
||
# 行程限制
|
||
max_travel_x: float = 340.0 # X轴最大行程
|
||
max_travel_y: float = 250.0 # Y轴最大行程
|
||
max_travel_z: float = 200.0 # Z轴最大行程
|
||
reference_distance: Dict[str, float] = field(default_factory=lambda: {
|
||
"x": 29,
|
||
"y": -13,
|
||
"z": -75.5
|
||
})
|
||
# 安全移动参数
|
||
safe_z_height: float = 0.0 # Z轴安全移动高度 (mm) - 液体处理工作站安全高度
|
||
z_approach_height: float = 5.0 # Z轴接近高度 (mm) - 在目标位置上方的预备高度
|
||
|
||
# 回零参数
|
||
homing_speed: int = 50 # 回零速度 (mm/s)
|
||
homing_timeout: float = 30.0 # 回零超时时间
|
||
safe_clearance: float = 10.0 # 安全间隙 (mm)
|
||
position_stable_time: float = 1.0 # 位置稳定检测时间（秒）
|
||
position_check_interval: float = 0.2 # 位置检查间隔（秒）
|
||
|
||
# 运动参数
|
||
default_speed: int = 50 # 默认运动速度 (mm/s)
|
||
default_acceleration: int = 1000 # 默认加速度
|
||
|
||
|
||
@dataclass
|
||
class CoordinateOrigin:
|
||
"""坐标原点信息"""
|
||
machine_origin_steps: Dict[str, int] = None # 机械原点步数位置
|
||
work_origin_steps: Dict[str, int] = None # 工作原点步数位置
|
||
is_homed: bool = False # 是否已回零
|
||
timestamp: str = "" # 设定时间戳
|
||
|
||
def __post_init__(self):
|
||
if self.machine_origin_steps is None:
|
||
self.machine_origin_steps = {"x": 0, "y": 0, "z": 0}
|
||
if self.work_origin_steps is None:
|
||
self.work_origin_steps = {"x": 0, "y": 0, "z": 0}
|
||
|
||
|
||
class CoordinateSystemError(Exception):
|
||
"""坐标系统异常"""
|
||
pass
|
||
|
||
|
||
class XYZController(XYZStepperController):
|
||
"""XYZ三轴控制器"""
|
||
|
||
def __init__(self, port: str, baudrate: int = 115200,
|
||
machine_config: Optional[MachineConfig] = None,
|
||
config_file: str = "machine_config.json",
|
||
auto_connect: bool = True):
|
||
"""
|
||
初始化XYZ控制器
|
||
|
||
Args:
|
||
port: 串口端口
|
||
baudrate: 波特率
|
||
machine_config: 机械配置参数
|
||
config_file: 配置文件路径
|
||
auto_connect: 是否自动连接设备
|
||
"""
|
||
super().__init__(port, baudrate)
|
||
|
||
# 机械配置
|
||
self.machine_config = machine_config or MachineConfig()
|
||
self.config_file = config_file
|
||
|
||
# 坐标系统
|
||
self.coordinate_origin = CoordinateOrigin()
|
||
import os
|
||
self.origin_file = os.path.join(os.path.dirname(os.path.abspath(__file__)), "coordinate_origin.json")
|
||
|
||
# 连接状态
|
||
self.is_connected = False
|
||
|
||
# 加载配置
|
||
self._load_config()
|
||
self._load_coordinate_origin()
|
||
|
||
# 自动连接设备
|
||
if auto_connect:
|
||
self.connect_device()
|
||
|
||
def connect_device(self) -> bool:
|
||
"""
|
||
连接设备并初始化
|
||
|
||
Returns:
|
||
bool: 连接是否成功
|
||
"""
|
||
try:
|
||
logger.info(f"正在连接设备: {self.port}")
|
||
|
||
# 连接硬件
|
||
if not self.connect():
|
||
logger.error("硬件连接失败")
|
||
return False
|
||
|
||
self.is_connected = True
|
||
logger.info("设备连接成功")
|
||
|
||
# 使能所有轴
|
||
enable_results = self.enable_all_axes(True)
|
||
success_count = sum(1 for result in enable_results.values() if result)
|
||
logger.info(f"轴使能结果: {success_count}/{len(enable_results)} 成功")
|
||
|
||
# 获取系统状态
|
||
try:
|
||
status = self.get_system_status()
|
||
logger.info(f"系统状态获取成功: {len(status)} 项信息")
|
||
except Exception as e:
|
||
logger.warning(f"获取系统状态失败: {e}")
|
||
|
||
return True
|
||
|
||
except Exception as e:
|
||
logger.error(f"设备连接失败: {e}")
|
||
self.is_connected = False
|
||
return False
|
||
|
||
def disconnect_device(self):
|
||
"""断开设备连接"""
|
||
try:
|
||
if self.is_connected:
|
||
self.disconnect() # 使用父类的disconnect方法
|
||
self.is_connected = False
|
||
logger.info("设备连接已断开")
|
||
except Exception as e:
|
||
logger.error(f"断开连接失败: {e}")
|
||
|
||
def _load_config(self):
|
||
"""加载机械配置"""
|
||
try:
|
||
if os.path.exists(self.config_file):
|
||
with open(self.config_file, 'r', encoding='utf-8') as f:
|
||
config_data = json.load(f)
|
||
# 更新配置参数
|
||
for key, value in config_data.items():
|
||
if hasattr(self.machine_config, key):
|
||
setattr(self.machine_config, key, value)
|
||
logger.info("机械配置加载完成")
|
||
except Exception as e:
|
||
logger.warning(f"加载机械配置失败: {e}，使用默认配置")
|
||
|
||
def _save_config(self):
|
||
"""保存机械配置"""
|
||
try:
|
||
with open(self.config_file, 'w', encoding='utf-8') as f:
|
||
json.dump(asdict(self.machine_config), f, indent=2, ensure_ascii=False)
|
||
logger.info("机械配置保存完成")
|
||
except Exception as e:
|
||
logger.error(f"保存机械配置失败: {e}")
|
||
|
||
def _load_coordinate_origin(self):
|
||
"""加载坐标原点信息"""
|
||
try:
|
||
if os.path.exists(self.origin_file):
|
||
with open(self.origin_file, 'r', encoding='utf-8') as f:
|
||
origin_data = json.load(f)
|
||
|
||
for key, value in origin_data["machine_origin_steps"].items():
|
||
origin_data["machine_origin_steps"][key] = round(self.mm_to_steps(MotorAxis[key.upper()], value), 2)
|
||
|
||
for key, value in origin_data["work_origin_steps"].items():
|
||
origin_data["work_origin_steps"][key] = round(self.mm_to_steps(MotorAxis[key.upper()], value), 2)
|
||
|
||
self.coordinate_origin = CoordinateOrigin(**origin_data)
|
||
logger.info("坐标原点信息加载完成")
|
||
except Exception as e:
|
||
logger.warning(f"加载坐标原点失败: {e}，使用默认设置")
|
||
|
||
def _save_coordinate_origin(self):
|
||
"""保存坐标原点信息"""
|
||
try:
|
||
# 更新时间戳
|
||
from datetime import datetime
|
||
self.coordinate_origin.timestamp = datetime.now().isoformat()
|
||
|
||
with open(self.origin_file, 'w', encoding='utf-8') as f:
|
||
json_data = asdict(self.coordinate_origin)
|
||
for key, value in json_data["machine_origin_steps"].items():
|
||
json_data["machine_origin_steps"][key] = round(self.steps_to_mm(MotorAxis[key.upper()], value), 2)
|
||
|
||
for key, value in json_data["work_origin_steps"].items():
|
||
json_data["work_origin_steps"][key] = round(self.steps_to_mm(MotorAxis[key.upper()], value), 2)
|
||
|
||
json.dump(json_data, f, indent=2, ensure_ascii=False)
|
||
logger.info("坐标原点信息保存完成")
|
||
except Exception as e:
|
||
logger.error(f"保存坐标原点失败: {e}")
|
||
|
||
# ==================== 坐标转换方法 ====================
|
||
|
||
def mm_to_steps(self, axis: MotorAxis, mm: float) -> int:
|
||
"""毫米转步数"""
|
||
if axis == MotorAxis.X:
|
||
return int(mm * self.machine_config.steps_per_mm_x)
|
||
elif axis == MotorAxis.Y:
|
||
return int(mm * self.machine_config.steps_per_mm_y)
|
||
elif axis == MotorAxis.Z:
|
||
return int(mm * self.machine_config.steps_per_mm_z)
|
||
else:
|
||
raise ValueError(f"未知轴: {axis}")
|
||
|
||
def steps_to_mm(self, axis: MotorAxis, steps: int) -> float:
|
||
"""步数转毫米"""
|
||
if axis == MotorAxis.X:
|
||
return steps / self.machine_config.steps_per_mm_x
|
||
elif axis == MotorAxis.Y:
|
||
return steps / self.machine_config.steps_per_mm_y
|
||
elif axis == MotorAxis.Z:
|
||
return steps / self.machine_config.steps_per_mm_z
|
||
else:
|
||
raise ValueError(f"未知轴: {axis}")
|
||
|
||
def work_to_machine_steps(self, x: float = None, y: float = None, z: float = None) -> Dict[str, int]:
|
||
"""工作坐标转机械坐标步数"""
|
||
machine_steps = {}
|
||
|
||
if x is not None:
|
||
work_steps = self.mm_to_steps(MotorAxis.X, x)
|
||
machine_steps['x'] = self.coordinate_origin.work_origin_steps['x'] + work_steps + self.coordinate_origin.machine_origin_steps['x']
|
||
|
||
if y is not None:
|
||
work_steps = self.mm_to_steps(MotorAxis.Y, y)
|
||
machine_steps['y'] = self.coordinate_origin.work_origin_steps['y'] + work_steps + self.coordinate_origin.machine_origin_steps['y']
|
||
|
||
if z is not None:
|
||
work_steps = self.mm_to_steps(MotorAxis.Z, z)
|
||
machine_steps['z'] = self.coordinate_origin.work_origin_steps['z'] + work_steps + self.coordinate_origin.machine_origin_steps['z']
|
||
|
||
return machine_steps
|
||
|
||
def machine_to_work_coords(self, machine_steps: Dict[str, int]) -> Dict[str, float]:
|
||
"""机械坐标步数转工作坐标"""
|
||
work_coords = {}
|
||
|
||
for axis_name, steps in machine_steps.items():
|
||
axis = MotorAxis[axis_name.upper()]
|
||
work_origin_steps = self.coordinate_origin.work_origin_steps[axis_name]
|
||
relative_steps = steps - work_origin_steps - self.coordinate_origin.machine_origin_steps[axis_name]
|
||
work_coords[axis_name] = self.steps_to_mm(axis, relative_steps)
|
||
|
||
return work_coords
|
||
|
||
def check_travel_limits(self, x: float = None, y: float = None, z: float = None) -> bool:
|
||
"""检查行程限制"""
|
||
min_x = min(0, -50)
|
||
min_y = min(0, -50)
|
||
min_z = min(0, -50)
|
||
|
||
if x is not None and (x < min_x or x > self.machine_config.max_travel_x):
|
||
raise CoordinateSystemError(f"X轴超出行程范围: {x}mm ({min_x} ~ {self.machine_config.max_travel_x}mm)")
|
||
|
||
if y is not None and (y < min_y or y > self.machine_config.max_travel_y):
|
||
raise CoordinateSystemError(f"Y轴超出行程范围: {y}mm ({min_y} ~ {self.machine_config.max_travel_y}mm)")
|
||
|
||
if z is not None and (z < min_z or z > self.machine_config.max_travel_z):
|
||
raise CoordinateSystemError(f"Z轴超出行程范围: {z}mm ({min_z} ~ {self.machine_config.max_travel_z}mm)")
|
||
|
||
return True
|
||
|
||
# ==================== 回零和原点设定方法 ====================
|
||
|
||
def home_axis(self, axis: MotorAxis, direction: int = -1, speed: float = None) -> bool:
|
||
"""
|
||
单轴回零到限位开关 - 使用步数变化检测
|
||
|
||
Args:
|
||
axis: 要回零的轴
|
||
direction: 回零方向 (-1负方向, 1正方向)
|
||
|
||
Returns:
|
||
bool: 回零是否成功
|
||
"""
|
||
if not self.is_connected:
|
||
logger.error("设备未连接，无法执行回零操作")
|
||
return False
|
||
|
||
try:
|
||
logger.info(f"开始{axis.name}轴回零")
|
||
|
||
# 使能电机
|
||
if not self.enable_motor(axis, True):
|
||
raise CoordinateSystemError(f"{axis.name}轴使能失败")
|
||
|
||
# 设置回零速度模式，根据方向设置正负
|
||
if speed is None:
|
||
speed_ = self.machine_config.homing_speed
|
||
else:
|
||
speed_ = speed
|
||
|
||
speed_ = min(max(speed_, 0), 500)
|
||
if not self.set_speed_mode(axis, self.ms_to_rpm(axis, speed_) * direction):
|
||
raise CoordinateSystemError(f"{axis.name}轴设置回零速度失败")
|
||
|
||
|
||
|
||
# 智能回零检测 - 基于步数变化
|
||
start_time = time.time()
|
||
limit_detected = False
|
||
final_position = None
|
||
|
||
# 步数变化检测参数（从配置获取）
|
||
position_stable_time = self.machine_config.position_stable_time
|
||
check_interval = self.machine_config.position_check_interval
|
||
last_position = None
|
||
stable_start_time = None
|
||
|
||
logger.info(f"{axis.name}轴开始移动，监测步数变化...")
|
||
|
||
while time.time() - start_time < self.machine_config.homing_timeout:
|
||
status = self.get_motor_status(axis)
|
||
current_position = status.steps
|
||
|
||
# 检查是否明确触碰限位开关
|
||
if (direction < 0 and status.status == MotorStatus.REVERSE_LIMIT_STOP) or \
|
||
(direction > 0 and status.status == MotorStatus.FORWARD_LIMIT_STOP):
|
||
# 停止运动
|
||
self.emergency_stop(axis)
|
||
time.sleep(0.5)
|
||
|
||
# 记录机械原点位置
|
||
final_position = current_position
|
||
limit_detected = True
|
||
logger.info(f"{axis.name}轴检测到限位开关信号，位置: {final_position}步")
|
||
break
|
||
|
||
# 检查是否发生碰撞
|
||
if status.status == MotorStatus.COLLISION_STOP:
|
||
raise CoordinateSystemError(f"{axis.name}轴回零时发生碰撞")
|
||
|
||
# 步数变化检测逻辑
|
||
if last_position is not None:
|
||
# 检查位置是否发生变化
|
||
if abs(current_position - last_position) <= 1: # 允许1步的误差
|
||
# 位置基本没有变化
|
||
if stable_start_time is None:
|
||
stable_start_time = time.time()
|
||
logger.debug(f"{axis.name}轴位置开始稳定在 {current_position}步")
|
||
elif time.time() - stable_start_time >= position_stable_time:
|
||
# 位置稳定超过指定时间，认为已到达限位
|
||
self.emergency_stop(axis)
|
||
time.sleep(0.5)
|
||
|
||
final_position = current_position
|
||
limit_detected = True
|
||
logger.info(f"{axis.name}轴位置稳定{position_stable_time}秒，假设已到达限位开关，位置: {final_position}步")
|
||
break
|
||
else:
|
||
# 位置发生变化，重置稳定计时
|
||
stable_start_time = None
|
||
logger.debug(f"{axis.name}轴位置变化: {last_position} -> {current_position}")
|
||
|
||
last_position = current_position
|
||
time.sleep(check_interval)
|
||
|
||
# 超时处理
|
||
if not limit_detected:
|
||
logger.warning(f"{axis.name}轴回零超时({self.machine_config.homing_timeout}秒)，强制停止")
|
||
self.emergency_stop(axis)
|
||
time.sleep(0.5)
|
||
|
||
# 获取当前位置作为机械原点
|
||
try:
|
||
status = self.get_motor_status(axis)
|
||
final_position = status.steps
|
||
logger.info(f"{axis.name}轴超时后位置: {final_position}步")
|
||
except Exception as e:
|
||
logger.error(f"获取{axis.name}轴位置失败: {e}")
|
||
return False
|
||
|
||
# 记录机械原点位置
|
||
self.coordinate_origin.machine_origin_steps[axis.name.lower()] = final_position
|
||
|
||
# 从限位开关退出安全距离
|
||
try:
|
||
clearance_steps = self.mm_to_steps(axis, self.machine_config.safe_clearance)
|
||
safe_position = final_position + (clearance_steps * -direction) # 反方向退出
|
||
|
||
if not self.move_to_position(axis, safe_position,
|
||
self.ms_to_rpm(axis, speed_)):
|
||
logger.warning(f"{axis.name}轴无法退出到安全位置")
|
||
else:
|
||
self.wait_for_completion(axis, 10.0)
|
||
logger.info(f"{axis.name}轴已退出到安全位置: {safe_position}步")
|
||
except Exception as e:
|
||
logger.warning(f"{axis.name}轴退出安全位置时出错: {e}")
|
||
|
||
status_msg = "限位检测成功" if limit_detected else "超时假设成功"
|
||
logger.info(f"{axis.name}轴回零完成 ({status_msg})，机械原点: {final_position}步")
|
||
return True
|
||
|
||
except Exception as e:
|
||
logger.error(f"{axis.name}轴回零失败: {e}")
|
||
self.emergency_stop(axis)
|
||
return False
|
||
|
||
def home_all_axes(self, sequence: list = None) -> bool:
|
||
"""
|
||
全轴回零 (液体处理工作站安全回零)
|
||
|
||
液体处理工作站回零策略：
|
||
1. Z轴必须首先回零，避免与容器、试管架等碰撞
|
||
2. 然后XY轴回零，确保移动路径安全
|
||
3. 严格按照Z->X->Y顺序执行，不允许更改
|
||
|
||
Args:
|
||
sequence: 回零顺序，液体处理工作站固定为Z->X->Y，不建议修改
|
||
|
||
Returns:
|
||
bool: 全轴回零是否成功
|
||
"""
|
||
if not self.is_connected:
|
||
logger.error("设备未连接，无法执行回零操作")
|
||
return False
|
||
|
||
# 液体处理工作站安全回零序列：Z轴绝对优先
|
||
safe_sequence = [MotorAxis.Z, MotorAxis.X, MotorAxis.Y]
|
||
|
||
if sequence is not None and sequence != safe_sequence:
|
||
logger.warning(f"液体处理工作站不建议修改回零序列，使用安全序列: {[axis.name for axis in safe_sequence]}")
|
||
|
||
sequence = safe_sequence # 强制使用安全序列
|
||
|
||
logger.info("开始全轴回零")
|
||
|
||
try:
|
||
for axis in sequence:
|
||
if not self.home_axis(axis):
|
||
logger.error(f"全轴回零失败，停止在{axis.name}轴")
|
||
return False
|
||
# 轴间等待时间
|
||
time.sleep(0.5)
|
||
|
||
# 标记为已回零
|
||
self.coordinate_origin.is_homed = True
|
||
self._save_coordinate_origin()
|
||
|
||
logger.info("全轴回零完成")
|
||
return True
|
||
|
||
except Exception as e:
|
||
logger.error(f"全轴回零异常: {e}")
|
||
return False
|
||
|
||
def set_work_origin_here(self) -> bool:
|
||
"""将当前位置设置为工作原点"""
|
||
if not self.is_connected:
|
||
logger.error("设备未连接，无法设置工作原点")
|
||
return False
|
||
|
||
try:
|
||
if not self.coordinate_origin.is_homed:
|
||
logger.warning("建议先执行回零操作再设置工作原点")
|
||
|
||
# 获取当前各轴位置
|
||
positions = self.get_all_positions()
|
||
|
||
machine_steps = {
|
||
'x': self.mm_to_steps(MotorAxis.X, self.machine_config.reference_distance['x']),
|
||
'y': self.mm_to_steps(MotorAxis.Y, self.machine_config.reference_distance['y']),
|
||
'z': self.mm_to_steps(MotorAxis.Z, self.machine_config.reference_distance['z'])
|
||
}
|
||
|
||
for axis in MotorAxis:
|
||
axis_name = axis.name.lower()
|
||
current_steps = positions[axis].steps
|
||
self.coordinate_origin.work_origin_steps[axis_name] = current_steps + machine_steps[axis_name] - self.coordinate_origin.machine_origin_steps[axis_name]
|
||
|
||
logger.info(f"{axis.name}轴工作原点设置为: {current_steps + machine_steps[axis_name] - self.coordinate_origin.machine_origin_steps[axis_name]}步 "
|
||
f"({self.steps_to_mm(axis, current_steps + machine_steps[axis_name] - self.coordinate_origin.machine_origin_steps[axis_name]):.2f}mm)")
|
||
|
||
self._save_coordinate_origin()
|
||
logger.info("工作原点设置完成")
|
||
return True
|
||
|
||
except Exception as e:
|
||
logger.error(f"设置工作原点失败: {e}")
|
||
return False
|
||
|
||
def ms_to_rpm(self, axis: MotorAxis, velocity_mms: float) -> int:
|
||
"""
|
||
将速度从米/秒(m/s)转换为转速(rpm)
|
||
|
||
Args:
|
||
axis: 电机轴
|
||
velocity_ms: 速度（米/秒）
|
||
|
||
Returns:
|
||
转速（rpm）
|
||
"""
|
||
# 获取每转的行程（mm）
|
||
if axis == MotorAxis.X:
|
||
lead_mm = 80.0
|
||
elif axis == MotorAxis.Y:
|
||
lead_mm = 80.0
|
||
elif axis == MotorAxis.Z:
|
||
lead_mm = 5.0
|
||
else:
|
||
raise ValueError(f"未知轴: {axis}")
|
||
|
||
# mm/s -> rps
|
||
rps = velocity_mms / lead_mm
|
||
# rps -> rpm
|
||
rpm = int(rps * 60.0)
|
||
return min(max(rpm, 0), 150)
|
||
|
||
|
||
# ==================== 高级运动控制方法 ====================
|
||
|
||
def move_to_work_coord_safe(self, x: float = None, y: float = None, z: float = None,
|
||
speed: float = None, acceleration: int = None) -> bool:
|
||
"""
|
||
安全移动到工作坐标系指定位置 (液体处理工作站专用)
|
||
移动策略：Z轴先上升到安全高度 -> XY轴移动到目标位置 -> Z轴下降到目标位置
|
||
|
||
Args:
|
||
x, y, z: 工作坐标系下的目标位置 (mm)
|
||
speed: 运动速度 (m/s)
|
||
acceleration: 加速度 (rpm/s)
|
||
|
||
Returns:
|
||
bool: 移动是否成功
|
||
"""
|
||
if not self.is_connected:
|
||
logger.error("设备未连接，无法执行移动操作")
|
||
return False
|
||
|
||
try:
|
||
# 检查坐标系是否已设置
|
||
if not self.coordinate_origin.work_origin_steps:
|
||
raise CoordinateSystemError("工作原点未设置，请先调用set_work_origin_here()")
|
||
|
||
# 检查行程限制
|
||
# self.check_travel_limits(x, y, z)
|
||
|
||
# 设置运动参数
|
||
speed = speed or self.machine_config.default_speed
|
||
acceleration = acceleration or self.machine_config.default_acceleration
|
||
|
||
xy_success = True
|
||
# 步骤1: Z轴先上升到安全高度
|
||
|
||
machine_steps = self.work_to_machine_steps(x, y, z)
|
||
|
||
if z is not None and (x is not None or y is not None):
|
||
safe_z_steps = self.work_to_machine_steps(None, None, self.machine_config.safe_z_height)
|
||
if not self.move_to_position(MotorAxis.Z, safe_z_steps['z'], self.ms_to_rpm(MotorAxis.Z, speed), acceleration):
|
||
logger.error("Z轴上升到安全高度失败")
|
||
return False
|
||
logger.info(f"Z轴上升到安全高度: {self.machine_config.safe_z_height} mm")
|
||
|
||
# 等待Z轴移动完成
|
||
self.wait_for_completion(MotorAxis.Z, 10.0)
|
||
|
||
# 步骤2: XY轴移动到目标位置
|
||
if x is not None:
|
||
if not self.move_to_position(MotorAxis.X, machine_steps['x'], self.ms_to_rpm(MotorAxis.X, speed), acceleration):
|
||
xy_success = False
|
||
if y is not None:
|
||
if not self.move_to_position(MotorAxis.Y, machine_steps['y'], self.ms_to_rpm(MotorAxis.Y, speed), acceleration):
|
||
xy_success = False
|
||
|
||
if not xy_success:
|
||
logger.error("XY轴移动失败")
|
||
return False
|
||
|
||
|
||
if x is not None or y is not None:
|
||
logger.info(f"XY轴移动到目标位置: X:{x} Y:{y} mm")
|
||
# 等待XY轴移动完成
|
||
if x is not None:
|
||
self.wait_for_completion(MotorAxis.X, 10.0)
|
||
if y is not None:
|
||
self.wait_for_completion(MotorAxis.Y, 10.0)
|
||
|
||
# 步骤3: Z轴下降到目标位置
|
||
if z is not None:
|
||
if not self.move_to_position(MotorAxis.Z, machine_steps['z'], self.ms_to_rpm(MotorAxis.Z, speed), acceleration):
|
||
logger.error("Z轴下降到目标位置失败")
|
||
return False
|
||
logger.info(f"Z轴下降到目标位置: {z} mm")
|
||
self.wait_for_completion(MotorAxis.Z, 10.0)
|
||
|
||
logger.info(f"安全移动到工作坐标 X:{x} Y:{y} Z:{z} (mm) 完成")
|
||
return True
|
||
|
||
except Exception as e:
|
||
logger.error(f"安全移动失败: {e}")
|
||
return False
|
||
|
||
def move_to_work_coord(self, x: float = None, y: float = None, z: float = None,
|
||
speed: int = None, acceleration: int = None) -> bool:
|
||
"""
|
||
移动到工作坐标 (已禁用)
|
||
|
||
此方法已被禁用，请使用 move_to_work_coord_safe() 方法。
|
||
|
||
Raises:
|
||
RuntimeError: 方法已禁用
|
||
"""
|
||
error_msg = "Method disabled, use move_to_work_coord_safe instead"
|
||
logger.error(error_msg)
|
||
raise RuntimeError(error_msg)
|
||
|
||
def move_relative_work_coord(self, dx: float = 0, dy: float = 0, dz: float = 0,
|
||
speed: float = None, acceleration: int = None) -> bool:
|
||
"""
|
||
相对当前位置移动
|
||
|
||
Args:
|
||
dx, dy, dz: 相对移动距离 (mm)
|
||
speed: 运动速度 (m/s)
|
||
acceleration: 加速度 (rpm/s)
|
||
|
||
Returns:
|
||
bool: 移动是否成功
|
||
"""
|
||
if not self.is_connected:
|
||
logger.error("设备未连接，无法执行移动操作")
|
||
return False
|
||
|
||
try:
|
||
# 获取当前工作坐标
|
||
current_work = self.get_current_work_coords()
|
||
|
||
# 计算目标坐标
|
||
target_x = current_work['x'] + dx if dx != 0 else None
|
||
target_y = current_work['y'] + dy if dy != 0 else None
|
||
target_z = current_work['z'] + dz if dz != 0 else None
|
||
|
||
return self.move_to_work_coord_safe(x=target_x, y=target_y, z=target_z, speed=speed, acceleration=acceleration)
|
||
|
||
except Exception as e:
|
||
logger.error(f"相对移动失败: {e}")
|
||
return False
|
||
|
||
def get_current_work_coords(self) -> Dict[str, float]:
|
||
"""获取当前工作坐标"""
|
||
if not self.is_connected:
|
||
logger.error("设备未连接，无法获取当前坐标")
|
||
return {'x': 0.0, 'y': 0.0, 'z': 0.0}
|
||
|
||
try:
|
||
# 获取当前机械坐标
|
||
positions = self.get_all_positions()
|
||
machine_steps = {axis.name.lower(): pos.steps for axis, pos in positions.items()}
|
||
|
||
# 转换为工作坐标
|
||
return self.machine_to_work_coords(machine_steps)
|
||
|
||
except Exception as e:
|
||
logger.error(f"获取工作坐标失败: {e}")
|
||
return {'x': 0.0, 'y': 0.0, 'z': 0.0}
|
||
|
||
def get_current_position_mm(self) -> Dict[str, float]:
|
||
"""获取当前位置坐标（毫米单位）"""
|
||
return self.get_current_work_coords()
|
||
|
||
def wait_for_move_completion(self, timeout: float = 30.0) -> bool:
|
||
"""等待所有轴运动完成"""
|
||
if not self.is_connected:
|
||
return False
|
||
|
||
for axis in MotorAxis:
|
||
if not self.wait_for_completion(axis, timeout):
|
||
return False
|
||
return True
|
||
|
||
# ==================== 系统状态和配置方法 ====================
|
||
|
||
def get_system_status(self) -> Dict:
|
||
"""获取系统状态信息"""
|
||
status = {
|
||
"connection": {
|
||
"is_connected": self.is_connected,
|
||
"port": self.port,
|
||
"baudrate": self.baudrate
|
||
},
|
||
"coordinate_system": {
|
||
"is_homed": self.coordinate_origin.is_homed,
|
||
"machine_origin": self.coordinate_origin.machine_origin_steps,
|
||
"work_origin": self.coordinate_origin.work_origin_steps,
|
||
"timestamp": self.coordinate_origin.timestamp
|
||
},
|
||
"machine_config": asdict(self.machine_config),
|
||
"current_position": {}
|
||
}
|
||
|
||
if self.is_connected:
|
||
try:
|
||
# 获取当前位置
|
||
positions = self.get_all_positions()
|
||
for axis, pos in positions.items():
|
||
axis_name = axis.name.lower()
|
||
status["current_position"][axis_name] = {
|
||
"steps": pos.steps,
|
||
"mm": self.steps_to_mm(axis, pos.steps),
|
||
"status": pos.status.name if hasattr(pos.status, 'name') else str(pos.status)
|
||
}
|
||
|
||
# 获取工作坐标
|
||
work_coords = self.get_current_work_coords()
|
||
status["current_work_coords"] = work_coords
|
||
|
||
except Exception as e:
|
||
status["position_error"] = str(e)
|
||
|
||
return status
|
||
|
||
def update_machine_config(self, **kwargs):
|
||
"""更新机械配置参数"""
|
||
for key, value in kwargs.items():
|
||
if hasattr(self.machine_config, key):
|
||
setattr(self.machine_config, key, value)
|
||
logger.info(f"更新配置参数 {key}: {value}")
|
||
else:
|
||
logger.warning(f"未知配置参数: {key}")
|
||
|
||
# 保存配置
|
||
self._save_config()
|
||
|
||
def reset_coordinate_system(self):
|
||
"""重置坐标系统"""
|
||
self.coordinate_origin = CoordinateOrigin()
|
||
self._save_coordinate_origin()
|
||
logger.info("坐标系统已重置")
|
||
|
||
def __enter__(self):
|
||
"""上下文管理器入口"""
|
||
return self
|
||
|
||
def __exit__(self, exc_type, exc_val, exc_tb):
|
||
"""上下文管理器出口"""
|
||
self.disconnect_device()
|
||
|
||
|
||
def interactive_control(controller: XYZController):
|
||
"""
|
||
交互式控制模式
|
||
|
||
Args:
|
||
controller: 已连接的控制器实例
|
||
"""
|
||
print("\n" + "="*60)
|
||
print("进入交互式控制模式")
|
||
print("="*60)
|
||
|
||
# 显示当前状态
|
||
def show_status():
|
||
try:
|
||
current_pos = controller.get_current_position_mm()
|
||
print(f"\n当前位置: X={current_pos['x']:.2f}mm, Y={current_pos['y']:.2f}mm, Z={current_pos['z']:.2f}mm")
|
||
except Exception as e:
|
||
print(f"获取位置失败: {e}")
|
||
|
||
# 显示帮助信息
|
||
def show_help():
|
||
print("\n可用命令:")
|
||
print(" move <轴> <距离> - 相对移动，例: move x 10.5")
|
||
print(" goto <x> <y> <z> - 绝对移动到指定坐标，例: goto 10 20 5")
|
||
print(" home [轴] - 回零操作，例: home 或 home x")
|
||
print(" origin - 设置当前位置为工作原点")
|
||
print(" status - 显示当前状态")
|
||
print(" speed <速度> - 设置运动速度(mm/s)，例: speed 2000")
|
||
print(" limits - 显示行程限制")
|
||
print(" config - 显示机械配置")
|
||
print(" help - 显示此帮助信息")
|
||
print(" quit/exit - 退出交互模式")
|
||
print("\n提示:")
|
||
print(" - 轴名称: x, y, z")
|
||
print(" - 距离单位: 毫米(mm)")
|
||
print(" - 正数向正方向移动，负数向负方向移动")
|
||
|
||
|
||
|
||
# 安全回零操作
|
||
def safe_homing():
|
||
print("\n系统安全初始化...")
|
||
print("为确保操作安全，系统将执行回零操作")
|
||
print("提示: 已安装限位开关，超时后将假设回零成功")
|
||
|
||
# 询问用户是否继续
|
||
while True:
|
||
user_choice = input("是否继续执行回零操作? (y/n/skip): ").strip().lower()
|
||
if user_choice in ['y', 'yes', '是']:
|
||
print("\n开始执行全轴回零...")
|
||
print("回零过程可能需要一些时间，请耐心等待...")
|
||
|
||
# 执行回零操作
|
||
homing_success = controller.home_all_axes()
|
||
|
||
if homing_success:
|
||
print("回零操作完成，系统已就绪")
|
||
# 设置当前位置为工作原点
|
||
|
||
# if controller.set_work_origin_here():
|
||
# print("工作原点已设置为回零位置")
|
||
# else:
|
||
# print("工作原点设置失败，但可以继续操作")
|
||
return True
|
||
else:
|
||
print("回零操作失败")
|
||
print("这可能是由于通信问题，但限位开关应该已经起作用")
|
||
|
||
# 询问是否继续
|
||
retry_choice = input("是否仍要继续操作? (y/n): ").strip().lower()
|
||
if retry_choice in ['y', 'yes', '是']:
|
||
print("继续操作，请手动确认设备位置安全")
|
||
return True
|
||
else:
|
||
return False
|
||
|
||
elif user_choice in ['n', 'no', '否']:
|
||
print("用户取消回零操作，退出交互模式")
|
||
return False
|
||
elif user_choice in ['skip', 's', '跳过']:
|
||
print("跳过回零操作，请注意安全！")
|
||
print("建议在开始操作前手动执行 'home' 命令")
|
||
return True
|
||
else:
|
||
print("请输入 y(继续)/n(取消)/skip(跳过)")
|
||
|
||
# 安全回原点操作
|
||
def safe_return_home():
|
||
print("\n系统安全关闭...")
|
||
print("正在将所有轴移动到安全位置...")
|
||
|
||
try:
|
||
# 移动到工作原点 (0,0,0) - 使用安全移动方法
|
||
if controller.move_to_work_coord_safe(0, 0, 0, speed=50):
|
||
print("已安全返回工作原点")
|
||
show_status()
|
||
else:
|
||
print("返回原点失败，请手动检查设备位置")
|
||
except Exception as e:
|
||
print(f"返回原点时出错: {e}")
|
||
|
||
# 当前运动速度
|
||
current_speed = controller.machine_config.default_speed
|
||
|
||
try:
|
||
# 1. 首先执行安全回零
|
||
if not safe_homing():
|
||
return
|
||
|
||
# 2. 显示初始状态和帮助
|
||
show_status()
|
||
show_help()
|
||
|
||
while True:
|
||
try:
|
||
# 获取用户输入
|
||
user_input = input("\n请输入命令 (输入 help 查看帮助): ").strip().lower()
|
||
|
||
if not user_input:
|
||
continue
|
||
|
||
# 解析命令
|
||
parts = user_input.split()
|
||
command = parts[0]
|
||
|
||
if command in ['quit', 'exit', 'q']:
|
||
print("准备退出交互模式...")
|
||
# 执行安全回原点操作
|
||
safe_return_home()
|
||
print("退出交互模式")
|
||
break
|
||
|
||
elif command == 'help' or command == 'h':
|
||
show_help()
|
||
|
||
elif command == 'status' or command == 's':
|
||
show_status()
|
||
print(f"当前速度: {current_speed} m/s")
|
||
print(f"是否已回零: {controller.coordinate_origin.is_homed}")
|
||
|
||
elif command == 'move' or command == 'm':
|
||
if len(parts) != 3:
|
||
print("格式错误，正确格式: move <轴> <距离>")
|
||
print(" 例如: move x 10.5")
|
||
continue
|
||
|
||
axis = parts[1].lower()
|
||
try:
|
||
distance = float(parts[2])
|
||
except ValueError:
|
||
print("距离必须是数字")
|
||
continue
|
||
|
||
if axis not in ['x', 'y', 'z']:
|
||
print("轴名称必须是 x, y 或 z")
|
||
continue
|
||
|
||
print(f"{axis.upper()}轴移动 {distance:+.2f}mm...")
|
||
|
||
# 执行移动
|
||
kwargs = {f'd{axis}': distance, 'speed': current_speed}
|
||
if controller.move_relative_work_coord(**kwargs):
|
||
print(f"{axis.upper()}轴移动完成")
|
||
show_status()
|
||
else:
|
||
print(f"{axis.upper()}轴移动失败")
|
||
|
||
elif command == 'goto' or command == 'g':
|
||
if len(parts) != 4:
|
||
print("格式错误，正确格式: goto <x> <y> <z>")
|
||
print(" 例如: goto 10 20 5")
|
||
continue
|
||
|
||
try:
|
||
x = float(parts[1])
|
||
y = float(parts[2])
|
||
z = float(parts[3])
|
||
except ValueError:
|
||
print("坐标必须是数字")
|
||
continue
|
||
|
||
print(f"移动到坐标 ({x}, {y}, {z})...")
|
||
print("使用安全移动策略: Z轴先上升 → XY移动 → Z轴下降")
|
||
|
||
if controller.move_to_work_coord_safe(x, y, z, speed=current_speed):
|
||
print("安全移动到目标位置完成")
|
||
show_status()
|
||
else:
|
||
print("移动失败")
|
||
|
||
elif command == 'home':
|
||
if len(parts) == 1:
|
||
# 全轴回零
|
||
print("开始全轴回零...")
|
||
if controller.home_all_axes():
|
||
print("全轴回零完成")
|
||
show_status()
|
||
else:
|
||
print("回零失败")
|
||
elif len(parts) == 2:
|
||
# 单轴回零
|
||
axis_name = parts[1].lower()
|
||
if axis_name not in ['x', 'y', 'z']:
|
||
print("轴名称必须是 x, y 或 z")
|
||
continue
|
||
|
||
axis = MotorAxis[axis_name.upper()]
|
||
print(f"{axis_name.upper()}轴回零...")
|
||
|
||
if controller.home_axis(axis):
|
||
print(f"{axis_name.upper()}轴回零完成")
|
||
show_status()
|
||
else:
|
||
print(f"{axis_name.upper()}轴回零失败")
|
||
else:
|
||
print("格式错误，正确格式: home 或 home <轴>")
|
||
|
||
elif command == 'origin' or command == 'o':
|
||
print("设置当前位置为工作原点...")
|
||
if controller.set_work_origin_here():
|
||
print("工作原点设置完成")
|
||
show_status()
|
||
else:
|
||
print("工作原点设置失败")
|
||
|
||
elif command == 'speed':
|
||
if len(parts) != 2:
|
||
print("格式错误，正确格式: speed <速度>")
|
||
print(" 例如: speed 200")
|
||
continue
|
||
|
||
try:
|
||
new_speed = int(parts[1])
|
||
if new_speed <= 0:
|
||
print("速度必须大于0")
|
||
continue
|
||
if new_speed > 500:
|
||
print("速度不能超过500 mm/s")
|
||
continue
|
||
|
||
current_speed = new_speed
|
||
print(f"运动速度设置为: {current_speed} mm/s")
|
||
|
||
except ValueError:
|
||
print("速度必须是整数")
|
||
|
||
elif command == 'limits' or command == 'l':
|
||
config = controller.machine_config
|
||
print("\n行程限制:")
|
||
print(f" X轴: 0 ~ {config.max_travel_x} mm")
|
||
print(f" Y轴: 0 ~ {config.max_travel_y} mm")
|
||
print(f" Z轴: 0 ~ {config.max_travel_z} mm")
|
||
|
||
elif command == 'config' or command == 'c':
|
||
config = controller.machine_config
|
||
print("\n机械配置:")
|
||
print(f" X轴步距: {config.steps_per_mm_x:.1f} 步/mm")
|
||
print(f" Y轴步距: {config.steps_per_mm_y:.1f} 步/mm")
|
||
print(f" Z轴步距: {config.steps_per_mm_z:.1f} 步/mm")
|
||
print(f" 回零速度: {config.homing_speed} mm/s")
|
||
print(f" 默认速度: {config.default_speed} mm/s")
|
||
print(f" 安全间隙: {config.safe_clearance} mm")
|
||
|
||
else:
|
||
print(f"未知命令: {command}")
|
||
print("输入 help 查看可用命令")
|
||
|
||
except KeyboardInterrupt:
|
||
print("\n\n用户中断，退出交互模式")
|
||
break
|
||
except Exception as e:
|
||
print(f"命令执行错误: {e}")
|
||
print("输入 help 查看正确的命令格式")
|
||
|
||
finally:
|
||
# 确保正确断开连接
|
||
try:
|
||
controller.disconnect_device()
|
||
print("设备连接已断开")
|
||
except Exception as e:
|
||
print(f"断开连接时出错: {e}")
|
||
|
||
|
||
def run_tests():
|
||
"""运行测试函数"""
|
||
print("=== XYZ控制器测试 ===")
|
||
|
||
# 1. 测试机械配置
|
||
print("\n1. 测试机械配置")
|
||
config = MachineConfig(
|
||
steps_per_mm_x=204.8, # 16384步/圈 ÷ 80mm导程
|
||
steps_per_mm_y=204.8, # 16384步/圈 ÷ 80mm导程
|
||
steps_per_mm_z=3276.8, # 16384步/圈 ÷ 5mm导程
|
||
max_travel_x=340.0,
|
||
max_travel_y=250.0,
|
||
max_travel_z=160.0,
|
||
homing_speed=50,
|
||
default_speed=50
|
||
)
|
||
print(f"X轴步距: {config.steps_per_mm_x} 步/mm")
|
||
print(f"Y轴步距: {config.steps_per_mm_y} 步/mm")
|
||
print(f"Z轴步距: {config.steps_per_mm_z} 步/mm")
|
||
print(f"行程限制: X={config.max_travel_x}mm, Y={config.max_travel_y}mm, Z={config.max_travel_z}mm")
|
||
|
||
# 2. 测试坐标原点数据结构
|
||
print("\n2. 测试坐标原点数据结构")
|
||
origin = CoordinateOrigin()
|
||
print(f"初始状态: 已回零={origin.is_homed}")
|
||
print(f"机械原点: {origin.machine_origin_steps}")
|
||
print(f"工作原点: {origin.work_origin_steps}")
|
||
|
||
# 设置示例数据
|
||
origin.machine_origin_steps = {'x': 0, 'y': 0, 'z': 0}
|
||
origin.work_origin_steps = {'x': 16384, 'y': 16384, 'z': 13107} # 5mm, 5mm, 2mm (基于16384步/圈)
|
||
origin.is_homed = True
|
||
origin.timestamp = "2024-09-26 12:00:00"
|
||
print(f"设置后: 已回零={origin.is_homed}")
|
||
print(f"机械原点: {origin.machine_origin_steps}")
|
||
print(f"工作原点: {origin.work_origin_steps}")
|
||
|
||
# 3. 测试离线功能
|
||
print("\n3. 测试离线功能")
|
||
|
||
# 创建离线控制器（不自动连接）
|
||
offline_controller = XYZController(
|
||
port='/dev/ttyUSB_CH340',
|
||
machine_config=config,
|
||
auto_connect=False
|
||
)
|
||
|
||
# 测试单位转换
|
||
print("\n单位转换测试:")
|
||
test_distances = [1.0, 5.0, 10.0, 25.5]
|
||
for distance in test_distances:
|
||
x_steps = offline_controller.mm_to_steps(MotorAxis.X, distance)
|
||
y_steps = offline_controller.mm_to_steps(MotorAxis.Y, distance)
|
||
z_steps = offline_controller.mm_to_steps(MotorAxis.Z, distance)
|
||
print(f"{distance}mm -> X:{x_steps}步, Y:{y_steps}步, Z:{z_steps}步")
|
||
|
||
# 反向转换验证
|
||
x_mm = offline_controller.steps_to_mm(MotorAxis.X, x_steps)
|
||
y_mm = offline_controller.steps_to_mm(MotorAxis.Y, y_steps)
|
||
z_mm = offline_controller.steps_to_mm(MotorAxis.Z, z_steps)
|
||
print(f"反向转换: X:{x_mm:.2f}mm, Y:{y_mm:.2f}mm, Z:{z_mm:.2f}mm")
|
||
|
||
# 测试坐标系转换
|
||
print("\n坐标系转换测试:")
|
||
offline_controller.coordinate_origin = origin # 使用示例原点
|
||
work_coords = [(0, 0, 0), (10, 15, 5), (50, 30, 20)]
|
||
|
||
for x, y, z in work_coords:
|
||
try:
|
||
machine_steps = offline_controller.work_to_machine_steps(x, y, z)
|
||
print(f"工作坐标 ({x}, {y}, {z}) -> 机械步数 {machine_steps}")
|
||
|
||
# 反向转换验证
|
||
work_coords_back = offline_controller.machine_to_work_coords(machine_steps)
|
||
print(f"反向转换: ({work_coords_back['x']:.2f}, {work_coords_back['y']:.2f}, {work_coords_back['z']:.2f})")
|
||
except Exception as e:
|
||
print(f"转换失败: {e}")
|
||
|
||
# 测试行程限制检查
|
||
print("\n行程限制检查测试:")
|
||
test_positions = [
|
||
(50, 50, 25, "正常位置"),
|
||
(250, 50, 25, "X轴超限"),
|
||
(50, 350, 25, "Y轴超限"),
|
||
(50, 50, 150, "Z轴超限"),
|
||
(-10, 50, 25, "X轴负超限"),
|
||
(50, -10, 25, "Y轴负超限"),
|
||
(50, 50, -5, "Z轴负超限")
|
||
]
|
||
|
||
# for x, y, z, desc in test_positions:
|
||
# try:
|
||
# offline_controller.check_travel_limits(x, y, z)
|
||
# print(f"{desc} ({x}, {y}, {z}): 有效")
|
||
# except CoordinateSystemError as e:
|
||
# print(f"{desc} ({x}, {y}, {z}): 超限 - {e}")
|
||
|
||
print("\n=== 离线功能测试完成 ===")
|
||
|
||
# 4. 硬件连接测试
|
||
print("\n4. 硬件连接测试")
|
||
print("尝试连接真实设备...")
|
||
|
||
# 可能的串口列表
|
||
possible_ports = [
|
||
'/dev/ttyUSB_CH340'
|
||
]
|
||
|
||
connected_controller = None
|
||
|
||
for port in possible_ports:
|
||
try:
|
||
print(f"尝试连接端口: {port}")
|
||
controller = XYZController(
|
||
port=port,
|
||
machine_config=config,
|
||
auto_connect=True
|
||
)
|
||
|
||
if controller.is_connected:
|
||
print(f"成功连接到 {port}")
|
||
connected_controller = controller
|
||
|
||
# 获取系统状态
|
||
status = controller.get_system_status()
|
||
print("\n系统状态:")
|
||
print(f" 连接状态: {status['connection']['is_connected']}")
|
||
print(f" 是否已回零: {status['coordinate_system']['is_homed']}")
|
||
|
||
if 'current_position' in status:
|
||
print(" 当前位置:")
|
||
for axis, pos_info in status['current_position'].items():
|
||
print(f" {axis.upper()}轴: {pos_info['steps']}步 ({pos_info['mm']:.2f}mm)")
|
||
|
||
# 测试基本移动功能
|
||
print("\n测试基本移动功能:")
|
||
try:
|
||
# 获取当前位置
|
||
current_pos = controller.get_current_position_mm()
|
||
print(f"当前工作坐标: {current_pos}")
|
||
|
||
# 小幅移动测试
|
||
print("执行小幅移动测试 (X+1mm)...")
|
||
if controller.move_relative_work_coord(dx=1.0, speed=500):
|
||
print("移动成功")
|
||
time.sleep(1)
|
||
new_pos = controller.get_current_position_mm()
|
||
print(f"移动后坐标: {new_pos}")
|
||
else:
|
||
print("移动失败")
|
||
|
||
except Exception as e:
|
||
print(f"移动测试失败: {e}")
|
||
|
||
break
|
||
|
||
except Exception as e:
|
||
print(f"连接 {port} 失败: {e}")
|
||
continue
|
||
|
||
if not connected_controller:
|
||
print("未找到可用的设备端口")
|
||
print("请检查:")
|
||
print(" 1. 设备是否正确连接")
|
||
print(" 2. 串口端口是否正确")
|
||
print(" 3. 设备驱动是否安装")
|
||
else:
|
||
# 进入交互式控制模式
|
||
interactive_control(connected_controller)
|
||
|
||
print("\n=== XYZ控制器测试完成 ===")
|
||
|
||
|
||
# ==================== 测试和示例代码 ====================
|
||
if __name__ == "__main__":
|
||
run_tests()
|
||
# xyz_controller = XYZController(port='/dev/ttyUSB_CH340', auto_connect=True)
|
||
# # xyz_controller.stop_all_axes()
|
||
# xyz_controller.connect_device()
|
||
# time.sleep(1)
|
||
# xyz_controller.home_all_axes()
|