forked from huangxin/ailai
178 lines
6.0 KiB
Python
178 lines
6.0 KiB
Python
import logging
|
||
from types import SimpleNamespace
|
||
|
||
# 模拟 Constant
|
||
class Constant:
|
||
str_feed_take = "[测试] 开始抓料流程"
|
||
str_feed_take_success = "[测试] ✅ 抓料成功"
|
||
str_feed_takePhoto_fail = "[测试] ❌ 抓料点位获取失败"
|
||
|
||
# 模拟 FeedStatus 和 CatchStatus
|
||
class FeedStatus:
|
||
FTake = "FTake"
|
||
|
||
class CatchStatus:
|
||
CNone = "CNone"
|
||
CTake = "CTake"
|
||
COk = "COk"
|
||
|
||
class DetectStatus:
|
||
DOk = "DOk"
|
||
|
||
# 模拟 Position 类
|
||
class Position:
|
||
def __init__(self, x=0, y=0, z=0, a=0, b=0, c=0):
|
||
self.X = x
|
||
self.Y = y
|
||
self.Z = z
|
||
self.A = a
|
||
self.B = b
|
||
self.C = c
|
||
|
||
def get_position(self):
|
||
return self
|
||
|
||
def compare(self, other, is_action=False):
|
||
return (
|
||
abs(self.X - other.X) < 0.1 and
|
||
abs(self.Y - other.Y) < 0.1 and
|
||
abs(self.Z - other.Z) < 0.1
|
||
)
|
||
|
||
# 模拟 Detect 类
|
||
class Detect:
|
||
def __init__(self):
|
||
self.detect_position = None
|
||
self.position_index = 0
|
||
self.detect_status = DetectStatus.DOk
|
||
# 模拟配置点位（类似 list.ini）
|
||
self.positions = {
|
||
0: Position(100, 200, 300, 0, 0, 0),
|
||
1: Position(150, 250, 350, 0, 0, 0),
|
||
2: Position(200, 300, 400, 0, 0, 0),
|
||
3: Position(100, 200, 300, 0, 0, 0),
|
||
4: Position(150, 250, 350, 0, 0, 0),
|
||
5: Position(200, 300, 400, 0, 0, 0),
|
||
|
||
}
|
||
|
||
def run(self):
|
||
"""根据当前索引加载点位"""
|
||
pos = self.positions.get(self.position_index, None)
|
||
if pos:
|
||
self.detect_position = pos
|
||
return True
|
||
else:
|
||
print(f"[❌ 失败] 索引 {self.position_index} 没有对应点位")
|
||
return False
|
||
|
||
# 模拟 FeedLine 类
|
||
class FeedLine:
|
||
def __init__(self):
|
||
self.drop_position = Position()
|
||
|
||
def get_take_position(self):
|
||
return SimpleNamespace(get_position=lambda: Position(100, 200, 300))
|
||
|
||
def set_drop_position(self, position):
|
||
self.drop_position = position
|
||
|
||
# 模拟 RelayController
|
||
class RelayController:
|
||
def get_all_device_status(self, *args):
|
||
return {"SENSOR2": True}
|
||
|
||
def open(self, conveyor2=False):
|
||
pass
|
||
|
||
# 模拟主类，用于测试
|
||
class TestRunner:
|
||
def __init__(self):
|
||
self.feedStatus = FeedStatus.FTake
|
||
self.catch = SimpleNamespace(catch_status=CatchStatus.CNone)
|
||
self.detect = Detect()
|
||
self.debug_run_count = 0
|
||
self.sensor2_ready = True
|
||
self.relay_controller = RelayController()
|
||
|
||
# feedConfig 模拟
|
||
self.feedConfig = SimpleNamespace(feedLine=FeedLine())
|
||
|
||
# real_position 模拟
|
||
self.real_position = Position(100, 200, 300)
|
||
|
||
def log_signal(self, level, msg):
|
||
logging.log(level, msg)
|
||
|
||
def next_position(self):
|
||
self.log_signal(logging.INFO, "[调试] 模拟移动到下一个位置")
|
||
|
||
def run_test(self):
|
||
if self.feedStatus == FeedStatus.FTake:
|
||
self.log_signal(logging.INFO, Constant.str_feed_take)
|
||
take_position = self.feedConfig.feedLine.get_take_position()
|
||
if not take_position or not take_position.get_position():
|
||
self.log_signal(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||
return
|
||
|
||
if not take_position.get_position().compare(self.real_position, is_action=True):
|
||
self.log_signal(logging.INFO, "🟡 机器人尚未到达抓料点位")
|
||
return
|
||
|
||
self.log_signal(logging.INFO, "🟢 机器人已到达抓料点位")
|
||
|
||
# 传感器等待逻辑（模拟已触发）
|
||
sensors = self.relay_controller.get_all_device_status('sensors')
|
||
sensor2_value = sensors.get("SENSOR2", False)
|
||
if not sensor2_value:
|
||
self.log_signal(logging.INFO, "⏳ 等待传感器2料包信号...")
|
||
return
|
||
|
||
self.log_signal(logging.INFO, "✅ 传感器2检测到料包到位，开始执行抓取")
|
||
|
||
# 执行抓取动作
|
||
if self.catch.catch_status == CatchStatus.CNone:
|
||
self.catch.catch_status = CatchStatus.CTake
|
||
if self.catch.catch_status == CatchStatus.CTake:
|
||
self.log_signal(logging.INFO, "正在执行抓料动作...")
|
||
self.catch.catch_status = CatchStatus.COk
|
||
if self.catch.catch_status == CatchStatus.COk:
|
||
self.log_signal(logging.INFO, Constant.str_feed_take_success)
|
||
if not self.sensor2_ready:
|
||
self.log_signal(logging.INFO, "抓取完成，重新启动 conveyor2")
|
||
self.relay_controller.open(conveyor2=True)
|
||
|
||
self.catch.catch_status = CatchStatus.CNone
|
||
self.detect.detect_status = DetectStatus.DOk
|
||
self.log_signal(logging.INFO, "修改抓取点")
|
||
print(f"[调试] 即将丢包点位索引: {self.detect.position_index}")
|
||
# ✅ 索引递增
|
||
self.detect.position_index += 1
|
||
print(f"[调试] 下一个要加载的索引: {self.detect.position_index}")
|
||
|
||
# ✅ 加载新点位
|
||
if not self.detect.run():
|
||
self.log_signal(logging.ERROR, "❌ 加载新点位失败，停止流程")
|
||
return
|
||
|
||
# ✅ 设置丢包点
|
||
self.feedConfig.feedLine.set_drop_position(self.detect.detect_position)
|
||
|
||
self.debug_run_count += 1
|
||
self.next_position()
|
||
return
|
||
else:
|
||
self.log_signal(logging.ERROR, Constant.str_feed_takePhoto_fail)
|
||
|
||
# ========================
|
||
# 主程序入口
|
||
# ========================
|
||
if __name__ == "__main__":
|
||
logging.basicConfig(level=logging.INFO, format='%(asctime)s [%(levelname)s] %(message)s')
|
||
|
||
runner = TestRunner()
|
||
print("🔧 开始测试索引递增和点位加载逻辑...\n")
|
||
|
||
for i in range(5):
|
||
print(f"\n🔄 第 {i+1} 次测试")
|
||
runner.run_test() |