diff --git a/cfg_subtask.toml b/cfg_subtask.toml index 6ad28f2..88671b0 100644 --- a/cfg_subtask.toml +++ b/cfg_subtask.toml @@ -35,7 +35,7 @@ pid_ki = 0 pid_kd = 0 [put_hanoi2] -pid_kp = 1 +pid_kp = 1.0 pid_ki = 0 pid_kd = 0 pos_gap = 160 diff --git a/main_upper.py b/main_upper.py index 1507330..5cfd16b 100644 --- a/main_upper.py +++ b/main_upper.py @@ -45,6 +45,7 @@ def main_func(_queue, _skip_queue): act.axis.x2(140) act.axis.storage(20) act.axis.scoop(25) + act.axis.claw_arm(225) act.axis.exec() logger.info(cfg_main) diff --git a/majtask.py b/majtask.py index e1e642b..e943fe9 100644 --- a/majtask.py +++ b/majtask.py @@ -96,7 +96,11 @@ class main_task(): # pid_out = -pid_out # 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") + 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 47a4e9f..083ef9f 100644 --- a/subtask.py +++ b/subtask.py @@ -431,10 +431,10 @@ class get_block1(): by_cmd.send_position_axis_z(30, 130) time.sleep(1) by_cmd.send_position_axis_x(1, 140) - by_cmd.send_angle_claw_arm(220) + by_cmd.send_angle_claw_arm(225) time.sleep(0.5) - by_cmd.send_angle_storage(55) - time.sleep(1) + # by_cmd.send_angle_storage(55) + # time.sleep(1) pass def nexec(self): @@ -466,7 +466,7 @@ class get_block2(): calibrate_new(var.second_block, offset = 15, run = True, run_speed = 5) logger.info("抓取块") time.sleep(0.5) - by_cmd.send_angle_claw_arm(220) + by_cmd.send_angle_claw_arm(225) by_cmd.send_angle_claw(63) by_cmd.send_position_axis_z(30, 60) time.sleep(1) @@ -535,7 +535,7 @@ class put_block(): time.sleep(1) by_cmd.send_position_axis_z(30, 110) time.sleep(1) - by_cmd.send_angle_claw_arm(220) + by_cmd.send_angle_claw_arm(225) time.sleep(1) by_cmd.send_position_axis_z(30, 0) time.sleep(0.5) @@ -554,7 +554,7 @@ class put_block(): time.sleep(1) while by_cmd.send_position_axis_x(1, 0) == -1: pass - while by_cmd.send_angle_claw_arm(36) == -1: + while by_cmd.send_angle_claw_arm(45) == -1: pass # 任务检查间隔 # time.sleep(2) @@ -569,7 +569,7 @@ class get_bball(): # time.sleep(0.5) # by_cmd.send_position_axis_x(1, 0) # time.sleep(2) - # by_cmd.send_angle_claw_arm(36) + # by_cmd.send_angle_claw_arm(45) while (by_cmd.send_angle_storage(0) == -1): by_cmd.send_angle_storage(0) @@ -582,13 +582,14 @@ class get_bball(): self.record = CountRecord(5) def find(self): # 目标检测蓝球 - ret = filter.find(tlabel.BBALL) + # ret = filter.find(tlabel.BBALL) + ret = filter.find_mult([tlabel.BBALL, tlabel.YBALL]) # ret = filter.find(tlabel.BBALL) or filter.find(tlabel.YBALL) # TODO 此处使用连续检出判断感觉不是很好，黄球放置远时停车较晚，可能跟请求速度相关 # if ret: # if self.record(tlabel.BBALL): # return True - if ret: + if ret[0] or ret[1]: return True return False def exec(self): @@ -599,7 +600,7 @@ class get_bball(): 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) + by_cmd.send_angle_claw_arm(45) by_cmd.send_angle_claw(54) by_cmd.send_position_axis_x(1, 160) time.sleep(1.2) @@ -615,7 +616,7 @@ class get_bball(): time.sleep(0.5) by_cmd.send_angle_claw(54) time.sleep(0.5) - by_cmd.send_angle_claw_arm(36) + by_cmd.send_angle_claw_arm(45) time.sleep(1) by_cmd.send_position_axis_z(30, 135) # 继续向前走 @@ -845,19 +846,19 @@ class put_hanoi1(): # by_cmd.send_distance_x(10, 200) # by_cmd.send_distance_x(10, 180) - by_cmd.send_distance_x(10, 250) - time.sleep(1) + by_cmd.send_distance_x(10, 180) + time.sleep(1.5) car_stop() # 根据方向初始化执行器位置 if utils.direction is tlabel.RMARK: # FIXME 右侧的爪子会被 storage 挡住 by_cmd.send_position_axis_x(1, 0) - by_cmd.send_angle_claw_arm(36) + by_cmd.send_angle_claw_arm(45) by_cmd.send_angle_storage(0) else: by_cmd.send_position_axis_x(1, 150) - by_cmd.send_angle_claw_arm(220) + by_cmd.send_angle_claw_arm(225) by_cmd.send_angle_storage(55) time.sleep(1) @@ -884,11 +885,16 @@ class put_hanoi1(): def nexec(self): pass def after(self): - var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"]) + 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"]) + else: + var.pid_turning.set(cfg["put_hanoi1"]["pid_kp"], cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"]) pass class put_hanoi2(): def __init__(self): + if cfg['put_hanoi2']['first_target'] == "lp": self.target_label = tlabel.LPILLER elif cfg['put_hanoi2']['first_target'] == "mp": @@ -897,6 +903,7 @@ class put_hanoi2(): self.target_label = tlabel.SPILLER def init(self): logger.info("物资盘点 2 初始化") + var.task_speed = 8.5 if utils.direction == tlabel.RMARK: # 15 self.offset = 19 @@ -910,7 +917,6 @@ class put_hanoi2(): # ret, box = filter.get(self.target_label) ret, box = filter.get(tlabel.TPLATFORM) if ret: - var.task_speed = 8.5 error = (box[0][2] + box[0][0] - 320) / 2 + self.platform_offset if error > 0: return True @@ -1094,9 +1100,9 @@ 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) - car_stop() + # by_cmd.send_speed_y(15) + # time.sleep(0.1) + # car_stop() pass else: by_cmd.send_position_axis_z(30, 170) @@ -1116,6 +1122,7 @@ class put_hanoi2(): 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 pass class put_hanoi3(): @@ -1130,7 +1137,7 @@ class put_hanoi3(): while by_cmd.send_position_axis_x(1, 0) == -1: pass time.sleep(1) - # while by_cmd.send_angle_claw_arm(220) == -1: + # while by_cmd.send_angle_claw_arm(225) == -1: # pass # while by_cmd.send_angle_claw(90) == -1: # pass @@ -1209,7 +1216,7 @@ class move_area1(): # 任务检查间隔 by_cmd.send_position_axis_x(1, 150) # time.sleep(1) - by_cmd.send_angle_claw_arm(220) + by_cmd.send_angle_claw_arm(225) pass @@ -1387,11 +1394,12 @@ class kick_ass(): 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_angle_claw_arm(220) + # by_cmd.send_angle_claw_arm(225) # time.sleep(0.5) by_cmd.send_position_axis_x(1, 20) time.sleep(3) diff --git a/variable.py b/variable.py index 8565a41..a79b4b8 100644 --- a/variable.py +++ b/variable.py @@ -16,4 +16,6 @@ llm_text = '' skip_llm_task_flag = False first_block = None -second_block = None \ No newline at end of file +second_block = None + +switch_lane_model = False \ No newline at end of file