diff --git a/app.py b/app.py index 249671a..064db04 100644 --- a/app.py +++ b/app.py @@ -15,8 +15,9 @@ from main_upper import main_func server_command = [ {"path": "/home/evan/Workplace/project_capture/build/", "script": "./capture"}, {"path": "/home/evan/Workplace/project_infer/lane_server/", "script": "lane_infer_server.py"}, + {"path": "/home/evan/Workplace/project_infer/lane_server/", "script": "lane_infer_server1.py"}, {"path": "/home/evan/Workplace/project_infer/yolo_server/", "script": "yolo_infer_server.py"}, - # {"path": "/home/evan/Workplace/project_infer/ocr_server/", "script": "ocr_infer_server.py"}, + ] processes = [] diff --git a/majtask.py b/majtask.py index e943fe9..8479108 100644 --- a/majtask.py +++ b/majtask.py @@ -8,9 +8,14 @@ import variable as var class main_task(): def __init__(self,by_cmd): + # lane infer server self.context = zmq.Context() self.socket = self.context.socket(zmq.REQ) self.socket.connect("tcp://localhost:6666") + # lane infer server1 + self.context1 = zmq.Context() + self.socket1 = self.context.socket(zmq.REQ) + self.socket1.connect("tcp://localhost:6669") # 赛道回归相关 self.x = 0 @@ -97,10 +102,9 @@ class main_task(): # logger.debug(f"err={self.lane_error}, pwm out={pid_out}") self.by_cmd.send_speed_omega(pid_out) if var.switch_lane_model: - self.socket.send_string("1") + self.socket1.send_string("") + resp = self.socket1.recv_pyobj() + else: + self.socket.send_string("") resp = self.socket.recv_pyobj() - var.switch_lane_model = False - return - self.socket.send_string("") - resp = self.socket.recv_pyobj() self.parse_data(resp) diff --git a/subtask.py b/subtask.py index 083ef9f..41b6f54 100644 --- a/subtask.py +++ b/subtask.py @@ -118,7 +118,7 @@ def calibrate_right_new(label, offset, run = True, run_speed = 3.5): logger.info(f"calibrate_right_new:停车后的误差是{error}") if abs(error) > 8: logger.info(f"calibrate_right_new:停车后的误差大于 8 使用 distance 校准") - error = error * 3 + error = error * 2.3 if error > 0: by_cmd.send_distance_x(-10, int(error)) else: @@ -406,7 +406,7 @@ class get_block1(): else: var.first_block = tlabel.BBLOCK var.second_block = tlabel.RBLOCK - calibrate_new(var.first_block, offset = 15, run = True, run_speed = 5) + calibrate_new(var.first_block, offset = 16, run = True, run_speed = 5) logger.info("抓取块") by_cmd.send_position_axis_z(30, 60) @@ -421,10 +421,10 @@ class get_block1(): by_cmd.send_angle_claw_arm(175) time.sleep(0.1) - by_cmd.send_position_axis_x(1, 120) + by_cmd.send_position_axis_x(1, 100) time.sleep(1) by_cmd.send_position_axis_z(30, 70) - time.sleep(0.1) + time.sleep(0.5) by_cmd.send_angle_claw(63) time.sleep(0.5) @@ -435,6 +435,7 @@ class get_block1(): time.sleep(0.5) # by_cmd.send_angle_storage(55) # time.sleep(1) + by_cmd.send_position_axis_z(30, 60) pass def nexec(self): @@ -463,13 +464,12 @@ class get_block2(): return False def exec(self): car_stop() - calibrate_new(var.second_block, offset = 15, run = True, run_speed = 5) + calibrate_new(var.second_block, offset = 16, run = True, run_speed = 5) logger.info("抓取块") time.sleep(0.5) by_cmd.send_angle_claw_arm(225) by_cmd.send_angle_claw(63) - by_cmd.send_position_axis_z(30, 60) - time.sleep(1) + time.sleep(0.1) by_cmd.send_position_axis_x(1, 20) time.sleep(1) by_cmd.send_angle_claw(25) @@ -521,7 +521,7 @@ class put_block(): # 放置第二個塊 by_cmd.send_angle_storage(20) - by_cmd.send_position_axis_x(1, 130) + by_cmd.send_position_axis_x(1, 110) by_cmd.send_position_axis_z(30, 120) time.sleep(1.5) by_cmd.send_angle_claw_arm(180) @@ -597,7 +597,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 = 5) + calibrate_right_new(tlabel.BBALL, offset = 18, run = True, run_speed = 5) logger.info("抓蓝色球") time.sleep(0.5) by_cmd.send_angle_claw_arm(45) @@ -835,20 +835,30 @@ class put_hanoi1(): logger.info("应该向左转") - # # 校准 omega - # if error > 0: - # by_cmd.send_angle_omega(-20,abs(var.lane_error)) - # else: - # by_cmd.send_angle_omega(20,abs(var.lane_error)) - # time.sleep(0.5) - # car_stop() - # time.sleep(0.5) - # by_cmd.send_distance_x(10, 200) - - # by_cmd.send_distance_x(10, 180) - by_cmd.send_distance_x(10, 180) - time.sleep(1.5) + # 校准 omega + for _ in range(10): + ret, box = filter.get(utils.direction) + if ret: + error = (box[0][2] + box[0][0] - 320) / 2 + by_cmd.send_speed_omega(-error * 0.8) + time.sleep(0.2) car_stop() + + + # 前进 + # by_cmd.send_distance_x(10, 200) + # by_cmd.send_distance_x(10, 180) + # by_cmd.send_distance_x(10, 180) + # time.sleep(1.5) + # car_stop() + + while True: + by_cmd.send_speed_x(8.5) + ret, box = filter.get(utils.direction) + if ret: + if abs(box[0][2] - box[0][0]) > 41: + car_stop() + break # 根据方向初始化执行器位置 if utils.direction is tlabel.RMARK: @@ -867,16 +877,18 @@ class put_hanoi1(): if utils.direction_right > utils.direction_left: utils.direction = tlabel.RMARK # by_cmd.send_angle_omega(-25,430) - by_cmd.send_angle_omega(-45,238) # by_cmd.send_angle_omega(-55,194) + # by_cmd.send_angle_omega(-45,238) + by_cmd.send_angle_omega(-45,252) time.sleep(2) while (by_cmd.send_angle_camera(90) == -1): by_cmd.send_angle_camera(90) else: utils.direction = tlabel.LMARK # by_cmd.send_angle_omega(25,430) - by_cmd.send_angle_omega(45,238) # by_cmd.send_angle_omega(55,194) + # by_cmd.send_angle_omega(45,238) + by_cmd.send_angle_omega(45,252) time.sleep(2) while (by_cmd.send_angle_camera(0) == -1): by_cmd.send_angle_camera(0) @@ -903,16 +915,16 @@ class put_hanoi2(): self.target_label = tlabel.SPILLER def init(self): logger.info("物资盘点 2 初始化") - var.task_speed = 8.5 + var.task_speed = 10 if utils.direction == tlabel.RMARK: # 15 - self.offset = 19 + self.offset = 14 # self.platform_offset = -25 - self.platform_offset = -10 + self.platform_offset = -19 else: - self.offset = 10 + self.offset = 14 #self.platform_offset = -30 - self.platform_offset = -15 + self.platform_offset = -19 def find(self): # ret, box = filter.get(self.target_label) ret, box = filter.get(tlabel.TPLATFORM) @@ -941,7 +953,7 @@ class put_hanoi2(): logger.info("抓大平台") if utils.direction is tlabel.RMARK: by_cmd.send_position_axis_z(30, 10) - by_cmd.send_position_axis_x(1, 130) + by_cmd.send_position_axis_x(1, 150) by_cmd.send_angle_claw(63) time.sleep(2) by_cmd.send_angle_claw(40) @@ -970,7 +982,7 @@ class put_hanoi2(): time.sleep(0.5) logger.info("放大平台") if utils.direction is tlabel.RMARK: - by_cmd.send_position_axis_x(1, 130) + by_cmd.send_position_axis_x(1, 150) time.sleep(1.5) by_cmd.send_distance_axis_z(30, -20) time.sleep(1) @@ -1001,7 +1013,7 @@ class put_hanoi2(): logger.info("抓中平台") if utils.direction is tlabel.RMARK: by_cmd.send_position_axis_z(30, 10) - by_cmd.send_position_axis_x(1, 130) + by_cmd.send_position_axis_x(1, 150) by_cmd.send_angle_claw(63) time.sleep(2) by_cmd.send_angle_claw(35) @@ -1030,9 +1042,9 @@ class put_hanoi2(): time.sleep(0.5) logger.info("放中平台") if utils.direction is tlabel.RMARK: - by_cmd.send_position_axis_z(30, 100) + by_cmd.send_position_axis_z(30, 120) time.sleep(2) - by_cmd.send_position_axis_x(1, 130) + by_cmd.send_position_axis_x(1, 150) time.sleep(2) by_cmd.send_distance_axis_z(30, -20) time.sleep(0.5) @@ -1042,7 +1054,7 @@ class put_hanoi2(): time.sleep(1) pass else: - by_cmd.send_position_axis_z(30, 100) + by_cmd.send_position_axis_z(30, 120) time.sleep(2) by_cmd.send_position_axis_x(1, 40) time.sleep(2) @@ -1061,7 +1073,7 @@ class put_hanoi2(): logger.info("抓小平台") if utils.direction is tlabel.RMARK: by_cmd.send_position_axis_z(30, 10) - by_cmd.send_position_axis_x(1, 130) + by_cmd.send_position_axis_x(1, 150) by_cmd.send_angle_claw(50) time.sleep(2) by_cmd.send_angle_claw(27) @@ -1090,9 +1102,9 @@ class put_hanoi2(): time.sleep(0.5) logger.info("放小平台") if utils.direction is tlabel.RMARK: - by_cmd.send_position_axis_z(30, 170) + by_cmd.send_position_axis_z(30, 190) # 170 time.sleep(1.5) - by_cmd.send_position_axis_x(1, 130) + by_cmd.send_position_axis_x(1, 150) time.sleep(2) by_cmd.send_distance_axis_z(30, -20) time.sleep(0.5) @@ -1100,12 +1112,13 @@ class put_hanoi2(): time.sleep(0.5) by_cmd.send_position_axis_x(1, 10) time.sleep(1) - # by_cmd.send_speed_y(15) - # time.sleep(0.1) + + # by_cmd.send_speed_y(10) + # time.sleep(0.12) # car_stop() pass else: - by_cmd.send_position_axis_z(30, 170) + by_cmd.send_position_axis_z(30, 190) time.sleep(1.5) by_cmd.send_position_axis_x(1, 40) time.sleep(2) @@ -1121,8 +1134,12 @@ class put_hanoi2(): def nexec(self): pass def after(self): - var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) - var.switch_lane_model = True + var.switch_lane_model = False + if utils.direction is tlabel.RMARK: + var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"] - 0.2, cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) + else: + var.pid_turning.set(cfg["put_hanoi2"]["pid_kp"], cfg["put_hanoi2"]["pid_ki"], cfg["put_hanoi2"]["pid_kd"]) + pass class put_hanoi3():