453 lines
18 KiB
Python
453 lines
18 KiB
Python
|
|
|||
|
|
from config.settings import app_set_config
|
|||
|
|
from hardware.relay import RelayController
|
|||
|
|
from hardware.transmitter import TransmitterController
|
|||
|
|
import time
|
|||
|
|
import threading
|
|||
|
|
from datetime import datetime
|
|||
|
|
|
|||
|
|
class VisualCallback:
|
|||
|
|
# 类变量，用于存储实例引用，实现单例检测
|
|||
|
|
_instance = None
|
|||
|
|
_lock = threading.Lock()
|
|||
|
|
|
|||
|
|
def __new__(cls):
|
|||
|
|
"""检测实例是否存在，实现单例模式"""
|
|||
|
|
with cls._lock:
|
|||
|
|
if cls._instance is None:
|
|||
|
|
cls._instance = super().__new__(cls)
|
|||
|
|
return cls._instance
|
|||
|
|
|
|||
|
|
def __init__(self):
|
|||
|
|
"""初始化视觉回调处理器"""
|
|||
|
|
# 避免重复初始化
|
|||
|
|
if hasattr(self, '_initialized') and self._initialized:
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
self.angle_mode = "normal"
|
|||
|
|
self.relay_controller = RelayController()
|
|||
|
|
self.transmitter_controller = TransmitterController(self.relay_controller)
|
|||
|
|
self.init_weight = 100
|
|||
|
|
self.mould_finish_weight = 0
|
|||
|
|
self.mould_need_weight = 100
|
|||
|
|
self.finish_count = 0
|
|||
|
|
self.overflow = False
|
|||
|
|
self.is_start_visual=False
|
|||
|
|
|
|||
|
|
# 线程安全的参数传递
|
|||
|
|
self._current_angle = None
|
|||
|
|
self._overflow_detected = None
|
|||
|
|
self._new_data_available = threading.Event()
|
|||
|
|
self._is_processing = threading.Lock()
|
|||
|
|
self._stop_event = threading.Event()
|
|||
|
|
|
|||
|
|
# 添加下料斗门控制锁，防止两个线程同时控制
|
|||
|
|
self._door_control_lock = threading.Lock()
|
|||
|
|
# 记录当前控制门的线程名称，用于调试
|
|||
|
|
self._current_controlling_thread = None
|
|||
|
|
# 新增标志位：指示safe_control_lower_close是否正在执行
|
|||
|
|
self._is_safe_closing = False
|
|||
|
|
|
|||
|
|
self._is_feed_start=False
|
|||
|
|
|
|||
|
|
# 创建并启动单个持续运行的线程
|
|||
|
|
self.callback_thread = threading.Thread(
|
|||
|
|
target=self._run_thread_loop,
|
|||
|
|
daemon=True
|
|||
|
|
)
|
|||
|
|
self.callback_thread.start()
|
|||
|
|
|
|||
|
|
|
|||
|
|
self.feed_thread = threading.Thread(
|
|||
|
|
target=self._run_feed,
|
|||
|
|
daemon=True
|
|||
|
|
)
|
|||
|
|
self.feed_thread.start()
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
# 初始化控制间隔和堆料状态跟踪属性
|
|||
|
|
self._last_overflow_state = False
|
|||
|
|
self._last_control_time = 0
|
|||
|
|
self._initialized = True
|
|||
|
|
|
|||
|
|
def angle_visual_callback(self, current_angle, overflow_detected):
|
|||
|
|
"""
|
|||
|
|
视觉控制主逻辑，供外部推送数据
|
|||
|
|
使用单个持续运行的线程，通过参数设置传递数据
|
|||
|
|
如果线程正在处理数据，则丢弃此次推送
|
|||
|
|
"""
|
|||
|
|
print(f"{datetime.now().strftime('%H:%M:%S.%f')[:-3]} 收到推送数据")
|
|||
|
|
# 尝试获取处理锁，若失败则说明正在处理，丢弃数据
|
|||
|
|
if not self._is_processing.acquire(blocking=False):
|
|||
|
|
print("回调线程仍在执行，丢弃此次推送数据")
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
# 更新参数
|
|||
|
|
|
|||
|
|
if overflow_detected is not None:
|
|||
|
|
print(f"{datetime.now().strftime('%H:%M:%S.%f')[:-3]} 收到溢料:{overflow_detected}")
|
|||
|
|
self._overflow_detected = overflow_detected
|
|||
|
|
if current_angle is not None:
|
|||
|
|
print(f"{datetime.now().strftime('%H:%M:%S.%f')[:-3]} 收到角度:{current_angle}")
|
|||
|
|
self._current_angle = current_angle
|
|||
|
|
# 通知线程有新数据可用
|
|||
|
|
self._new_data_available.set()
|
|||
|
|
finally:
|
|||
|
|
# 释放处理锁
|
|||
|
|
self._is_processing.release()
|
|||
|
|
|
|||
|
|
|
|||
|
|
def _run_thread_loop(self):
|
|||
|
|
"""
|
|||
|
|
线程主循环，持续运行
|
|||
|
|
等待新数据，然后调用处理方法
|
|||
|
|
"""
|
|||
|
|
while not self._stop_event.is_set():
|
|||
|
|
# 等待新数据可用
|
|||
|
|
self._new_data_available.wait()
|
|||
|
|
|
|||
|
|
# 重置事件
|
|||
|
|
self._new_data_available.clear()
|
|||
|
|
|
|||
|
|
# 获取当前参数（使用临时变量避免被其他线程修改）
|
|||
|
|
current_angle = self._current_angle
|
|||
|
|
overflow_detected = self._overflow_detected
|
|||
|
|
self._is_feed_start=True
|
|||
|
|
if self.is_start_visual:
|
|||
|
|
# 处理数据
|
|||
|
|
self._process_angle_callback(current_angle, overflow_detected)
|
|||
|
|
time.sleep(0.1)
|
|||
|
|
def _run_feed(self):
|
|||
|
|
while True:
|
|||
|
|
print("------------已启动----------------")
|
|||
|
|
if self._is_feed_start:
|
|||
|
|
print("------------下料启动----------------")
|
|||
|
|
print("------------下料启动----------------")
|
|||
|
|
print("------------下料启动----------------")
|
|||
|
|
self.run_feed()
|
|||
|
|
break
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
def run_feed(self):
|
|||
|
|
"""第一阶段下料：下料斗向模具车下料（低速）"""
|
|||
|
|
print("--------------------开始下料--------------------")
|
|||
|
|
loc_relay=self.relay_controller
|
|||
|
|
loc_mitter=self.transmitter_controller
|
|||
|
|
|
|||
|
|
initial_lower_weight=loc_mitter.read_data(2)
|
|||
|
|
initial_upper_weight=loc_mitter.read_data(1)
|
|||
|
|
first_finish_weight=0
|
|||
|
|
start_time=None
|
|||
|
|
self.is_start_visual=True
|
|||
|
|
|
|||
|
|
def safe_control_lower_close():
|
|||
|
|
"""线程安全的下料斗关闭方法"""
|
|||
|
|
thread_name = threading.current_thread().name
|
|||
|
|
print(f"[{thread_name}] 尝试关闭下料斗...")
|
|||
|
|
# 设置标志位，指示正在执行安全关闭操作
|
|||
|
|
self._is_safe_closing = True
|
|||
|
|
try:
|
|||
|
|
with self._door_control_lock:
|
|||
|
|
self._current_controlling_thread = thread_name
|
|||
|
|
print(f"[{thread_name}] 获得下料斗控制权，执行关闭操作")
|
|||
|
|
loc_relay.control_lower_close()
|
|||
|
|
self._current_controlling_thread = None
|
|||
|
|
print(f"[{thread_name}] 释放下料斗控制权")
|
|||
|
|
finally:
|
|||
|
|
# 无论成功失败，都要重置标志位
|
|||
|
|
self._is_safe_closing = False
|
|||
|
|
|
|||
|
|
while True:
|
|||
|
|
loc_mitter.is_start_lower=True
|
|||
|
|
current_weight = loc_mitter.read_data(2)
|
|||
|
|
first_finish_weight=initial_lower_weight-current_weight
|
|||
|
|
if current_weight<500:
|
|||
|
|
# 破拱控制
|
|||
|
|
if start_time is None or time.time()-start_time>5:
|
|||
|
|
start_time=time.time()
|
|||
|
|
loc_relay.control_arch_lower_open()
|
|||
|
|
|
|||
|
|
if current_weight<100:
|
|||
|
|
start_time=None
|
|||
|
|
self.is_start_visual=False
|
|||
|
|
loc_mitter.is_start_lower=False
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
safe_control_lower_close()
|
|||
|
|
break
|
|||
|
|
print(f'------------已下料: {first_finish_weight}kg-------------')
|
|||
|
|
time.sleep(1)
|
|||
|
|
|
|||
|
|
#打开上料斗出砼门，开5就，开三分之一下
|
|||
|
|
loc_relay.control_upper_open_sync(4)
|
|||
|
|
upper_open_time=time.time()
|
|||
|
|
while True:
|
|||
|
|
print(f'------------上料斗向下料斗转移-------------')
|
|||
|
|
loc_mitter.is_start_upper=True
|
|||
|
|
current_upper_weight = loc_mitter.read_data(1)
|
|||
|
|
if current_upper_weight<4000:
|
|||
|
|
#关5秒
|
|||
|
|
loc_relay.control_upper_close()
|
|||
|
|
loc_mitter.is_start_upper=False
|
|||
|
|
break
|
|||
|
|
else:
|
|||
|
|
if time.time()-upper_open_time>2:
|
|||
|
|
upper_open_time=time.time()
|
|||
|
|
loc_relay.control_upper_open_sync(0.5)
|
|||
|
|
else:
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
# time.sleep(0.4)
|
|||
|
|
|
|||
|
|
self.is_start_visual=True
|
|||
|
|
loc_mitter.is_start_lower=False
|
|||
|
|
loc_mitter.test_lower_weight=2000
|
|||
|
|
initial_lower_weight=loc_mitter.read_data(2)
|
|||
|
|
|
|||
|
|
while True:
|
|||
|
|
loc_mitter.is_start_lower=True
|
|||
|
|
current_weight = loc_mitter.read_data(2)
|
|||
|
|
second_finish_weight=initial_lower_weight-current_weight
|
|||
|
|
if current_weight<500:
|
|||
|
|
#关5秒
|
|||
|
|
if start_time is None or time.time()-start_time>5:
|
|||
|
|
start_time=time.time()
|
|||
|
|
loc_relay.control_arch_lower_open()
|
|||
|
|
if current_weight<100:
|
|||
|
|
start_time=None
|
|||
|
|
self.is_start_visual=False
|
|||
|
|
loc_mitter.is_start_lower=False
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
|
|||
|
|
safe_control_lower_close()
|
|||
|
|
break
|
|||
|
|
print(f'------------已下料: {first_finish_weight+second_finish_weight}kg-------------')
|
|||
|
|
time.sleep(1)
|
|||
|
|
|
|||
|
|
print(f'------------已完成-------------')
|
|||
|
|
print(f'------------已完成-------------')
|
|||
|
|
print(f'------------已完成-------------')
|
|||
|
|
|
|||
|
|
|
|||
|
|
def _process_angle_callback(self, current_angle, overflow_detected):
|
|||
|
|
"""
|
|||
|
|
实时精细控制 - 基于PID思想，无固定间隔
|
|||
|
|
"""
|
|||
|
|
try:
|
|||
|
|
# 记录控制时间戳（用于微分计算，而非限制）
|
|||
|
|
current_time = time.time()
|
|||
|
|
# 确保所有PID相关属性都被正确初始化
|
|||
|
|
if not hasattr(self, '_last_control_time'):
|
|||
|
|
self._last_control_time = current_time
|
|||
|
|
if not hasattr(self, '_last_error'):
|
|||
|
|
self._last_error = 0
|
|||
|
|
if not hasattr(self, '_error_integral'):
|
|||
|
|
self._error_integral = 0
|
|||
|
|
|
|||
|
|
print(f"{self.angle_mode}")
|
|||
|
|
self.overflow = overflow_detected in ["大堆料", "小堆料"]
|
|||
|
|
|
|||
|
|
if current_angle is None:
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
print(f"{datetime.now().strftime('%H:%M:%S.%f')[:-3]} 当前角度: {current_angle:.2f}°")
|
|||
|
|
|
|||
|
|
# 根据溢料状态动态调整目标角度
|
|||
|
|
if overflow_detected == "大堆料":
|
|||
|
|
TARGET_ANGLE = 15.0 # 大堆料时控制在15度左右
|
|||
|
|
elif overflow_detected == "小堆料":
|
|||
|
|
TARGET_ANGLE = 35.0 # 小堆料时控制在35度左右
|
|||
|
|
else:
|
|||
|
|
TARGET_ANGLE = 56.0 # 未溢料时开到最大56度
|
|||
|
|
|
|||
|
|
# 确保目标角度在硬件范围内（5-56度）
|
|||
|
|
TARGET_ANGLE = max(5.0, min(56.0, TARGET_ANGLE))
|
|||
|
|
|
|||
|
|
# PID控制参数
|
|||
|
|
KP = 0.1 # 比例系数
|
|||
|
|
KI = 0.01 # 积分系数
|
|||
|
|
KD = 0.05 # 微分系数
|
|||
|
|
|
|||
|
|
# 计算误差
|
|||
|
|
error = current_angle - TARGET_ANGLE
|
|||
|
|
dt = current_time - self._last_control_time
|
|||
|
|
|
|||
|
|
# 积分项（抗饱和）
|
|||
|
|
self._error_integral += error * dt
|
|||
|
|
self._error_integral = max(min(self._error_integral, 50), -50) # 积分限幅
|
|||
|
|
|
|||
|
|
# 微分项
|
|||
|
|
error_derivative = (error - self._last_error) / dt if dt > 0 else 0
|
|||
|
|
|
|||
|
|
# PID输出
|
|||
|
|
pid_output = (KP * error + KI * self._error_integral + KD * error_derivative)
|
|||
|
|
|
|||
|
|
# 更新历史值
|
|||
|
|
self._last_error = error
|
|||
|
|
self._last_control_time = current_time
|
|||
|
|
|
|||
|
|
# 状态机 + PID控制
|
|||
|
|
|
|||
|
|
if self.angle_mode == "normal":
|
|||
|
|
self._normal_mode_advanced(current_angle, pid_output)
|
|||
|
|
|
|||
|
|
elif self.angle_mode == "reducing":
|
|||
|
|
self._reducing_mode_advanced(current_angle, pid_output, TARGET_ANGLE)
|
|||
|
|
|
|||
|
|
elif self.angle_mode == "maintaining":
|
|||
|
|
self._maintaining_mode_advanced(current_angle, pid_output, TARGET_ANGLE)
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"处理视觉回调时发生异常: {e}")
|
|||
|
|
|
|||
|
|
def _normal_mode_advanced(self, current_angle, pid_output):
|
|||
|
|
"""高级正常模式控制"""
|
|||
|
|
if self.overflow:
|
|||
|
|
self.angle_mode = "reducing"
|
|||
|
|
print("检测到溢料，切换到减小模式")
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
# 基于PID输出的智能控制
|
|||
|
|
control_threshold = 2.0 # 控制死区
|
|||
|
|
|
|||
|
|
if abs(pid_output) > control_threshold:
|
|||
|
|
if pid_output > 0:
|
|||
|
|
# 需要减小角度（关门）
|
|||
|
|
pulse_time = min(0.3, pid_output * 0.1)
|
|||
|
|
self._pulse_control("close", pulse_time)
|
|||
|
|
print(f"正常模式: 角度偏高{pid_output:.1f}，关门{pulse_time:.2f}秒")
|
|||
|
|
else:
|
|||
|
|
# 需要增大角度（开门）
|
|||
|
|
pulse_time = min(0.3, abs(pid_output) * 0.1)
|
|||
|
|
self._pulse_control("open", pulse_time)
|
|||
|
|
print(f"正常模式: 角度偏低{abs(pid_output):.1f}，开门{pulse_time:.2f}秒")
|
|||
|
|
else:
|
|||
|
|
# 在死区内，保持静止
|
|||
|
|
self._stop_door()
|
|||
|
|
print(f"正常模式: 角度在目标范围内，保持静止")
|
|||
|
|
|
|||
|
|
def _reducing_mode_advanced(self, current_angle, pid_output, target_angle):
|
|||
|
|
"""高级减小模式控制"""
|
|||
|
|
if not self.overflow:
|
|||
|
|
if current_angle <= target_angle + 5.0:
|
|||
|
|
self.angle_mode = "normal"
|
|||
|
|
print("溢料消除且角度合适，返回正常模式")
|
|||
|
|
else:
|
|||
|
|
# 缓慢恢复
|
|||
|
|
self._pulse_control("close", 0.1)
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
# 有溢料，积极减小角度
|
|||
|
|
if current_angle > target_angle:
|
|||
|
|
# 使用PID输出计算控制量
|
|||
|
|
pulse_time = min(0.5, max(0.1, pid_output * 0.15))
|
|||
|
|
self._pulse_control("close", pulse_time)
|
|||
|
|
print(f"减小模式: 积极关门{pulse_time:.2f}秒，PID输出:{pid_output:.1f}")
|
|||
|
|
else:
|
|||
|
|
self.angle_mode = "maintaining"
|
|||
|
|
print("角度已达标，进入维持模式")
|
|||
|
|
|
|||
|
|
def _maintaining_mode_advanced(self, current_angle, pid_output, target_angle):
|
|||
|
|
"""高级维持模式控制"""
|
|||
|
|
if not self.overflow:
|
|||
|
|
self.angle_mode = "normal"
|
|||
|
|
print("溢料消除，返回正常模式")
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
# 精确维持控制
|
|||
|
|
dead_zone = 1.5 # 更小的死区
|
|||
|
|
|
|||
|
|
if abs(pid_output) > dead_zone:
|
|||
|
|
pulse_time = min(0.2, abs(pid_output) * 0.05) # 更精细的控制
|
|||
|
|
|
|||
|
|
if pid_output > 0:
|
|||
|
|
self._pulse_control("close", pulse_time)
|
|||
|
|
print(f"维持模式: 微调关门{pulse_time:.2f}秒")
|
|||
|
|
else:
|
|||
|
|
self._pulse_control("open", pulse_time)
|
|||
|
|
print(f"维持模式: 微调开门{pulse_time:.2f}秒")
|
|||
|
|
else:
|
|||
|
|
self._stop_door()
|
|||
|
|
print("维持模式: 角度精确控制中")
|
|||
|
|
|
|||
|
|
def _pulse_control(self, action, duration):
|
|||
|
|
"""统一的脉冲控制方法"""
|
|||
|
|
# 检查是否正在执行safe_control_lower_close，如果是则跳过relay操作
|
|||
|
|
if self._is_safe_closing:
|
|||
|
|
thread_name = threading.current_thread().name
|
|||
|
|
print(f"[{thread_name}] safe_control_lower_close正在执行，跳过脉冲控制 {action}")
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
if duration <= 0:
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
thread_name = threading.current_thread().name
|
|||
|
|
print(f"[{thread_name}] 尝试脉冲控制 {action}，时长 {duration:.2f}秒...")
|
|||
|
|
|
|||
|
|
with self._door_control_lock:
|
|||
|
|
self._current_controlling_thread = thread_name
|
|||
|
|
print(f"[{thread_name}] 获得下料斗控制权，执行脉冲控制")
|
|||
|
|
|
|||
|
|
if action == "open":
|
|||
|
|
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'close')
|
|||
|
|
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'open')
|
|||
|
|
time.sleep(duration)
|
|||
|
|
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'close')
|
|||
|
|
print(f"[{thread_name}] 开门脉冲: {duration:.2f}秒")
|
|||
|
|
else: # close
|
|||
|
|
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'close')
|
|||
|
|
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'open')
|
|||
|
|
time.sleep(duration)
|
|||
|
|
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'close')
|
|||
|
|
print(f"[{thread_name}] 关门脉冲: {duration:.2f}秒")
|
|||
|
|
|
|||
|
|
self._current_controlling_thread = None
|
|||
|
|
print(f"[{thread_name}] 释放下料斗控制权")
|
|||
|
|
|
|||
|
|
def _stop_door(self):
|
|||
|
|
"""停止门运动"""
|
|||
|
|
# 检查是否正在执行safe_control_lower_close，如果是则跳过relay操作
|
|||
|
|
if self._is_safe_closing:
|
|||
|
|
thread_name = threading.current_thread().name
|
|||
|
|
print(f"[{thread_name}] safe_control_lower_close正在执行，跳过停止门运动操作")
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
thread_name = threading.current_thread().name
|
|||
|
|
print(f"[{thread_name}] 尝试停止门运动...")
|
|||
|
|
|
|||
|
|
with self._door_control_lock:
|
|||
|
|
self._current_controlling_thread = thread_name
|
|||
|
|
print(f"[{thread_name}] 获得下料斗控制权，执行停止操作")
|
|||
|
|
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'close')
|
|||
|
|
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'close')
|
|||
|
|
self._current_controlling_thread = None
|
|||
|
|
print(f"[{thread_name}] 释放下料斗控制权")
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def instance_exists(cls):
|
|||
|
|
"""检测实例是否存在"""
|
|||
|
|
return cls._instance is not None
|
|||
|
|
|
|||
|
|
def shutdown(self):
|
|||
|
|
"""关闭线程，清理资源"""
|
|||
|
|
# 设置停止事件
|
|||
|
|
self._stop_event.set()
|
|||
|
|
# 唤醒线程以便它能检测到停止事件
|
|||
|
|
self._new_data_available.set()
|
|||
|
|
# 等待线程结束
|
|||
|
|
if self.callback_thread.is_alive():
|
|||
|
|
self.callback_thread.join(timeout=1.0)
|
|||
|
|
|
|||
|
|
# 创建默认实例
|
|||
|
|
visual_callback_instance = VisualCallback()
|
|||
|
|
|
|||
|
|
# 兼容层，保持原来的函数调用方式可用
|
|||
|
|
def angle_visual_callback(current_angle, overflow_detected):
|
|||
|
|
"""
|
|||
|
|
兼容旧版本的函数调用方式
|
|||
|
|
将调用转发到默认实例的angle_visual_callback方法
|
|||
|
|
"""
|
|||
|
|
visual_callback_instance.angle_visual_callback(current_angle, overflow_detected)
|