新建8822参数

cfg_subtask.toml

@@ -1,5 +1,5 @@
 [get_block]
-pid_kp = 1.0
+pid_kp = 1.5
 pid_ki = 0
 pid_kd = 0
 first_block = "blue"
@@ -15,7 +15,7 @@ pid_ki = 0
 pid_kd = 0
 
 [up_tower]
-pid_kp = 1.0
+pid_kp = 1.3
 pid_ki = 0
 pid_kd = 0
 
@@ -25,12 +25,12 @@ pid_ki = 0
 pid_kd = 0
 
 [put_bball]
-pid_kp = 1.5
+pid_kp = 1.2
 pid_ki = 0
 pid_kd = 0
 
 [put_hanoi1]
-pid_kp = 0.5
+pid_kp = 0.7
 pid_ki = 0
 pid_kd = 0
 
@@ -53,7 +53,7 @@ pid_kd = 0
 llm_enable = true
 
 [kick_ass]
-pid_kp = 0.8
+pid_kp = 1.2
 pid_ki = 0
 pid_kd = 0
 pos_gap1 = 150

cfg_subtask.toml.8822.bak

@@ -0,0 +1,61 @@
+[get_block]
+pid_kp = 1.5
+pid_ki = 0
+pid_kd = 0
+first_block = "blue"
+
+[put_block]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[get_bball]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[up_tower]
+pid_kp = 1.3
+pid_ki = 0
+pid_kd = 0
+
+[get_rball]
+pid_kp = 1.0
+pid_ki = 0
+pid_kd = 0
+
+[put_bball]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi1]
+pid_kp = 0.7
+pid_ki = 0
+pid_kd = 0
+
+[put_hanoi2]
+pid_kp = 1.0
+pid_ki = 0
+pid_kd = 0
+pos_gap = 160
+first_target = "mp"
+
+[put_hanoi3]
+pid_kp = 1.5
+pid_ki = 0
+pid_kd = 0
+
+[move_area]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+llm_enable = true
+
+[kick_ass]
+pid_kp = 1.2
+pid_ki = 0
+pid_kd = 0
+pos_gap1 = 150
+pos_gap2 = 80
+target_person = 1

cfg_subtask.toml.raw.bak

@@ -1,5 +1,5 @@
 [get_block]
-pid_kp = 1.2
+pid_kp = 1.0
 pid_ki = 0
 pid_kd = 0
 first_block = "blue"
@@ -20,17 +20,17 @@ pid_ki = 0
 pid_kd = 0
 
 [get_rball]
-pid_kp = 1.2
+pid_kp = 1.0
 pid_ki = 0
 pid_kd = 0
 
 [put_bball]
-pid_kp = 1.8
+pid_kp = 1.5
 pid_ki = 0
 pid_kd = 0
 
 [put_hanoi1]
-pid_kp = 0.7
+pid_kp = 0.5
 pid_ki = 0
 pid_kd = 0
 
@@ -50,7 +50,7 @@ pid_kd = 0
 pid_kp = 1.2
 pid_ki = 0
 pid_kd = 0
-llm_enable = false
+llm_enable = true
 
 [kick_ass]
 pid_kp = 0.8

subtask.py

@@ -666,7 +666,7 @@ class get_bball():
             time.sleep(0.5)
             by_cmd.send_distance_axis_z(30, -40)
             time.sleep(0.5)
-            by_cmd.send_angle_claw_arm(80)
+            by_cmd.send_angle_claw_arm(90)
             time.sleep(0.5)
             by_cmd.send_angle_claw(54)
             time.sleep(0.5)
@@ -770,7 +770,8 @@ class get_rball():
         by_cmd.send_angle_scoop(20)
         # 上古參數
         # by_cmd.send_distance_y(-15, 50) # 50 # 70
-        by_cmd.send_distance_y(-15, 40) # 50 # 70
+        # by_cmd.send_distance_y(-15, 40) # 50 # 70
+        # time.sleep(1.5)
         # 6_9 參數
         # by_cmd.send_distance_y(-15, 35)
         # time.sleep(2)
@@ -778,6 +779,9 @@ class get_rball():
         # by_cmd.send_distance_y(-15, 45)
         # time.sleep(2)
         # car_stop()
+        # 8822 参数
+        by_cmd.send_distance_y(-15, 40)
+        time.sleep(1.5)
         calibrate_new(tlabel.RBALL,offset = 44, run = True)
         time.sleep(1)
         logger.info("抓红球")
@@ -825,14 +829,14 @@ class put_bball():
         calibrate_new(tlabel.BASKET,offset = -40, run = True, run_speed = 6)
         # by_cmd.send_distance_x(10, 10)
         # 向左运动
-        by_cmd.send_distance_y(-10, 35)
+        # by_cmd.send_distance_y(-10, 35)
         # by_cmd.send_angle_storage(10)
         # time.sleep(1)
 
         by_cmd.send_angle_storage(50)
         logger.info("把球放篮筐里")
 
-        time.sleep(1)
+        time.sleep(2)
         # by_cmd.send_distance_y(10, 55)
         by_cmd.send_angle_storage(20)
         # time.sleep(1)
@@ -958,7 +962,7 @@ class put_hanoi1():
     def after(self):
         # var.switch_lane_model = True
         if utils.direction == tlabel.RMARK:
-            var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.2, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
+            var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"] - 0.3, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
         else:
             var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"])
         pass
@@ -1017,7 +1021,7 @@ class put_hanoi2():
         time.sleep(0.5)
         logger.info("抓大平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 30)
+            by_cmd.send_position_axis_z(30, 40)
             by_cmd.send_position_axis_x(1, 150)
             by_cmd.send_angle_claw(63)
             time.sleep(2)
@@ -1029,7 +1033,7 @@ class put_hanoi2():
             time.sleep(1)
             pass
         else:
-            by_cmd.send_position_axis_z(30, 30)
+            by_cmd.send_position_axis_z(30, 40)
             by_cmd.send_position_axis_x(1, 40)
             by_cmd.send_angle_claw(63)
             time.sleep(2)
@@ -1077,7 +1081,7 @@ class put_hanoi2():
         time.sleep(0.5)
         logger.info("抓中平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 30)
+            by_cmd.send_position_axis_z(30, 40)
             by_cmd.send_position_axis_x(1, 150)
             by_cmd.send_angle_claw(55)
             time.sleep(2)
@@ -1089,7 +1093,7 @@ class put_hanoi2():
             time.sleep(1)
             pass
         else:
-            by_cmd.send_position_axis_z(30, 30)
+            by_cmd.send_position_axis_z(30, 40)
             by_cmd.send_position_axis_x(1, 40)
             by_cmd.send_angle_claw(55)
             time.sleep(2)
@@ -1138,7 +1142,7 @@ class put_hanoi2():
         time.sleep(0.5)
         logger.info("抓小平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 30)
+            by_cmd.send_position_axis_z(30, 40)
             by_cmd.send_position_axis_x(1, 150)
             by_cmd.send_angle_claw(50)
             time.sleep(2)
@@ -1150,7 +1154,7 @@ class put_hanoi2():
             time.sleep(2)
             pass
         else:
-            by_cmd.send_position_axis_z(30, 30)
+            by_cmd.send_position_axis_z(30, 40)
             by_cmd.send_position_axis_x(1, 40)
             by_cmd.send_angle_claw(50)
             time.sleep(2)
@@ -1314,7 +1318,7 @@ class move_area1():
         # 任务检查间隔
         by_cmd.send_position_axis_x(1, 150)
         # time.sleep(1)
-        by_cmd.send_angle_claw_arm(225)
+        # by_cmd.send_angle_claw_arm(225)
 
         pass
 
@@ -1567,7 +1571,8 @@ class move_area2():
         pass
     def after(self):
         var.pid_turning.set(cfg["move_area"]["pid_kp"], cfg["move_area"]["pid_ki"], cfg["move_area"]["pid_kd"])
-        by_cmd.send_position_axis_z(30, 0)
+        # by_cmd.send_position_axis_z(30, 0)
+        by_cmd.send_position_axis_z(30, 120)
         while by_cmd.send_angle_claw(90) == -1:
             pass
         time.sleep(2)
@@ -1583,7 +1588,7 @@ class kick_ass():
         self.target_person = cfg_args['lane_mode']['mode_index']
 
         # by_cmd.send_angle_claw(15)
-        by_cmd.send_position_axis_x(1, 160)
+        # by_cmd.send_position_axis_x(1, 160)
 
     def find(self):
         ret = filter.find(tlabel.SIGN)
@@ -1598,12 +1603,20 @@ class kick_ass():
         calibrate_new(tlabel.SIGN, offset = 8, run = True)
         by_cmd.send_angle_claw(15)
         time.sleep(0.5)
-        by_cmd.send_position_axis_z(30, 80)
-        by_cmd.send_position_axis_x(1, 130)
+        # by_cmd.send_position_axis_z(30, 80)
+        # OCR 摄像头向前移动
+        by_cmd.send_position_axis_x(1, 50)
+        time.sleep(1)
+        by_cmd.send_position_axis_x(1, 150)
 
         # 移动到中间
         by_cmd.send_distance_x(10, 295)
-        time.sleep(4)
+        time.sleep(1)
+        by_cmd.send_angle_claw(15)
+        by_cmd.send_angle_claw_arm(225)
+        time.sleep(1)
+        by_cmd.send_position_axis_z(30, 80)
+        time.sleep(1)
         if self.target_person == 1:
             by_cmd.send_distance_x(-10, 150)
             time.sleep(1.5)

subtask_raw.py

@@ -2,7 +2,8 @@ from enum import Enum
 from loguru import logger
 from utils import label_filter
 from utils import tlabel
-from utils import LLM
+# from utils import LLM
+from utils import LLM_deepseek
 from utils import CountRecord
 import utils
 import toml
@@ -11,10 +12,14 @@ import time
 import variable as var
 import action as act
 import re
-import threading
-import ctypes
+import math
+import json
+import json5
+# import threading
+# import ctypes
 cfg = None
 cfg_args = None
+cfg_move_area = None
 by_cmd = None
 filter = None
 llm_bot = None
@@ -47,9 +52,15 @@ def import_obj(_by_cmd, skip_queue):
 
     global cfg
     global cfg_args
+    global cfg_move_area
     global global_skip_queue
     cfg = toml.load('/home/evan/Workplace/project_main/cfg_subtask.toml') # 加载任务配置
     cfg_args = toml.load('/home/evan/Workplace/project_main/cfg_args.toml')
+    try:
+        with open('/home/evan/Workplace/project_main/cfg_move_area.json', 'r') as f:
+            cfg_move_area = json.load(f)
+    except:
+        cfg_move_area = None
     by_cmd = _by_cmd
     global_skip_queue = skip_queue
 
@@ -60,14 +71,14 @@ def import_obj(_by_cmd, skip_queue):
     logger.info("subtask yolo client init")
 
     # ocr socket 客户端
-    # context1 = zmq.Context()
-    # ocr_socket = context1.socket(zmq.REQ)
-    # ocr_socket.connect("tcp://localhost:6668")
-    # logger.info("subtask ocr client init")
+    context1 = zmq.Context()
+    ocr_socket = context1.socket(zmq.REQ)
+    ocr_socket.connect("tcp://localhost:6668")
+    logger.info("subtask ocr client init")
 
     filter = label_filter(socket)
     if cfg['move_area']['llm_enable']:
-        llm_bot = LLM()
+        llm_bot = LLM_deepseek()
 def car_stop():
     for _ in range(3):
         by_cmd.send_speed_x(0)
@@ -452,26 +463,26 @@ class get_block1():
         calibrate_new(var.first_block, offset = 16, run = True, run_speed = 5)
         logger.info("抓取块")
 
-        by_cmd.send_position_axis_z(30, 60)
+        by_cmd.send_position_axis_z(30, 80)
         time.sleep(1)
         by_cmd.send_angle_claw(63)
         by_cmd.send_position_axis_x(1, 20)
         time.sleep(1)
         by_cmd.send_angle_claw(25)
         time.sleep(0.5)
-        by_cmd.send_position_axis_z(30, 90)
+        by_cmd.send_position_axis_z(30, 110)
         time.sleep(0.5)
 
         by_cmd.send_angle_claw_arm(175)
         time.sleep(0.1)
         by_cmd.send_position_axis_x(1, 100)
         time.sleep(1)
-        by_cmd.send_position_axis_z(30, 70)
+        by_cmd.send_position_axis_z(30, 100)
         time.sleep(0.5)
         by_cmd.send_angle_claw(63)
         time.sleep(0.5)
 
-        by_cmd.send_position_axis_z(30, 130)
+        by_cmd.send_position_axis_z(30, 150)
         time.sleep(1)
         by_cmd.send_position_axis_x(1, 140)
         by_cmd.send_angle_claw_arm(225)
@@ -546,7 +557,7 @@ class put_block():
         ret, box = filter.get(tlabel.HOSPITAL)
         if ret > 0:
             width = box[0][2] - box[0][0]
-            if width > 130:
+            if width > 135:
                 return True
         return False
     def exec(self):
@@ -565,18 +576,18 @@ class put_block():
         # 放置第二個塊
         by_cmd.send_angle_storage(20)
         by_cmd.send_position_axis_x(1, 110)
-        by_cmd.send_position_axis_z(30, 120)
+        by_cmd.send_position_axis_z(30, 140)
         time.sleep(1.5)
         by_cmd.send_angle_claw_arm(180)
         by_cmd.send_angle_claw(85)
         # by_cmd.send_angle_storage(0)
         time.sleep(1)
-        by_cmd.send_position_axis_z(30,70)
+        by_cmd.send_position_axis_z(30,100)
         time.sleep(1)
         by_cmd.send_angle_claw(25)
         by_cmd.send_distance_x(-10, 110)
         time.sleep(1)
-        by_cmd.send_position_axis_z(30, 110)
+        by_cmd.send_position_axis_z(30, 130)
         time.sleep(1)
         by_cmd.send_angle_claw_arm(225)
         time.sleep(1)
@@ -592,7 +603,7 @@ class put_block():
     def after(self):
         var.pid_turning.set(cfg["put_block"]["pid_kp"], cfg["put_block"]["pid_ki"], cfg["put_block"]["pid_kd"])
         # 下一动作预备位置
-        while by_cmd.send_position_axis_z(30, 130) == -1:
+        while by_cmd.send_position_axis_z(30, 150) == -1:
             pass
         time.sleep(1)
         while by_cmd.send_position_axis_x(1, 0) == -1:
@@ -661,7 +672,7 @@ class get_bball():
             time.sleep(0.5)
             by_cmd.send_angle_claw_arm(45)
             time.sleep(1)
-            by_cmd.send_position_axis_z(30, 135)
+            by_cmd.send_position_axis_z(30, 155)
             # 继续向前走
             # by_cmd.send_speed_x(4)
         pass
@@ -671,10 +682,10 @@ class get_bball():
         var.pid_turning.set(cfg["get_bball"]["pid_kp"], cfg["get_bball"]["pid_ki"], cfg["get_bball"]["pid_kd"])
         # 下一动作预备位置
         by_cmd.send_angle_claw(30)
-        time.sleep(0.5)
+        # time.sleep(0.5)
         while by_cmd.send_position_axis_z(30, 0) == -1:
             pass
-        time.sleep(1)
+        time.sleep(0.5)
         # # 任务检查间隔
         # time.sleep(1)
 
@@ -702,13 +713,13 @@ class up_tower():
         by_cmd.send_distance_x(-10, 120)
         time.sleep(1)
         # 上古參數
-        by_cmd.send_distance_y(-10, 60)
+        by_cmd.send_distance_y(-10, 50) # 80
         # 6_9 模型參數
         # by_cmd.send_distance_y(-10, 40)
         # 7_12_3 模型參數
         # by_cmd.send_distance_y(-10, 50)
-        time.sleep(1)
-        car_stop()
+        # time.sleep(2)
+        # car_stop()
         # FIXME 如果下發 distance 後直接 car_stop，則 distance 執行時間僅由指令間處理延時決定
         # time.sleep(3)
         # by_cmd.send_speed_y(-10)
@@ -758,21 +769,25 @@ class get_rball():
         # 靠近塔
         by_cmd.send_angle_scoop(20)
         # 上古參數
-        by_cmd.send_distance_y(-15, 50) # 50
+        # by_cmd.send_distance_y(-15, 50) # 50 # 70
+        by_cmd.send_distance_y(-15, 40) # 50 # 70
+        time.sleep(1.5)
         # 6_9 參數
         # by_cmd.send_distance_y(-15, 35)
         # time.sleep(2)
         # 7_12_3 參數
         # by_cmd.send_distance_y(-15, 45)
-        time.sleep(2)
-        car_stop()
+        # time.sleep(2)
+        # car_stop()
         calibrate_new(tlabel.RBALL,offset = 44, run = True)
         time.sleep(1)
         logger.info("抓红球")
-        # by_cmd.send_angle_scoop(15)
+        # by_cmd.send_angle_scoop(12)
+        time.sleep(0.5)
+        by_cmd.send_position_axis_z(30, 200)
+        time.sleep(3)
+        by_cmd.send_angle_scoop(12)
         time.sleep(0.5)
-        by_cmd.send_position_axis_z(30, 170)
-        time.sleep(2.5)
         by_cmd.send_angle_scoop(7)
         time.sleep(0.5)
         by_cmd.send_speed_y(15)
@@ -861,7 +876,7 @@ class put_hanoi1():
             by_cmd.send_speed_omega(0)
         time.sleep(0.2)
         
-        by_cmd.send_position_axis_z(30, 130)
+        by_cmd.send_position_axis_z(30, 150)
 
         # 校准牌子
         if utils.direction_right > utils.direction_left:
@@ -1003,7 +1018,7 @@ class put_hanoi2():
         time.sleep(0.5)
         logger.info("抓大平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_z(30, 30)
             by_cmd.send_position_axis_x(1, 150)
             by_cmd.send_angle_claw(63)
             time.sleep(2)
@@ -1015,7 +1030,7 @@ class put_hanoi2():
             time.sleep(1)
             pass
         else:
-            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_z(30, 30)
             by_cmd.send_position_axis_x(1, 40)
             by_cmd.send_angle_claw(63)
             time.sleep(2)
@@ -1063,7 +1078,7 @@ class put_hanoi2():
         time.sleep(0.5)
         logger.info("抓中平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_z(30, 30)
             by_cmd.send_position_axis_x(1, 150)
             by_cmd.send_angle_claw(55)
             time.sleep(2)
@@ -1075,7 +1090,7 @@ class put_hanoi2():
             time.sleep(1)
             pass
         else:
-            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_z(30, 30)
             by_cmd.send_position_axis_x(1, 40)
             by_cmd.send_angle_claw(55)
             time.sleep(2)
@@ -1094,7 +1109,7 @@ class put_hanoi2():
         time.sleep(0.5)
         logger.info("放中平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 120)
+            by_cmd.send_position_axis_z(30, 140)
             time.sleep(2)
             by_cmd.send_position_axis_x(1, 150)
             time.sleep(2)
@@ -1106,7 +1121,7 @@ class put_hanoi2():
             time.sleep(1)
             pass
         else:
-            by_cmd.send_position_axis_z(30, 120)
+            by_cmd.send_position_axis_z(30, 140)
             time.sleep(2)
             by_cmd.send_position_axis_x(1, 40)
             time.sleep(2)
@@ -1124,7 +1139,7 @@ class put_hanoi2():
         time.sleep(0.5)
         logger.info("抓小平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_z(30, 30)
             by_cmd.send_position_axis_x(1, 150)
             by_cmd.send_angle_claw(50)
             time.sleep(2)
@@ -1136,7 +1151,7 @@ class put_hanoi2():
             time.sleep(2)
             pass
         else:
-            by_cmd.send_position_axis_z(30, 10)
+            by_cmd.send_position_axis_z(30, 30)
             by_cmd.send_position_axis_x(1, 40)
             by_cmd.send_angle_claw(50)
             time.sleep(2)
@@ -1155,7 +1170,7 @@ class put_hanoi2():
         time.sleep(0.5)
         logger.info("放小平台")
         if utils.direction is tlabel.RMARK:
-            by_cmd.send_position_axis_z(30, 190) # 170
+            by_cmd.send_position_axis_z(30, 200) # 170 #190(new)
             time.sleep(1.5)
             by_cmd.send_position_axis_x(1, 150)
             time.sleep(2)
@@ -1171,7 +1186,7 @@ class put_hanoi2():
             # car_stop()
             pass
         else:
-            by_cmd.send_position_axis_z(30, 190)
+            by_cmd.send_position_axis_z(30, 200)
             time.sleep(1.5)
             by_cmd.send_position_axis_x(1, 40)
             time.sleep(2)
@@ -1194,12 +1209,12 @@ class put_hanoi2():
         else:
             var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) 
         # time.sleep(2)
-        var.task_speed = 10
+        var.task_speed = 13
         pass
 
 class put_hanoi3():
     def init(self):
-        while by_cmd.send_position_axis_z(30, 130) == -1:
+        while by_cmd.send_position_axis_z(30, 150) == -1:
             pass
         time.sleep(3)
         logger.info("完成任务，爪回左侧")
@@ -1217,14 +1232,16 @@ class put_hanoi3():
         time.sleep(1)
         return True
     def exec(self):
-        while by_cmd.send_position_axis_z(30, 100) == -1:
+        while by_cmd.send_position_axis_z(30, 120) == -1:
             pass
         pass
     def nexec(self):
         pass
     def after(self):
+        by_cmd.send_angle_storage(20)
         by_cmd.send_position_axis_x(1, 150)
-        var.pid_turning.set(cfg["put_hanoi3"]["pid_kp"], cfg["put_hanoi3"]["pid_ki"], cfg["put_hanoi3"]["pid_kd"])
+        var.pid_turning.set(cfg["put_hanoi3"]["pid_kp"] - 0.2, cfg["put_hanoi3"]["pid_ki"], cfg["put_hanoi3"]["pid_kd"])
+        # FIXME 此处 -0.2 在 `2e6ce3e1f7d326d6ce8110855e2339ebc03ab2da` 前没有
 
 # 应急避险 第一阶段 找目标牌
 class move_area1():
@@ -1232,6 +1249,7 @@ class move_area1():
         logger.info("应急避险第一阶段初始化")
         while (by_cmd.send_angle_camera(0) == -1):
             by_cmd.send_angle_camera(0)
+        filter.switch_camera(1)
     def find(self):
         ret = filter.find(tlabel.SIGN)
         if ret:
@@ -1243,7 +1261,7 @@ class move_area1():
         car_stop()
         time.sleep(1)
         # calibrate_new(tlabel.SIGN, offset = 8, run = True)
-        calibrate_new(tlabel.SIGN, offset = -30, run = True, run_speed = 5)
+        calibrate_new(tlabel.SIGN, offset = -1, run = True, run_speed = 5)
         time.sleep(1)
 
         by_cmd.send_position_axis_x(1, 50)
@@ -1251,33 +1269,42 @@ class move_area1():
 
         # filter_w = (148, 560)
         # filter_h = (165, 390)
-        
-        counts = 0
-        while True:
-            ocr_socket.send_string("")
-            resp = ocr_socket.recv_pyobj()
-            var.llm_text = ''
-            counts += 1
-            if resp.get('code') == 0:
-                for item in resp.get('content'):
-                    
-                    if item['probability']['average'] < 0.80:
-                        continue
-                    # box = item['location']
-                    # center_x = box['left'] + box['width'] / 2
-                    # center_y = box['top'] + box['height'] / 2
-                    # if center_x < filter_w[0] or center_x > filter_w[1] \
-                    #         or center_y < filter_h[0] or center_y > filter_h[1]:
-                    #     continue
-                    var.llm_text += item['words']
-                break
-            if counts >= 2:
+        if cfg_move_area == None:
+            counts = 0
+            while True:
+                ocr_socket.send_string("")
+                resp = ocr_socket.recv_pyobj()
+                var.llm_text = ''
+                counts += 1
+                if resp.get('code') == 0:
+                    for item in resp.get('content'):
+                        
+                        if item['probability']['average'] < 0.80:
+                            continue
+                        # box = item['location']
+                        # center_x = box['left'] + box['width'] / 2
+                        # center_y = box['top'] + box['height'] / 2
+                        # if center_x < filter_w[0] or center_x > filter_w[1] \
+                        #         or center_y < filter_h[0] or center_y > filter_h[1]:
+                        #     continue
+                        var.llm_text += item['words']
+                    break
+                if counts >= 2:
+                    var.skip_llm_task_flag = True
+                    return
+            logger.error(f"OCR 检出字符：\"{var.llm_text}\"")
+
+            
+
+            if len(var.llm_text) < 5:
                 var.skip_llm_task_flag = True
                 return
-        logger.error(var.llm_text)
-        if len(var.llm_text) < 3:
-            var.skip_llm_task_flag = True
-            return
+            else:
+                # 当有效字符大于 5 个文字时 才请求大模型
+                llm_bot.request(var.llm_text)
+        else:
+            # 不需要文字识别 直接使用传入的参数执行 action
+            pass
 
         var.task_speed = 9 # 12
 
@@ -1288,146 +1315,261 @@ class move_area1():
         # 任务检查间隔
         by_cmd.send_position_axis_x(1, 150)
         # time.sleep(1)
-        by_cmd.send_angle_claw_arm(225)
+        # by_cmd.send_angle_claw_arm(225)
 
         pass
 
 # 应急避险 第二阶段 找停车区域
 class move_area2():
+    def __init__(self):
+        self.action_dict = {
+            'beep_seconds': self.beep_seconds,
+            'beep_counts': self.beep_counts,
+            'light_seconds': self.light_seconds,
+            'light_counts': self.light_counts,
+            'beep_light_counts': self.beep_light_counts,
+            'beep_light_seconds': self.beep_light_seconds,
+            'go_front': self.go_front,
+            'go_back': self.go_back,
+            'go_left': self.go_left,
+            'go_right': self.go_right,
+            'go_left_rotate': self.go_left_rotate,
+            'go_right_rotate': self.go_right_rotate,
+            'go_sleep': self.go_sleep
+        }
+        self.front_time = 0
+        self.back_time = 0
+        self.left_time = 0
+        self.right_time = 0
+        self.sum_rotate_angle = 0
+        self.abs_x = 0 # 为了和程序指令适配，其中 x y 方向互换
+        self.abs_y = 0
+        self.abs_w = 0
+        pass
     def init(self):
         logger.info("应急避险第二阶段初始化")
-        self.offset = 15
+        self.offset = 60
         self.delta_x = 0
         self.delta_y = 0
         self.delta_omage = 0
     def find(self):
-        # time.sleep(0.001)
-        if var.skip_llm_task_flag:
+        if var.skip_llm_task_flag and cfg_move_area == None:
             return 5000
         ret, box = filter.get(tlabel.SHELTER)
         if ret:
             error = (box[0][2] + box[0][0] - 320) / 2 + self.offset
-            if abs(error) < 20:
+            # 增加了一个宽度过滤
+            if abs(error) < 30 and abs(box[0][2] - box[0][0]) > 180:
                 return 5000
         return False
-    def sub_light(self, delay_time):
-        by_cmd.send_light(1)
-        time.sleep(delay_time)
-        by_cmd.send_light(0)
-    def sub_beep(self,delay_time):
+    def add_item(self, item):
+        # FIXME 没有对传入参数的范围进行校验，如果出现单位错误，可能会导致无法回位的问题
+        try:
+            return self.action_dict[item.get('action')](item.get('time'))
+        except:
+            pass
+        return False
+    def beep_seconds(self, _time):
         by_cmd.send_beep(1)
-        time.sleep(delay_time)
+        time.sleep(_time * 0.7)
         by_cmd.send_beep(0)
-    def sub_move(self, x, y):
-        # FIXME 如果同時有 xy，是否會造成 delay 不足
-        self.delta_x += x
-        self.delta_y += y
-        
-        if x != 0:
-            delay_time = int(abs(x) * 500)
-            if x > 0:
-                by_cmd.send_distance_x(15, delay_time)
-            else:
-                by_cmd.send_distance_x(-15, delay_time)
-        elif y != 0:
-            delay_time = int(abs(y) * 500)
-            if y > 0: # 向左
-                by_cmd.send_distance_y(-15, delay_time)
-            else:
-                by_cmd.send_distance_y(15, delay_time)
-        time.sleep(delay_time / 500)
-        car_stop()
-        pass
-    def sub_turn(self, angle):
-        self.delta_omage += angle
-        delay_time = int(abs(angle) * 400 / 90)
-        if angle < 0:
-            # 左转
-            by_cmd.send_angle_omega(+55, delay_time)
+        return True
+    def beep_counts(self, _time):
+        for _ in range(_time):
+            by_cmd.send_beep(1)
+            time.sleep(0.3)
+            by_cmd.send_beep(0)
+            time.sleep(0.2)
+        return True
+    def light_seconds(self, _time):
+        by_cmd.send_light(1)
+        time.sleep(_time * 0.7)
+        by_cmd.send_light(0)
+        return True
+    def light_counts(self, _time):
+        for _ in range(_time):
+            by_cmd.send_light(1)
+            time.sleep(0.3)
+            by_cmd.send_light(0)
+            time.sleep(0.2)
+        return True
+    def beep_light_counts(self, _time):
+        for _ in range(_time):
+            by_cmd.send_beep(1)
+            by_cmd.send_light(1)
+            time.sleep(0.3)
+            by_cmd.send_beep(0)
+            by_cmd.send_light(0)
+            time.sleep(0.2)
+        return True
+    def beep_light_seconds(self, _time):
+        by_cmd.send_beep(1)
+        by_cmd.send_light(1)
+        time.sleep(_time * 0.3)
+        by_cmd.send_beep(0)
+        by_cmd.send_light(0)
+        return True
+    def go_front(self, _time):
+        self.abs_y -=  math.sin(self.abs_w) * _time
+        self.abs_x +=  math.cos(self.abs_w) * _time
+        logger.info(f"向前移动：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]")
+        speed_time = int(abs(_time) * 750)
+        by_cmd.send_distance_x(10, speed_time)
+        time.sleep(speed_time / 100)
+        self.front_time += speed_time
+        return True
+    def go_back(self, _time):
+        self.abs_y +=  math.sin(self.abs_w) * _time
+        self.abs_x -=  math.cos(self.abs_w) * _time
+        logger.info(f"向后移动：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]")
+        speed_time = int(abs(_time) * 750)
+        by_cmd.send_distance_x(-10, speed_time)
+        time.sleep(speed_time / 100)
+        self.back_time += speed_time
+        return True
+    def go_left(self, _time):
+        self.abs_y -=  math.cos(self.abs_w) * _time
+        self.abs_x -=  math.sin(self.abs_w) * _time
+        logger.info(f"向左移动：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]")
+        speed_time = int(abs(_time) * 750)
+        by_cmd.send_distance_y(-10, speed_time)
+        time.sleep(speed_time / 100)
+        self.left_time += speed_time
+        return True
+    def go_right(self, _time):
+        self.abs_y +=  math.cos(self.abs_w) * _time
+        self.abs_x +=  math.sin(self.abs_w) * _time
+        logger.info(f"向右移动：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]")
+        speed_time = int(abs(_time) * 750)
+        by_cmd.send_distance_y(10, speed_time)
+        time.sleep(speed_time / 100)
+        self.right_time += speed_time
+        return True
+    def go_shift(self, _dis_x, _dis_y):
+        direct_x = 1.0 if (_dis_x > 0) else -1.0
+        direct_y = 1.0 if (_dis_y > 0) else -1.0
+        self.abs_y -=  math.sin(self.abs_w) * _dis_x
+        self.abs_x +=  math.cos(self.abs_w) * _dis_x
+        self.abs_y +=  math.cos(self.abs_w) * _dis_y
+        self.abs_x +=  math.sin(self.abs_w) * _dis_y
+        logger.info(f"水平移动：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]")
+        speed_time_x = int(abs(_dis_x) * 750)
+        speed_time_y = int(abs(_dis_y) * 750)
+        by_cmd.send_distance_x(10 * direct_x, speed_time_x)
+        by_cmd.send_distance_y(10 * direct_y, speed_time_y)
+        time.sleep(max(speed_time_x, speed_time_y) / 150) #FIXME 除以 100 是否正确
+        return True
+    def go_left_rotate(self, _time):
+        self.abs_w += math.radians(_time) # 弧度制
+        logger.info(f"向左旋转：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]")
+        self.sum_rotate_angle -= _time
+        speed_time = int(abs(_time) * 3.8)
+        by_cmd.send_angle_omega(50, speed_time)
+        time.sleep(speed_time / 200 + 0.5)
+        return True
+    def go_right_rotate(self, _time):
+        self.abs_w -= math.radians(_time) # 弧度制
+        logger.info(f"向右旋转：[目标位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)} ]")
+        self.sum_rotate_angle += _time
+        speed_time = int(abs(_time) * 3.8)
+        by_cmd.send_angle_omega(-50, speed_time)
+        time.sleep(speed_time / 200 + 0.5)
+        return True
+    def go_sleep(self, _time):
+        time.sleep(_time*0.7)
+        return True
+    def reset(self):
+        logger.info(f"开始复位：[当前位置 ({self.abs_y:.2f}, {self.abs_x:.2f}) - 角度 {math.degrees(self.abs_w)}]")
+        # 归一化角度到 0-2pi
+        left_dregee = math.degrees(self.abs_w % (2 * math.pi))
+        # 确定旋转方向 (寻找回正角度最小旋转方向)
+        if math.sin(self.abs_w) < 0:
+            logger.info(f"需要左旋 {360.0 - left_dregee} 回正")
+            self.go_left_rotate(360.0 - left_dregee)
         else:
-            # 右转
-            by_cmd.send_angle_omega(-55, delay_time)
-        time.sleep(delay_time / 300 * 1.5)
+            logger.info(f"需要右旋 {left_dregee} 回正")
+            self.go_right_rotate(left_dregee)
+        time.sleep(0.1)
+
+        # 在框中原点添加向左 0.6m 的偏移值，以便直接回到赛道
+        self.go_shift(self.abs_x * -1.0, self.abs_y * -1.0 - 0.6) 
+
+        logger.info(f"回正后最终位置： ({self.abs_y:.2f}, {self.abs_x:.2f}), 角度： {math.degrees(self.abs_w % (2 * math.pi))}")
     def exec(self):
         var.task_speed = 0
-        if var.skip_llm_task_flag:
-            logger.error("ocr 识别出错 直接跳过改任务")
+        if var.skip_llm_task_flag and cfg_move_area == None:
+            logger.error("ocr 识别出错 直接跳过该任务")
             return
         logger.info("开始寻找停车区域")
         car_stop()
-        calibrate_new(tlabel.SHELTER, offset = 15, run = True)
+        calibrate_new(tlabel.SHELTER, offset = 30, run = True)
         time.sleep(0.5)
-        # 调用大模型 然后执行动作
-        try:
-            resp = llm_bot.get_command_json(var.llm_text)
-            logger.info(resp)
-        except:
-            logger.error("大模型超时，跳过任务")
-            return
-        
-        
-        try:
-            resp_commands = eval(re.findall("```json(.*?)```", resp, re.S)[0])
-            if len(resp_commands) == 0:
+        if cfg_move_area == None:
+            resp = None
+            # 调用大模型 然后执行动作
+            try:
+                logger.info("llm 阻塞等待服务器返回中")
+                start_wait_time = time.perf_counter()
+                while True:
+                    if llm_bot.success_status.isSet():
+                        resp = llm_bot.response.choices[0].message.content
+                        logger.info(f"llm 返回原数据 {resp}")
+                        break
+                    now_time = time.perf_counter()
+                    if llm_bot.error_status.isSet() or (now_time - start_wait_time) > 6.5 :
+                        logger.error("大模型 llm_bot 超时，跳过任务")
+                        return
+                    
+            except:
+                logger.error("大模型 llm_bot 未知错误，跳过任务")
                 return
-            # 进入停车区域
-            # by_cmd.send_speed_y(15)
-            by_cmd.send_distance_y(25, 180)
-            time.sleep(1)
-            # time.sleep(1.25)
-            car_stop()
-            logger.info(resp_commands)
-            for command in resp_commands:
-                logger.info(command)
-                if command['func'] == 'move':
-                    self.sub_move(float(command['x']), float(command['y']))
-                elif command['func'] == 'light':
-                    self.sub_light(int(command['time']))
-                elif command['func'] == 'beep':
-                    self.sub_beep(int(command['time']))
-                elif command['func'] == 'turn':
-                    self.sub_turn(int(command['angle']))
-                    pass
+
+            try:
+                json_text = re.findall("```json(.*?)```", resp, re.S)
+                if len(json_text) == 0:
+                    # 返回的内容不带'''json
+                    resp_commands = eval(resp)
                 else:
-                    continue
+                    resp_commands = json5.loads(json_text[0])
+            
+                    
+                logger.info(f"解析后的动作序列 {resp_commands}")
+                if len(resp_commands) == 0:
+                    return
+                action_list = resp_commands
+                # 进入停车区域
+                by_cmd.send_distance_y(10, 450)
+                time.sleep((450 * 5 / 1000) + 0.5)
+                for action in action_list:
+                    self.add_item(action)
+                    time.sleep(0.1)
                 time.sleep(0.5)
-        except:
+                self.reset()
+            except:
+                logger.warning("任务解析失败并退出，文心一言真是废物 (毋庸置疑)")
+                pass
+            
+        else:
+            # 无需调用大模型 直接开始执行传入的参数
+            try:
+                by_cmd.send_distance_y(10, 450)
+                time.sleep((450 * 5 / 1000) + 0.5)
+                for action in cfg_move_area:
+                    self.add_item(action)
+                    time.sleep(0.1)
+                time.sleep(0.5)
+                self.reset()
+            except:
+                pass
             pass
-        time.sleep(1)
-        # 回到原位
-        
-        delay_time = int(abs(self.delta_omage) * 400 / 90)
-        if int(abs(self.delta_omage)) == 360:
-            delay_time = 0
-        if self.delta_omage < 0:
-            # 左转
-            by_cmd.send_angle_omega(-55, delay_time)
-        else:
-            # 右转
-            by_cmd.send_angle_omega(55, delay_time)
-        time.sleep(delay_time / 300 * 1.5)
-        if self.delta_y > 0:
-            # 向左移动的距离就要比进入的时候少一些 因为 action 已经向左运动了
-            delay_time = 180 - (self.delta_y * 500)
-        else:
-            delay_time = 180 + (abs(self.delta_y) * 500)
-        # 离开停车区域
-        by_cmd.send_distance_y(-25, delay_time)
-
-        time.sleep(delay_time * 5e-3)
-        
-        car_stop()
-
-        # FIXME 移动距离指令下发后未完成，再发送速度指令，将不会清除未完成的速度值
-        # by_cmd.send_distance_y(-15, 300)
-        pass
     def nexec(self):
         logger.warning("正在跳過大模型任務")
         time.sleep(2)
         pass
     def after(self):
         var.pid_turning.set(cfg["move_area"]["pid_kp"], cfg["move_area"]["pid_ki"], cfg["move_area"]["pid_kd"])
-        by_cmd.send_position_axis_z(30, 0)
+        # by_cmd.send_position_axis_z(30, 0)
+        by_cmd.send_position_axis_z(30, 120)
         while by_cmd.send_angle_claw(90) == -1:
             pass
         time.sleep(2)
@@ -1443,7 +1585,7 @@ class kick_ass():
         self.target_person = cfg_args['lane_mode']['mode_index']
 
         # by_cmd.send_angle_claw(15)
-        by_cmd.send_position_axis_x(1, 160)
+        # by_cmd.send_position_axis_x(1, 160)
 
     def find(self):
         ret = filter.find(tlabel.SIGN)
@@ -1458,18 +1600,51 @@ class kick_ass():
         calibrate_new(tlabel.SIGN, offset = 8, run = True)
         by_cmd.send_angle_claw(15)
         time.sleep(0.5)
-        by_cmd.send_position_axis_z(30, 60)
-        by_cmd.send_position_axis_x(1, 130)
+        # by_cmd.send_position_axis_z(30, 80)
+        # OCR 摄像头向前移动
+        by_cmd.send_position_axis_x(1, 50)
+        time.sleep(1)
+        by_cmd.send_position_axis_x(1, 150)
 
+        # 移动到中间
+        by_cmd.send_distance_x(10, 295)
+        time.sleep(1)
+        by_cmd.send_angle_claw(15)
+        by_cmd.send_angle_claw_arm(225)
+        time.sleep(1)
+        by_cmd.send_position_axis_z(30, 80)
+        time.sleep(1)
         if self.target_person == 1:
-            target_distance = self.pos_gap1
+            by_cmd.send_distance_x(-10, 150)
+            time.sleep(1.5)
+            car_stop()
+        elif self.target_person == 2:
+            by_cmd.send_distance_x(-10, 50)
+            time.sleep(1.5)
+            car_stop()
+        elif self.target_person == 3:
+            by_cmd.send_distance_x(10, 50)
+            time.sleep(1.5)
+            car_stop()
         else:
-            target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
-        by_cmd.send_distance_x(10, target_distance)
-        
-        logger.info(f"target distance {target_distance}")
-        time.sleep(1.5 + (self.target_person - 1) * 0.7 )
-        car_stop()
+            by_cmd.send_distance_x(10, 150)
+            time.sleep(1.5)
+            car_stop()
+
+        # 先移动到第一个人的地方 假动作
+        # by_cmd.send_distance_x(10, self.pos_gap1)
+        # time.sleep(1.5)
+
+        # if self.target_person == 1:
+        #     # target_distance = self.pos_gap1
+        #     pass
+        # else:
+        #     # target_distance = self.pos_gap1 + (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
+        #     target_distance = (self.target_person - 1) * self.pos_gap2 + (self.target_person - 1) * 10
+        #     by_cmd.send_distance_x(10, target_distance)
+        #     logger.info(f"target distance {target_distance}")
+        #     time.sleep(1.5 + (self.target_person - 1) * 0.7 )
+        # car_stop()
         
         # by_cmd.send_angle_claw_arm(225)
         # time.sleep(0.5)

utils.py

@@ -424,7 +424,8 @@ class LLM_deepseek:
             你的回复：[{"index":0,"action":"beep_light_counts","time": 1}]
             我的话：灯光闪烁 3 次同时蜂鸣器也叫 3 次
             你的回复：[{"index":0,"action":"beep_light_counts","time": 3}]
-            
+            我的话：闪烁 5 次
+            你的回复：[{"index":0,"action":"light_counts","time": 5}]
             强调一下，对于‘离开’这个指令，请忽略，这对我很重要！
         '''
     def request_thread(self):