forked from huangxin/ailai
410 lines
16 KiB
Python
410 lines
16 KiB
Python
#!/usr/bin/env python
|
||
# -*- coding: utf-8 -*-
|
||
import socket
|
||
import binascii
|
||
import time
|
||
import threading
|
||
import logging
|
||
from PySide6.QtCore import Signal, QObject
|
||
import numpy as np
|
||
|
||
|
||
class RelayController:
|
||
need_origin_signal = Signal(str)
|
||
take_no_photo_sigal = Signal()
|
||
update_detect_image = Signal(np.ndarray)
|
||
log_signal = Signal(int, str)
|
||
|
||
def __init__(self, host="192.168.0.18", port=50000):
|
||
# ===================== 全局线程延时参数 =====================
|
||
self.sensor1_loop_delay = 0.2 # SENSOR1 线程轮询间隔（秒）
|
||
self.sensor1_error_delay = 1.0 # SENSOR1 出错时延时（秒）
|
||
self.sensor1_post_action_delay = 0.2 # SENSOR1 每次循环后延时（秒）
|
||
|
||
self.sensor2_loop_delay = 0.5 # SENSOR2 线程轮询间隔（秒）
|
||
self.sensor2_error_delay = 0.5 # SENSOR2 出错时延时（秒）
|
||
self.sensor2_post_action_delay = 0.2 # SENSOR2 每次循环后延时（秒）
|
||
# ===================== 全局动作延时参数 =====================
|
||
self.delay_conveyor = (
|
||
0.5 # 传送带开/关动作延时（一半时间，我在控制程序和线程都加了一样的延时）
|
||
)
|
||
self.delay_pusher = 0.05 # 推板开/关动作延时
|
||
self.delay_clamp = 0.5 # 夹爪动作延时
|
||
self.delay_after_pusher = 3.0 # 推板推出后到重启传动带时间
|
||
|
||
# ===================== 传感器稳定检测参数 =====================
|
||
self.sensor_stable_duration = 1.0 # 传感器状态稳定检测时间（秒）
|
||
self.sensor_max_attempts = 3 # 连续检测次数达到此值判定有效
|
||
self.sensor1_debounce_time = 1.0 # 传感器1防抖时间（秒）
|
||
|
||
# ===================== 网络与设备映射 =====================
|
||
self.host = host
|
||
self.port = port
|
||
|
||
self.CONVEYOR1 = "conveyor1"
|
||
self.PUSHER = "pusher"
|
||
self.CONVEYOR2 = "conveyor2"
|
||
self.CLAMP = "clamp"
|
||
self.PUSHER1 = "pusher1"
|
||
self.SENSOR1 = "sensor1"
|
||
self.SENSOR2 = "sensor2"
|
||
|
||
self.valve_commands = {
|
||
self.CONVEYOR1: {
|
||
"open": "00000000000601050000FF00",
|
||
"close": "000000000006010500000000",
|
||
},
|
||
self.PUSHER: {
|
||
"open": "00000000000601050001FF00",
|
||
"close": "000000000006010500010000",
|
||
},
|
||
self.CONVEYOR2: {
|
||
"open": "000100000006020620000012",
|
||
"close": "000100000006020620000001",
|
||
},
|
||
self.CLAMP: {
|
||
"open": "00000000000601050003FF00",
|
||
"close": "000000000006010500030000",
|
||
},
|
||
self.PUSHER1: {
|
||
"open": "00000000000601050004FF00",
|
||
"close": "000000000006010500040000",
|
||
},
|
||
}
|
||
|
||
self.read_status_command = {
|
||
"devices": "000000000006010100000008",
|
||
"sensors": "000000000006010200000008",
|
||
}
|
||
|
||
self.device_bit_map = {
|
||
self.CONVEYOR1: 0,
|
||
self.PUSHER: 1,
|
||
self.CONVEYOR2: 2,
|
||
self.CLAMP: 3,
|
||
self.PUSHER1: 4,
|
||
}
|
||
|
||
self.sensor_bit_map = {
|
||
self.SENSOR1: 0,
|
||
self.SENSOR2: 1,
|
||
}
|
||
|
||
self.device_name_map = {
|
||
self.CONVEYOR1: "传送带1",
|
||
self.PUSHER: "推板开",
|
||
self.CONVEYOR2: "传送带2",
|
||
self.CLAMP: "机械臂夹爪",
|
||
self.PUSHER1: "推板关",
|
||
}
|
||
|
||
self.sensor_name_map = {
|
||
self.SENSOR1: "位置传感器1",
|
||
self.SENSOR2: "位置传感器2",
|
||
}
|
||
|
||
# ===================== 状态控制变量 =====================
|
||
self._running = False
|
||
self._sensor1_thread = None
|
||
self._sensor2_thread = None
|
||
|
||
self.required_codes = {"0101", "0103"} # 有效状态码（传感器1）
|
||
self.required_codes_1 = {"0102", "0103"} # 有效状态码（传感器2）
|
||
|
||
self.sensor1_triggered = False
|
||
self.sensor1_last_time = 0
|
||
|
||
self.sensor2_ready = True
|
||
self.motor_stopped_by_sensor2 = False
|
||
|
||
# ===================== 基础通信方法 =====================
|
||
def send_command(self, command_hex, retry_count=2, source="unknown"):
|
||
if not self._running:
|
||
return
|
||
|
||
byte_data = binascii.unhexlify(command_hex)
|
||
for attempt in range(retry_count):
|
||
if not self._running:
|
||
return
|
||
|
||
try:
|
||
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock:
|
||
sock.settimeout(10)
|
||
sock.connect((self.host, self.port))
|
||
sock.send(byte_data)
|
||
response = sock.recv(1024)
|
||
hex_response = binascii.hexlify(response).decode("utf-8")
|
||
# if source == 'sensor':
|
||
# print(f"[传感器响应] {hex_response}")
|
||
# elif source == 'device':
|
||
# print(f"[设备控制响应] {hex_response}")
|
||
# else:
|
||
# print(f"[通信响应] {hex_response}")
|
||
return response
|
||
except Exception as e:
|
||
print(
|
||
f"通信错误 ({source}): {e}, 尝试重连... ({attempt + 1}/{retry_count})"
|
||
)
|
||
time.sleep(5)
|
||
self.trigger_alarm()
|
||
return None
|
||
|
||
def trigger_alarm(self):
|
||
print("警告：连续多次通信失败，请检查设备连接！")
|
||
|
||
# ===================== 状态读取方法 =====================
|
||
def get_all_device_status(self, command_type="devices"):
|
||
command = self.read_status_command.get(command_type)
|
||
if not command:
|
||
print(f"未知的读取类型: {command_type}")
|
||
return {}
|
||
|
||
source = "sensor" if command_type == "sensors" else "device"
|
||
response = self.send_command(command, source=source)
|
||
status_dict = {}
|
||
|
||
if response and len(response) >= 10:
|
||
status_byte = response[9]
|
||
status_bin = f"{status_byte:08b}"[::-1]
|
||
bit_map = (
|
||
self.device_bit_map
|
||
if command_type == "devices"
|
||
else self.sensor_bit_map
|
||
)
|
||
name_map = (
|
||
self.device_name_map
|
||
if command_type == "devices"
|
||
else self.sensor_name_map
|
||
)
|
||
|
||
for key, bit_index in bit_map.items():
|
||
status_dict[key] = status_bin[bit_index] == "1"
|
||
else:
|
||
print(f"[{command_type}] 读取状态失败或响应无效")
|
||
|
||
return status_dict
|
||
|
||
def get_all_sensor_responses(self, command_type="sensors"):
|
||
"""
|
||
获取所有传感器的原始 Modbus 响应字符串
|
||
示例：{'sensor1': '00000000000401020101', 'sensor2': '00000000000401020100'}
|
||
"""
|
||
command = self.read_status_command.get(command_type)
|
||
if not command:
|
||
print(f"未知的读取类型: {command_type}")
|
||
return {}
|
||
|
||
source = "sensor" if command_type == "sensors" else "device"
|
||
response = self.send_command(command, source=source)
|
||
responses = {}
|
||
|
||
if response and len(response) >= 10:
|
||
hex_response = binascii.hexlify(response).decode("utf-8")
|
||
print(f"[原始响应][{command_type}] {hex_response}")
|
||
|
||
# 假设传感器数据从第 9 字节开始，长度为 2 字节
|
||
for name, bit_index in self.sensor_bit_map.items():
|
||
offset = 9 + (bit_index // 8)
|
||
bit_pos = bit_index % 8
|
||
byte = response[offset]
|
||
status = (byte >> bit_pos) & 1
|
||
responses[name] = hex_response
|
||
else:
|
||
print(f"[{command_type}] 无法获取响应数据")
|
||
return responses
|
||
|
||
def parse_status_code(self, response):
|
||
"""
|
||
从 Modbus 响应字符串中提取状态码（后两位）
|
||
示例：00000000000401020101 -> '01'
|
||
"""
|
||
if isinstance(response, str) and len(response) >= 18:
|
||
return response[16:20]
|
||
return None
|
||
|
||
def is_valid_sensor_status(self, sensor_name):
|
||
stable_count = 0
|
||
for _ in range(int(self.sensor_stable_duration / self.sensor1_loop_delay)):
|
||
responses = self.get_all_sensor_responses("sensors")
|
||
response = responses.get(sensor_name)
|
||
|
||
if not response:
|
||
stable_count = 0
|
||
else:
|
||
status_code = self.parse_status_code(response)
|
||
if status_code in self.required_codes:
|
||
stable_count += 1
|
||
if stable_count >= self.sensor_max_attempts:
|
||
return True
|
||
else:
|
||
stable_count = 0
|
||
time.sleep(self.sensor1_loop_delay)
|
||
return False
|
||
|
||
def is_valid_sensor_status_1(self, sensor_name):
|
||
stable_count = 0
|
||
for _ in range(int(self.sensor_stable_duration / self.sensor2_loop_delay)):
|
||
responses = self.get_all_sensor_responses("sensors")
|
||
response = responses.get(sensor_name)
|
||
|
||
if not response:
|
||
print(f"[警告] 无法获取 {sensor_name} 的响应，尝试重试...")
|
||
stable_count = 0
|
||
else:
|
||
status_code = self.parse_status_code(response)
|
||
if status_code in self.required_codes_1:
|
||
stable_count += 1
|
||
if stable_count >= self.sensor_max_attempts:
|
||
return True
|
||
else:
|
||
stable_count = 0
|
||
time.sleep(self.sensor2_loop_delay)
|
||
return False
|
||
|
||
# ===================== 动作控制方法 =====================
|
||
def open(
|
||
self, conveyor1=False, pusher=False, conveyor2=False, clamp=False, pusher1=False
|
||
):
|
||
status = self.get_all_device_status()
|
||
if conveyor1 and not status.get(self.CONVEYOR1, False):
|
||
self.send_command(self.valve_commands[self.CONVEYOR1]["open"])
|
||
time.sleep(self.delay_conveyor)
|
||
if pusher and not status.get(self.PUSHER, False):
|
||
self.send_command(self.valve_commands[self.PUSHER]["open"])
|
||
time.sleep(self.delay_pusher)
|
||
if conveyor2 and not status.get(self.CONVEYOR2, False):
|
||
self.send_command(self.valve_commands[self.CONVEYOR2]["open"])
|
||
time.sleep(self.delay_conveyor)
|
||
if clamp and not status.get(self.CLAMP, False):
|
||
self.send_command(self.valve_commands[self.CLAMP]["open"])
|
||
time.sleep(self.delay_clamp)
|
||
if pusher1 and not status.get(self.PUSHER1, False):
|
||
self.send_command(self.valve_commands[self.PUSHER1]["open"])
|
||
time.sleep(self.delay_pusher)
|
||
|
||
def close(
|
||
self, conveyor1=False, pusher=False, conveyor2=False, clamp=False, pusher1=False
|
||
):
|
||
if conveyor1:
|
||
self.send_command(self.valve_commands[self.CONVEYOR1]["close"])
|
||
time.sleep(self.delay_conveyor)
|
||
if pusher:
|
||
self.send_command(self.valve_commands[self.PUSHER]["close"])
|
||
time.sleep(self.delay_pusher)
|
||
if conveyor2:
|
||
self.send_command(self.valve_commands[self.CONVEYOR2]["close"])
|
||
time.sleep(self.delay_conveyor)
|
||
if clamp:
|
||
self.send_command(self.valve_commands[self.CLAMP]["close"])
|
||
time.sleep(self.delay_clamp)
|
||
if pusher1:
|
||
self.send_command(self.valve_commands[self.PUSHER1]["close"])
|
||
time.sleep(self.delay_pusher)
|
||
|
||
# ===================== 传感器处理线程 =====================
|
||
def handle_sensor1(self):
|
||
print("传感器线程1启动！！！")
|
||
while self._running:
|
||
try:
|
||
if self.is_valid_sensor_status(self.SENSOR1):
|
||
current_time = time.time()
|
||
if (
|
||
not self.sensor1_triggered
|
||
and (current_time - self.sensor1_last_time)
|
||
> self.sensor1_debounce_time
|
||
):
|
||
self.sensor1_triggered = True
|
||
self.sensor1_last_time = current_time
|
||
# 1.停止包装机皮带电机：关闭 conveyor1
|
||
self.close(conveyor1=True)
|
||
time.sleep(self.delay_conveyor)
|
||
# 2.推板开启：推出去动作
|
||
self.open(pusher=True)
|
||
time.sleep(self.delay_pusher)
|
||
self.close(pusher=True)
|
||
# 推板推出后重新启动电机时间传送带1延时
|
||
time.sleep(self.delay_after_pusher)
|
||
# 3.重新开启包装机皮带电机: 开启conveyor1
|
||
self.open(conveyor1=True)
|
||
time.sleep(self.delay_conveyor)
|
||
# 4.推板关闭：推板收回来动作
|
||
self.open(pusher1=True)
|
||
time.sleep(self.delay_pusher)
|
||
self.close(pusher1=True)
|
||
time.sleep(1)
|
||
# 5. 状态检查（可选）
|
||
status = self.get_all_device_status()
|
||
if status.get("conveyor1") and not status.get("pusher"):
|
||
print("流程完成1：皮带运行中，推板已收回")
|
||
else:
|
||
print("状态异常，请检查设备")
|
||
# 流程结束，重置触发标志
|
||
self.sensor1_triggered = False
|
||
time.sleep(self.sensor1_loop_delay)
|
||
except Exception as e:
|
||
print(f"SENSOR1 处理错误: {e}")
|
||
self.sensor1_triggered = False
|
||
time.sleep(self.sensor1_error_delay)
|
||
|
||
def handle_sensor2(self):
|
||
print("传感器线程12启动！！！")
|
||
while self._running:
|
||
try:
|
||
if self.is_valid_sensor_status_1(self.SENSOR2):
|
||
if not self.sensor2_ready:
|
||
self.close(conveyor2=True)
|
||
self.motor_stopped_by_sensor2 = True
|
||
self.sensor2_ready = True
|
||
else:
|
||
if self.sensor2_ready and self.motor_stopped_by_sensor2:
|
||
self.open(conveyor2=True)
|
||
self.motor_stopped_by_sensor2 = False
|
||
self.sensor2_ready = False
|
||
time.sleep(self.sensor2_loop_delay)
|
||
except Exception as e:
|
||
print(f"SENSOR2 处理错误: {e}")
|
||
time.sleep(self.sensor2_error_delay)
|
||
|
||
# ===================== 线程控制方法 =====================
|
||
def start(self):
|
||
if self._running:
|
||
print("线程已经在运行")
|
||
return
|
||
print("启动传感器线程")
|
||
self._running = True
|
||
self._sensor1_thread = threading.Thread(target=self.handle_sensor1, daemon=True)
|
||
self._sensor2_thread = threading.Thread(target=self.handle_sensor2, daemon=True)
|
||
self._sensor1_thread.start()
|
||
self._sensor2_thread.start()
|
||
|
||
def stop(self):
|
||
if not self._running:
|
||
print("线程未在运行")
|
||
return
|
||
print("停止传感器线程")
|
||
self._running = False
|
||
if self._sensor1_thread:
|
||
self._sensor1_thread.join()
|
||
if self._sensor2_thread:
|
||
self._sensor2_thread.join()
|
||
print("传感器线程已终止。")
|
||
|
||
def start_sensor1_only(self):
|
||
if self._running:
|
||
print("传感器线程已经在运行")
|
||
return
|
||
print("启动传感器1监听线程...")
|
||
self._running = True
|
||
self._sensor1_thread = threading.Thread(target=self.handle_sensor1, daemon=True)
|
||
self._sensor1_thread.start()
|
||
|
||
|
||
if __name__ == "__main__":
|
||
controller = RelayController()
|
||
controller.start()
|
||
try:
|
||
while True:
|
||
time.sleep(1)
|
||
except KeyboardInterrupt:
|
||
controller.stop()
|