stage_one
This commit is contained in:
@ -77,20 +77,20 @@ class FeedingController:
|
||||
current_time = time.time()
|
||||
|
||||
# 检查下料斗破拱（只有在下料过程中才检查）
|
||||
# if self.state.lower_feeding_stage in [1, 2, 3]: # 在所有下料阶段检查
|
||||
# lower_weight = self.transmitter_controller.read_data(2)
|
||||
# if lower_weight is not None:
|
||||
# # 检查重量变化是否过慢（小于0.1kg变化且时间超过10秒）
|
||||
# if (abs(lower_weight - self.state.last_lower_weight) < 0.1) and \
|
||||
# (current_time - self.state.last_weight_time) > 10:
|
||||
# print("下料斗可能堵塞，启动破拱")
|
||||
# self.state._lower_is_arch_=True
|
||||
# self.relay_controller.control(self.relay_controller.BREAK_ARCH_LOWER, 'open')
|
||||
# time.sleep(2)
|
||||
# self.relay_controller.control(self.relay_controller.BREAK_ARCH_LOWER, 'close')
|
||||
# self.state._lower_is_arch_=False
|
||||
if self.state.lower_feeding_stage in [1, 2, 3]: # 在所有下料阶段检查
|
||||
lower_weight = self.transmitter_controller.read_data(2)
|
||||
if lower_weight is not None:
|
||||
# 检查重量变化是否过慢（小于0.1kg变化且时间超过10秒）
|
||||
if (abs(lower_weight - self.state.last_lower_weight) < 10) and \
|
||||
(current_time - self.state.last_weight_time) > 1:
|
||||
print("下料斗可能堵塞，启动破拱")
|
||||
self.state._lower_is_arch_=True
|
||||
self.relay_controller.control(self.relay_controller.BREAK_ARCH_LOWER, 'open')
|
||||
time.sleep(2)
|
||||
self.relay_controller.control(self.relay_controller.BREAK_ARCH_LOWER, 'close')
|
||||
self.state._lower_is_arch_=False
|
||||
|
||||
# self.state.last_lower_weight = lower_weight
|
||||
self.state.last_lower_weight = lower_weight
|
||||
|
||||
# 检查上料斗破拱（在上料斗向下料斗下料时检查）
|
||||
if self.state._feed_status == FeedStatus.FUpperToLower: # 在任何阶段都可能需要上料斗破拱
|
||||
@ -98,8 +98,8 @@ class FeedingController:
|
||||
upper_weight = self.transmitter_controller.read_data(1)
|
||||
if upper_weight is not None:
|
||||
# 检查重量变化是否过慢（小于0.1kg变化且时间超过10秒）,觉得有点小。改成
|
||||
if (abs(upper_weight - self.state.last_upper_weight) < 100) and \
|
||||
(current_time - self.state.last_weight_time) > 5:
|
||||
if (abs(upper_weight - self.state.last_upper_weight) < 25) and \
|
||||
(current_time - self.state.last_weight_time) > 1:
|
||||
print("上料斗可能堵塞，启动破拱")
|
||||
self.state._upper_is_arch_=True
|
||||
self.relay_controller.control(self.relay_controller.BREAK_ARCH_UPPER, 'open')
|
||||
@ -119,58 +119,85 @@ class FeedingController:
|
||||
视觉控制主逻辑
|
||||
"""
|
||||
# 检测是否溢料
|
||||
self.state.overflow_detected = self.vision_detector.detect_overflow(current_frame)
|
||||
# print('视觉控制线程启用中...')
|
||||
# self.state.overflow_detected = self.vision_detector.detect_overflow(current_frame)
|
||||
overflow=self.state.overflow_detected in ["大堆料", "小堆料"]
|
||||
current_angle = self.vision_detector.detect_angle(image=current_frame)
|
||||
current_angle =self.vision_detector.detect_angle(image=current_frame)
|
||||
if current_angle is None:
|
||||
print("无法获取当前角度，跳过本次调整")
|
||||
return
|
||||
self.state.last_angle = current_angle
|
||||
self.state._lower_angle=current_angle
|
||||
print(f"当前角度: {current_angle:.2f}°, 溢料状态: {overflow}, 溢料返回: {self.state.overflow_detected}")
|
||||
return
|
||||
print(f"当前角度: {current_angle:.2f}°")
|
||||
# return
|
||||
if self.state.overflow_detected!="浇筑满":
|
||||
if current_angle is None:
|
||||
print("无法获取当前角度，跳过本次调整")
|
||||
return
|
||||
|
||||
print(f"当前角度: {current_angle:.2f}°, 溢料状态: {overflow}, 控制模式: {self.state.angle_control_mode}")
|
||||
# print(f"当前角度: {current_angle:.2f}°, 溢料状态: {overflow}, 控制模式: {self.state.angle_control_mode}")
|
||||
|
||||
# 状态机控制逻辑
|
||||
if self.state.angle_control_mode == "normal":
|
||||
# 正常模式大于app_set_config.angle_threshold=60度
|
||||
if overflow and current_angle > app_set_config.angle_threshold:
|
||||
# 检测到堆料且角度过大，进入角度减小模式
|
||||
print("检测到堆料且角度过大，关闭出砼门开始减小角度")
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'close')
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'open')
|
||||
if overflow:
|
||||
# if current_angle > app_set_config.angle_threshold:
|
||||
# # 检测到堆料且角度过大，进入角度减小模式
|
||||
# print("检测到堆料且角度过大，关闭出砼门开始减小角度")
|
||||
# self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'close')
|
||||
# self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'open')
|
||||
self.state.angle_control_mode = "reducing"
|
||||
else:
|
||||
else:
|
||||
# 保持正常开门 30
|
||||
# self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'open')
|
||||
if current_angle >app_set_config.target_angle:
|
||||
# 角度已降至目标范围，关闭出砼门
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'close')
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'open')
|
||||
else:
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'open')
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'close')
|
||||
print(f'当前重量:{self.state._mould_finish_weight:.2f}kg, 目标重量:{self.state._mould_need_weight:.2f}kg')
|
||||
if self.state._mould_need_weight>0:
|
||||
|
||||
if self.state._mould_finish_weight/self.state._mould_need_weight>=0.8:
|
||||
print(f"完成重量占比{self.state._mould_finish_weight/self.state._mould_need_weight:.2f}，半开出砼门")
|
||||
#半开出砼门
|
||||
if current_angle >app_set_config.target_angle:
|
||||
# 角度已降至目标范围，关闭出砼门
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'close')
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'open')
|
||||
time.sleep(0.3)
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'close')
|
||||
else:
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'close')
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'open')
|
||||
time.sleep(0.32)
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'close')
|
||||
else:
|
||||
#全开砼门
|
||||
if current_angle > app_set_config.angle_threshold:
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'close')
|
||||
|
||||
|
||||
elif self.state.angle_control_mode == "reducing":
|
||||
# 角度减小模式
|
||||
if current_angle <= app_set_config.target_angle + app_set_config.angle_tolerance:
|
||||
# 角度已达到目标范围
|
||||
if overflow:
|
||||
# 仍有堆料，进入维持模式
|
||||
print(f"角度已降至{current_angle:.2f}°，仍有堆料，进入维持模式")
|
||||
self.state.angle_control_mode = "maintaining"
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'close')
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'open') # 先打开门
|
||||
if overflow:
|
||||
if current_angle <= app_set_config.target_angle:
|
||||
# 角度已达到目标范围
|
||||
|
||||
# 仍有堆料，进入维持模式
|
||||
print(f"角度已降至{current_angle:.2f}°，仍有堆料，进入维持模式")
|
||||
if current_angle<=app_set_config.min_angle:
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'close')
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'open')
|
||||
time.sleep(0.1)
|
||||
self.state.angle_control_mode = "maintaining"
|
||||
# self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'close')
|
||||
# self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'open')
|
||||
else:
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'close')
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'open')
|
||||
# time.sleep(0.05)
|
||||
# 先打开门
|
||||
else:
|
||||
# 无堆料，恢复正常模式
|
||||
print(f"角度已降至{current_angle:.2f}°，无堆料，恢复正常模式")
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'open')
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'close')
|
||||
# self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'open')
|
||||
# self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'close')
|
||||
self.state.angle_control_mode = "normal"
|
||||
|
||||
elif self.state.angle_control_mode == "maintaining":
|
||||
@ -178,8 +205,8 @@ class FeedingController:
|
||||
if not overflow:
|
||||
# 堆料已消除，恢复正常模式
|
||||
print("堆料已消除，恢复正常模式")
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'open')
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'close')
|
||||
# self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'open')
|
||||
# self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'close')
|
||||
self.state.angle_control_mode = "normal"
|
||||
else:
|
||||
# 继续维持角度控制
|
||||
@ -198,7 +225,7 @@ class FeedingController:
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'close')
|
||||
self.state.angle_control_mode = "normal"
|
||||
else:
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'close')
|
||||
# self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'close')
|
||||
self.relay_controller.control_lower_close()
|
||||
|
||||
|
||||
@ -207,12 +234,13 @@ class FeedingController:
|
||||
用于维持模式的脉冲控制
|
||||
保持角度在目标范围内
|
||||
"""
|
||||
print("执行维持脉冲控制")
|
||||
# 关门1秒
|
||||
print("进入维持模式")
|
||||
# 关门时间
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'close')
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'open')
|
||||
time.sleep(1.0)
|
||||
# 开门1秒
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'open')
|
||||
time.sleep(0.2)
|
||||
# 开门时间
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_CLOSE, 'close')
|
||||
time.sleep(1.0)
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'open')
|
||||
time.sleep(0.25)
|
||||
self.relay_controller.control(self.relay_controller.DOOR_LOWER_OPEN, 'close')
|
||||
|
||||
Reference in New Issue
Block a user