feat: 新增文心一言接口

cfg_subtask.toml

@@ -6,8 +6,8 @@ pid_ki = 0
 pid_kd = 0
 
 # *第一个抓取的方块
-# first_block = "red"
+first_block = "red"
-first_block = "blue"
+# first_block = "blue"
 
 ################################################
 [put_block]
@@ -40,7 +40,7 @@ pid_kd = 0
 ################################################
 [put_bball]
 # pid 参数值
-pid_kp = 1.2
+pid_kp = 0.6
 pid_ki = 0
 pid_kd = 0
 
@@ -60,9 +60,9 @@ pid_kd = 0
 
 # 距离标定值
 pos_gap = 160 # 标定值，两个放置位置的标定距离 
-pos_lp = 2    # 1\2\3 数字越小越靠近红色置物区
+# first_target = "lp"
-pos_mp = 1
+first_target = "mp"
-
+# first_target = "sp"
 ################################################
 [put_hanoi3]
 # pid 参数值
@@ -77,7 +77,7 @@ pid_kp = 1.4
 pid_ki = 0
 pid_kd = 0
 
-llm_enable = false # 大模型机器人
+llm_enable = true # 大模型机器人
 
 ################################################
 [kick_ass]
@@ -88,5 +88,5 @@ pid_kd = 0
 
 pos_gap1 = 150    # 目标牌和第一个 person 之间的距离
 pos_gap2 = 80     # person 之间的距离
-target_person = 2 # 击打的人 - 最靠近标识牌的为 1
+target_person = 3 # 击打的人 - 最靠近标识牌的为 1
 ################################################

subtask.py

@@ -194,7 +194,7 @@ def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
                 if abs(error) < 8:
                     error = error * 3
                 else:
-                    error = error
+                    error = error * 2
                 if error > 0:
                     by_cmd.send_distance_x(-10, int(error))
                 else:
@@ -324,7 +324,7 @@ class get_block1():
         return False
     def exec(self):
         car_stop()
-        calibrate_new(self.target_label, offset = 15, run = True)
+        calibrate_new(self.target_label, offset = 15, run = True, run_speed = 5)
         logger.info("抓取块")
         # act.axis.wait(0.5)
         # act.axis.z2(60)
@@ -350,7 +350,7 @@ class get_block1():
         by_cmd.send_angle_claw(63)
         by_cmd.send_position_axis_x(1, 20)
         time.sleep(2)
-        by_cmd.send_angle_claw(30)
+        by_cmd.send_angle_claw(25)
         by_cmd.send_beep(1)
         time.sleep(0.5)
         by_cmd.send_beep(0)
@@ -399,7 +399,7 @@ class get_block2():
         return False
     def exec(self):
         car_stop()
-        calibrate_new(self.target_label, offset = 15, run = True)
+        calibrate_new(self.target_label, offset = 15, run = True, run_speed = 5)
         logger.info("抓取块")
         time.sleep(0.5)
         by_cmd.send_angle_claw_arm(220)
@@ -408,7 +408,7 @@ class get_block2():
         time.sleep(1)
         by_cmd.send_position_axis_x(1, 20)
         time.sleep(1)
-        by_cmd.send_angle_claw(30)
+        by_cmd.send_angle_claw(25)
         by_cmd.send_beep(1)
         time.sleep(0.5)
         by_cmd.send_beep(0)
@@ -468,7 +468,7 @@ class put_block():
         time.sleep(0.5)
         by_cmd.send_position_axis_x(1, 40)
         time.sleep(2)
-        by_cmd.send_angle_claw(30)
+        by_cmd.send_angle_claw(25)
         time.sleep(0.5)
         by_cmd.send_position_axis_z(30, 140)
         time.sleep(2)
@@ -489,7 +489,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, 135) == -1:
+        while by_cmd.send_position_axis_z(30, 130) == -1:
             pass
         time.sleep(1)
         while by_cmd.send_position_axis_x(1, 0) == -1:
@@ -536,7 +536,7 @@ class get_bball():
         car_stop()
         time.sleep(0.5)
         for _ in range(3):
-            calibrate_right_new(tlabel.BBALL, offset = 16, run = True, run_speed = 4)
+            calibrate_right_new(tlabel.BBALL, offset = 16, run = True, run_speed = 5)
             logger.info("抓蓝色球")
             time.sleep(0.5)
             by_cmd.send_angle_claw_arm(36)
@@ -545,7 +545,7 @@ class get_bball():
             time.sleep(1)
             by_cmd.send_angle_claw(60)
             time.sleep(1)
-            by_cmd.send_angle_claw(30)
+            by_cmd.send_angle_claw(25)
             time.sleep(1)
             by_cmd.send_distance_axis_z(30, 20)
             time.sleep(1)
@@ -570,7 +570,8 @@ class get_bball():
         # 下一动作预备位置
         by_cmd.send_angle_claw(30)
         time.sleep(0.5)
-        by_cmd.send_position_axis_z(30, 0)
+        while by_cmd.send_position_axis_z(30, 0) == -1:
+            pass
         time.sleep(1)
         # # 任务检查间隔
         # time.sleep(1)
@@ -651,7 +652,7 @@ class get_rball():
         # by_cmd.send_angle_scoop(15)
         time.sleep(0.5)
         by_cmd.send_position_axis_z(30, 170)
-        time.sleep(5)
+        time.sleep(3.5)
         by_cmd.send_angle_scoop(7)
         by_cmd.send_distance_y(15, 70)
         time.sleep(1)
@@ -672,9 +673,10 @@ class put_bball():
     def init(self):
         logger.info("派发物资初始化")
         filter.switch_camera(1)
-        by_cmd.send_position_axis_z(30, 0)
+        while by_cmd.send_position_axis_z(30, 0) == -1:
-        while (by_cmd.send_angle_camera(90) == -1):
+            pass
-            by_cmd.send_angle_camera(90)
+        while by_cmd.send_angle_camera(90) == -1:
+            pass
     def find(self):
         ret = filter.find(tlabel.BASKET)
         if ret > 0:
@@ -684,9 +686,10 @@ class put_bball():
     def exec(self):
         logger.info("找到篮筐")
         car_stop()
-        calibrate_new(tlabel.BASKET,offset = -21, run = True, run_speed = 5.5)
+        calibrate_new(tlabel.BASKET,offset = -21, run = True, run_speed = 6)
         by_cmd.send_distance_x(10, 20)
-        by_cmd.send_distance_y(-10, 55)
+        # 向左运动
+        by_cmd.send_distance_y(-10, 35)
         time.sleep(1)
 
         by_cmd.send_angle_storage(55)
@@ -709,6 +712,7 @@ class put_bball():
 # 物资盘点
 class put_hanoi1():
     def init(self):
+        var.pid_turning.set(cfg["put_bball"]["pid_kp"], cfg["put_bball"]["pid_ki"], cfg["put_bball"]["pid_kd"])
         logger.info("物资盘点 1 初始化")
         filter.switch_camera(2)
     def find(self):
@@ -804,23 +808,16 @@ class put_hanoi1():
 
 class put_hanoi2():
     def __init__(self):
-        self.pos_lp = cfg['put_hanoi2']['pos_lp']
+        if cfg['put_hanoi2']['first_target'] == "lp":
-        self.pos_mp = cfg['put_hanoi2']['pos_mp']
-        self.pos_sp = 6 - self.pos_lp - self.pos_mp
-        if self.pos_lp == 1:
             self.target_label = tlabel.LPILLER
-        elif self.pos_mp == 1:
+        elif cfg['put_hanoi2']['first_target'] == "mp":
             self.target_label = tlabel.MPILLER
-        else:
+        elif cfg['put_hanoi2']['first_target'] == "sp":
             self.target_label = tlabel.SPILLER
-        self.distance_lp = self.pos_lp * cfg['put_hanoi2']['pos_gap']
-        self.distance_mp = self.pos_mp * cfg['put_hanoi2']['pos_gap']
-        self.distance_sp = self.pos_sp * cfg['put_hanoi2']['pos_gap']
-        logger.info(f"setting hanoi pos_lp[{self.pos_lp}] pos_mp[{self.pos_mp}] pos_sp[{self.pos_sp}]") 
     def init(self):
         logger.info("物资盘点 2 初始化")
         if utils.direction == tlabel.RMARK:
-            self.offset = 25
+            self.offset = 22
         else:
             self.offset = 10
     def find(self):
@@ -846,28 +843,28 @@ class put_hanoi2():
             by_cmd.send_position_axis_x(1, 130)
             by_cmd.send_angle_claw(63)
             time.sleep(1)
-            by_cmd.send_angle_claw(90)
+            by_cmd.send_angle_claw(81)
             time.sleep(1.5)
-            by_cmd.send_angle_claw(45)
+            by_cmd.send_angle_claw(40)
-            time.sleep(2)
-            by_cmd.send_distance_axis_z(30, 20)
             time.sleep(1)
+            by_cmd.send_distance_axis_z(30, 20)
+            time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 10)
-            time.sleep(2)
+            time.sleep(1)
             pass
         else:
             by_cmd.send_position_axis_z(30, 10)
             by_cmd.send_position_axis_x(1, 40)
             by_cmd.send_angle_claw(63)
             time.sleep(1)
-            by_cmd.send_angle_claw(90)
+            by_cmd.send_angle_claw(81)
             time.sleep(1.5)
-            by_cmd.send_angle_claw(45)
+            by_cmd.send_angle_claw(40)
-            time.sleep(2)
-            by_cmd.send_distance_axis_z(30, 20)
             time.sleep(1)
+            by_cmd.send_distance_axis_z(30, 20)
+            time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 160)
-            time.sleep(2)
+            time.sleep(1)
             pass
         explore_calibrate_new(tlabel.TPLATFORM, offset = self.offset, run_direc = -1, run_speed = 5)
         time.sleep(1)
@@ -877,18 +874,24 @@ class put_hanoi2():
             time.sleep(1.5)
             by_cmd.send_distance_axis_z(30, -20)
             time.sleep(1)
-            by_cmd.send_angle_claw(90)
+            by_cmd.send_angle_claw(81)
-            time.sleep(1)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(63)
+            time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 10)
+            time.sleep(1)
             pass
         else:
             by_cmd.send_position_axis_x(1, 40)
             time.sleep(1.5)
             by_cmd.send_distance_axis_z(30, -20)
             time.sleep(1)
-            by_cmd.send_angle_claw(90)
+            by_cmd.send_angle_claw(81)
-            time.sleep(1)
+            time.sleep(0.5)
+            by_cmd.send_angle_claw(63)
+            time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 160)
+            time.sleep(1)
 
         explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = 1, run_speed =  5)
         time.sleep(1)
@@ -898,28 +901,28 @@ class put_hanoi2():
             by_cmd.send_position_axis_x(1, 130)
             by_cmd.send_angle_claw(63)
             time.sleep(1)
-            by_cmd.send_angle_claw(85)
+            by_cmd.send_angle_claw(75)
-            time.sleep(2)
-            by_cmd.send_angle_claw(40)
-            time.sleep(2)
-            by_cmd.send_distance_axis_z(30, 20)
             time.sleep(1)
+            by_cmd.send_angle_claw(35)
+            time.sleep(1)
+            by_cmd.send_distance_axis_z(30, 20)
+            time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 10)
-            time.sleep(3)
+            time.sleep(1)
             pass
         else:
             by_cmd.send_position_axis_z(30, 10)
             by_cmd.send_position_axis_x(1, 40)
             by_cmd.send_angle_claw(63)
             time.sleep(1)
-            by_cmd.send_angle_claw(85)
+            by_cmd.send_angle_claw(75)
-            time.sleep(2)
-            by_cmd.send_angle_claw(40)
-            time.sleep(2)
-            by_cmd.send_distance_axis_z(30, 20)
             time.sleep(1)
+            by_cmd.send_angle_claw(35)
+            time.sleep(1)
+            by_cmd.send_distance_axis_z(30, 20)
+            time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 160)
-            time.sleep(3)
+            time.sleep(1)
             pass
         explore_calibrate_new(tlabel.LPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
         time.sleep(1)
@@ -931,7 +934,7 @@ class put_hanoi2():
             time.sleep(2)
             by_cmd.send_distance_axis_z(30, -20)
             time.sleep(0.5)
-            by_cmd.send_angle_claw(90)
+            by_cmd.send_angle_claw(65)
             time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 10)
             time.sleep(1)
@@ -943,7 +946,7 @@ class put_hanoi2():
             time.sleep(2)
             by_cmd.send_distance_axis_z(30, -20)
             time.sleep(0.5)
-            by_cmd.send_angle_claw(90)
+            by_cmd.send_angle_claw(65)
             time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 160)
             time.sleep(1)
@@ -956,28 +959,28 @@ class put_hanoi2():
             by_cmd.send_position_axis_x(1, 130)
             by_cmd.send_angle_claw(63)
             time.sleep(1)
-            by_cmd.send_angle_claw(85)
+            by_cmd.send_angle_claw(70)
             time.sleep(1)
-            by_cmd.send_angle_claw(30)
+            by_cmd.send_angle_claw(27)
             time.sleep(1)
             by_cmd.send_distance_axis_z(30, 10)
-            time.sleep(1)
+            time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 10)
-            time.sleep(2)
+            time.sleep(1)
             pass
         else:
             by_cmd.send_position_axis_z(30, 10)
             by_cmd.send_position_axis_x(1, 40)
             by_cmd.send_angle_claw(63)
             time.sleep(1)
-            by_cmd.send_angle_claw(85)
+            by_cmd.send_angle_claw(70)
             time.sleep(1)
-            by_cmd.send_angle_claw(30)
+            by_cmd.send_angle_claw(27)
             time.sleep(1)
             by_cmd.send_distance_axis_z(30, 10)
-            time.sleep(1)
+            time.sleep(0.5)
             by_cmd.send_position_axis_x(1, 160)
-            time.sleep(2)
+            time.sleep(1)
             pass
         explore_calibrate_new(tlabel.MPILLER, offset = self.offset, run_direc = -1, run_speed = 5)
         time.sleep(1)
@@ -1031,7 +1034,8 @@ class put_hanoi3():
         time.sleep(1)
         return True
     def exec(self):
-        by_cmd.send_position_axis_z(30, 100)
+        while by_cmd.send_position_axis_z(30, 100) == -1:
+            pass
         pass
     def nexec(self):
         pass
@@ -1054,7 +1058,7 @@ class move_area1():
         # 停车 ocr 识别文字 调用大模型
         car_stop()
         time.sleep(2)
-        var.task_speed = 8
+        var.task_speed = 12
         pass
     def nexec(self):
         pass
@@ -1068,6 +1072,9 @@ class move_area2():
     def init(self):
         logger.info("应急避险第二阶段初始化")
         self.offset = 15
+        self.delta_x = 0
+        self.delta_y = 0
+        self.delta_omage = 0
     def find(self):
         # time.sleep(0.001)
         ret, box = filter.get(tlabel.SHELTER)
@@ -1076,25 +1083,88 @@ class move_area2():
             if abs(error) < 20:
                 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):
+        by_cmd.send_beep(1)
+        time.sleep(delay_time)
+        by_cmd.send_beep(0)
+    def sub_move(self, x, y):
+        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 / 400 * 1.5)
+        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)
+        else:
+            # 右转
+            by_cmd.send_angle_omega(-55, delay_time)
+        time.sleep(delay_time / 300 * 1.5)
     def exec(self):
         var.task_speed = 0
         logger.info("开始寻找停车区域")
         car_stop()
         calibrate_new(tlabel.SHELTER, offset = 15, run = True)
-        time.sleep(1)
+        time.sleep(0.5)
         # 进入停车区域
-        by_cmd.send_distance_y(10, 450)
+        by_cmd.send_distance_y(15, 300)
 
 
-        time.sleep(3)
+        # time.sleep(1)
 
-        # TODO 调用大模型 然后执行动作
+        # 调用大模型 然后执行动作
-        by_cmd.send_light(1)
+        resp_commands = eval(llm_bot.get_command_json("旋转 90 度，发声 1 秒，照亮 1 秒").replace("'''json","").replace("'''",''))
-        time.sleep(2)
+        for command in resp_commands:
-        by_cmd.send_light(0)
+            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
+            time.sleep(0.5)
+        time.sleep(1)
+        # 回到原位
+        delay_time = int(abs(self.delta_omage) * 400 / 90)
+        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 = 300 - (self.delta_y * 500)
+        else:
+            delay_time = 300 + (abs(self.delta_y) * 500)
         # 离开停车区域
-        by_cmd.send_distance_y(-10, 450)
+        by_cmd.send_distance_y(-15, delay_time)
-        time.sleep(4)
+        time.sleep(delay_time / 400 * 1.5)
+        # FIXME 移动距离指令下发后未完成，再发送速度指令，将不会清除未完成的速度值
+        # by_cmd.send_distance_y(-15, 300)
         pass
     def nexec(self):
         pass

test/test_filter.py

@@ -9,6 +9,10 @@ from loguru import logger
 from utils import tlabel
 import zmq
 import time
+from by_cmd_py import by_cmd_py
+import time
+from utils import CountRecord
+by_cmd = by_cmd_py()
 
 context = zmq.Context()
 socket = context.socket(zmq.REQ)
@@ -17,35 +21,210 @@ logger.info("subtask yolo client init")
 
 filter = label_filter(socket)
 filter.switch_camera(1)
-offset = 12
-while True:
-    time.sleep(0.2)
-    # ret1 = filter.get_mult(tlabel.RMARK)
-    # ret, label, error = filter.get_mult_box([tlabel.LMARK, tlabel.RMARK])
-    # if ret:
-    #     # print(error)
-    #     if abs(error) < 40:
-    #         logger.info('yes')
 
-    label = tlabel.RBLOCK
+def car_stop():
+    for _ in range(3):
+        by_cmd.send_speed_x(0)
+        time.sleep(0.2)
+        by_cmd.send_speed_omega(0)
+# 蓝色球使用
+def calibrate_right_new(label, offset, run = True, run_speed = 3.5):
+    not_found_counts = 0
+    ret, error = filter.aim_right(label)
+    while not ret:
+        not_found_counts += 1
+        if not_found_counts >= 20:
+            not_found_counts = 0
+            error = -320 # error > 0 front run
+            logger.info("calibrate_right_new:找不到次数超过 20 帧 直接前进寻找")
+            break
+        ret, error = filter.aim_right(label)
+    
+    error += offset
+    if abs(error) > 10 and run:
+        if error > 0:
+            by_cmd.send_speed_x(-run_speed)
+        else:
+            by_cmd.send_speed_x(run_speed)
+        pass
+    # 停的位置已经很接近目标，可以直接使用 distance 校准
+    else:
+        # logger.info("停车时误差就小于")
+        # error = error * 3
+        # if error > 0:
+        #     by_cmd.send_distance_x(-10, int(error))
+        # else:
+        #     by_cmd.send_distance_x(10, int(-error))
+        return
+    while True:
+        ret, error = filter.aim_right(label)
+        while not ret:
+            ret, error = filter.aim_right(label)
+        error += offset
+        if ret:
+            if abs(error) <= 8:
+                car_stop()
+                logger.info("calibrate_right_new:行进时 误差小于 8 直接停车")
+
+                ret, error = filter.aim_right(label)
+                while not ret:
+                    ret, error = filter.aim_right(label)
+                error += offset
+                logger.info(f"calibrate_right_new:停车后的误差是{error}")
+                if abs(error) > 8:
+                    logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准")
+                    error = error * 3
+                    if error > 0:
+                        by_cmd.send_distance_x(-10, int(error))
+                    else:
+                        by_cmd.send_distance_x(10, int(-error))
+                
+                break
+'''
+description: 校准新方法 找到后停止 然后根据 error 判断前后低速前进 error < 5 后直接停车
+如果停车后 error > 8 则使用 distance 校准 
+这个方法仅用于在视野里能找到的情况下进行校准，并不能实现边走边寻找 然后再校准
+param {*} label
+param {*} offset
+param {*} run
+param {*} run_speed
+return {*}
+'''     
+def calibrate_new(label, offset, run = True, run_speed = 3.5):
+    not_found_counts = 0
+    ret, box = filter.get(label)
+    while not ret:
+        not_found_counts += 1
+        if not_found_counts >= 20:
+            not_found_counts = 0
+            error = -320 # error > 0 front run
+            logger.info("calibrate_new:找不到次数超过 20 帧 直接前进寻找")
+        ret, box = filter.get(label)
+    
+    error = (box[0][2] + box[0][0] - 320) / 2 + offset
+    if abs(error) > 10 and run:
+        if error > 0:
+            by_cmd.send_speed_x(-run_speed)
+        else:
+            by_cmd.send_speed_x(run_speed)
+    # 停的位置已经很接近目标，可以直接使用 distance 校准
+    else:
+        if abs(error) > 8:
+            logger.info(f"calibrate_new:停车后误差{error}大于 8 使用 distance 校准")
+            # error = error # 3
+            if error > 0:
+                by_cmd.send_distance_x(-10, int(error))
+            else:
+                by_cmd.send_distance_x(10, int(-error))
+            return
+        logger.info(f"calibrate_new:停车后误差{error}小于 8  不校准")
+        return 
+    while True:
+        ret, box = filter.get(label)
+        while not ret:
+            ret, box = filter.get(label)
+        
+        error = (box[0][2] + box[0][0] - 320) / 2 + offset
+        if ret:
+            if abs(error) <= 10: # 5
+                car_stop()
+                logger.info("calibrate_new:行进时 误差小于 10 直接停车")
+
+                ret, box = filter.get(label)
+                while not ret:
+                    ret, box = filter.get(label)
+
+                error = (box[0][2] + box[0][0] - 320) / 2 + offset
+                logger.info(f"calibrate_new:停车后的误差是{error}")
+                if abs(error) > 8:
+                    logger.info(f"calibrate_new:停车后的误差大于 8 使用 distance 校准")
+                    error = error * 3
+                    logger.error(f"error * 3{error}")
+                    if error > 0:
+                        by_cmd.send_distance_x(-10, int(error))
+                    else:
+                        by_cmd.send_distance_x(10, int(-error))
+                
+                break
+def explore_calibrate_new(label, offset, run_direc ,run_speed = 3.5):
+    # run_direc == 1 向前
+    if run_direc == 1:
+        by_cmd.send_speed_x(run_speed)
+    else:
+        by_cmd.send_speed_x(-run_speed)
+
+    while True:
         ret, box = filter.get(label)
         while not ret:
-        # 注意这里一定要保证摄像头内有该目标 否则会无限循环
             ret, box = filter.get(label)
         error = (box[0][2] + box[0][0] - 320) / 2 + offset
-    # ret, box = filter.get(tlabel.BBLOCK)
-    
         if ret:
-        print(error)
+            logger.info(f"当前误差:{error}, box[{box[0][2]},{box[0][0]}]")
+            # 校准速度越大 停车的条件越宽泛
+            if abs(error) <= 20:
+                car_stop()
+                logger.info("explore_calibrate_new:行进时 误差小于 10 直接停车")
+                
+                error_sum = 0
+                for _ in range(3):
+                    ret, box = filter.get(label)
+                    while not ret:
+                        ret, box = filter.get(label)
+                    error = (box[0][2] + box[0][0] - 320) / 2 + offset
+                    error_sum += error
+                error_sum /= 3
+                # logger.info(f"停车后像素误差:{error}")
+                logger.info(f"停车后像素误差:{error_sum}")
+                # if abs(error) > 8:
+                logger.info(f"explore_calibrate_new:停车后的误差大于 8 使用 distance 校准")
+                error_sum = error_sum * 3
+                logger.error(f"error * 3 {error}")
+                if error > 0:
+                    by_cmd.send_distance_x(-10, int(error))
                 else:
-        pass
+                    by_cmd.send_distance_x(10, int(-error))
+                
+                break
+
+offset = 10
+# by_cmd.send_angle_claw_arm(217)
+# while True:
+#     pass
+# while (by_cmd.send_angle_camera(180) == -1):
+#     by_cmd.send_angle_camera(180)
+while (by_cmd.send_angle_camera(180) == -1):
+    by_cmd.send_angle_camera(180)
+# by_cmd.send_speed_x(15)
+
+find_counts = 0
+label = tlabel.HOSPITAL
+record = CountRecord(5)
+while True:
+    time.sleep(0.2)
     
     
-    # if ret1:
+    # ret = filter.find(label)
-    #     print(error1)
+    # ret = filter.find(label)
-    #     if abs(error1) < 30:
+    # if ret > 0:
-    #         print("you")
+    #     find_counts += 1
-    # elif ret2:
+    #     if record(label):
-    #     print(error2)
+    #         car_stop()
-    #     if abs(error2) < 30:
+    # if find_counts >= 5:
-    #         print("zuo")
+    #     car_stop()
+    ret, box = filter.get(label)
+    while not ret:
+        ret, box = filter.get(label)
+    width = box[0][2] - box[0][0]
+    logger.info(width)
+    # error = (box[0][2] + box[0][0] - 320) / 2 + offset
+    # explore_calibrate_new(tlabel.LPILLER, offset = 10, run_direc = 1, run_speed = 5)
+    # calibrate_new(label, offset = 15, run = True)
+    # car_stop()
+    # time.sleep(0.5)
+    # for _ in range(3):
+    #     calibrate_right_new(tlabel.BBALL, offset = 27, run = True, run_speed = 4)
+    #     logger.info("抓蓝色球")
+    #     time.sleep(5)
+    # logger.info("抓取块")
+    # time.sleep(0.1)
+

test/test_llm.py

@@ -14,7 +14,7 @@ class LLM:
         
         self.model = 'ernie-3.5'
         self.prompt = '''你是一个机器人动作规划者，需要把我的话翻译成机器人动作规划并生成对应的 json 结果，机器人工作空间参考右手坐标系。
-                    严格按照下面的描述生成给定格式 json，从现在开始你仅仅给我返回 json 数据！'''
+                    严格按照下面的描述生成给定格式 json，从现在开始你仅仅给我返回 json 数据'''
         self.prompt += '''正确的示例如下：
                         向左移 0.1m, 向左转弯 85 度  [{'func': 'move', 'x': 0, 'y': 0.1},{'func': 'turn','angle': -85}],
                         向右移 0.2m, 向前 0.1m [{'func': 'move', 'x': 0, 'y': -0.2},{'func': 'move', 'x': 0.1, 'y': 0}],
@@ -22,6 +22,8 @@ class LLM:
                         原地左转 38 度 [{'func': 'turn','angle': -38}],
                         蜂鸣器发声 5 秒 [{'func': 'beep', 'time': 5}]
                         发光或者照亮 5 秒 [{'func': 'light', 'time': 5}]
+                        向右走 30cm，照亮 2s [{'func': 'move', 'x': 0, 'y': -0.3}, {'func': 'light', 'time': 2}],
+                        向左移 0.2m, 向后 0.1m [{'func': 'move', 'x': 0, 'y': 0.2},{'func': 'move', 'x': -0.1, 'y': 0}],
                     '''
         self.messages = []
         self.resp = None
@@ -47,6 +49,8 @@ if __name__ == "__main__":
     lmm_bot = LLM()
     while True:
         chat = input("输入：")
-        print(lmm_bot.get_command_json(chat))
+        resp = lmm_bot.get_command_json(chat).replace(' ', '').replace('\n', '').replace('\t', '')
+
+        print(eval(resp[7:-3]))
 
 

utils.py

@@ -256,6 +256,8 @@ class LLM:
                         原地左转 38 度 [{'func': 'turn','angle': -38}],
                         蜂鸣器发声 5 秒 [{'func': 'beep', 'time': 5}]
                         发光或者照亮 5 秒 [{'func': 'light', 'time': 5}]
+                        向右走 30cm，照亮 2s [{'func': 'move', 'x': 0, 'y': -0.3}, {'func': 'light', 'time': 2}],
+                        向左移 0.2m, 向后 0.1m [{'func': 'move', 'x': 0, 'y': 0.2},{'func': 'move', 'x': -0.1, 'y': 0}],
                     '''
         self.prompt += '''你无需回复我'''
         self.messages = []
@@ -277,7 +279,8 @@ class LLM:
                         messages=self.messages,
         )
         self.messages.append(self.resp.to_message())
-        return self.resp.get_result()
+        resp = self.resp.get_result().replace(' ', '').replace('\n', '').replace('\t', '')
+        return resp[7:-3]
 
 class CountRecord:
     def __init__(self, stop_count=2) -> None: