diff --git a/cfg_subtask.toml b/cfg_subtask.toml index 2f538a5..8a02906 100644 --- a/cfg_subtask.toml +++ b/cfg_subtask.toml @@ -1,5 +1,5 @@ [get_block] -pid_kp = 1.5 +pid_kp = 1.0 pid_ki = 0 pid_kd = 0 first_block = "blue" @@ -15,7 +15,7 @@ pid_ki = 0 pid_kd = 0 [up_tower] -pid_kp = 1.3 +pid_kp = 1.0 pid_ki = 0 pid_kd = 0 @@ -25,12 +25,12 @@ pid_ki = 0 pid_kd = 0 [put_bball] -pid_kp = 1.2 +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 @@ -53,7 +53,7 @@ pid_kd = 0 llm_enable = true [kick_ass] -pid_kp = 1.2 +pid_kp = 0.8 pid_ki = 0 pid_kd = 0 pos_gap1 = 150 diff --git a/subtask.py b/subtask.py index 530b1fd..ff32c6d 100644 --- a/subtask.py +++ b/subtask.py @@ -557,7 +557,7 @@ class put_block(): ret, box = filter.get(tlabel.HOSPITAL) if ret > 0: width = box[0][2] - box[0][0] - if width > 135: + if width > 130: #135 return True return False def exec(self): @@ -568,7 +568,7 @@ class put_block(): by_cmd.send_distance_x(10, 100) by_cmd.send_position_axis_z(30, 0) time.sleep(0.5) - by_cmd.send_position_axis_x(1, 50) # 20 + by_cmd.send_position_axis_x(1, 30) # 20 time.sleep(1) by_cmd.send_angle_claw(63) time.sleep(1) @@ -593,7 +593,7 @@ class put_block(): time.sleep(1) by_cmd.send_position_axis_z(30, 0) time.sleep(0.5) - by_cmd.send_position_axis_x(1, 50) + by_cmd.send_position_axis_x(1, 30) time.sleep(1.5) by_cmd.send_angle_claw(45) time.sleep(1) @@ -770,8 +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 - # time.sleep(1.5) + 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) @@ -779,9 +779,6 @@ 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("抓红球") @@ -829,14 +826,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(2) + time.sleep(1) # by_cmd.send_distance_y(10, 55) by_cmd.send_angle_storage(20) # time.sleep(1) @@ -962,7 +959,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.3, cfg["put_hanoi1"]["pid_ki"], cfg["put_hanoi1"]["pid_kd"]) + 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 @@ -1112,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, 140) + by_cmd.send_position_axis_z(30, 150) time.sleep(2) by_cmd.send_position_axis_x(1, 150) time.sleep(2) @@ -1124,7 +1121,7 @@ class put_hanoi2(): time.sleep(1) pass else: - by_cmd.send_position_axis_z(30, 140) + by_cmd.send_position_axis_z(30, 150) time.sleep(2) by_cmd.send_position_axis_x(1, 40) time.sleep(2) @@ -1173,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, 200) # 170 #190(new) + by_cmd.send_position_axis_z(30, 210) # 170 #190(new) time.sleep(1.5) by_cmd.send_position_axis_x(1, 150) time.sleep(2) @@ -1189,7 +1186,7 @@ class put_hanoi2(): # car_stop() pass else: - by_cmd.send_position_axis_z(30, 200) + by_cmd.send_position_axis_z(30, 210) time.sleep(1.5) by_cmd.send_position_axis_x(1, 40) time.sleep(2) @@ -1540,6 +1537,14 @@ class move_area2(): if len(resp_commands) == 0: return action_list = resp_commands + # 先检查一下 action 是否生成正确，如果不正确直接跳过 + actions_keys = self.action_dict.keys() + try: + for action in action_list: + if not (action.get('action') in actions_keys): + return + except: + return # 进入停车区域 by_cmd.send_distance_y(10, 450) time.sleep((450 * 5 / 1000) + 0.5) @@ -1610,44 +1615,26 @@ class kick_ass(): 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) + + # 先移动到第一个人的地方 + by_cmd.send_distance_x(10, self.pos_gap1) + time.sleep(1.5) if self.target_person == 1: - 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() + # target_distance = self.pos_gap1 + pass else: - 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() + # 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 - 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) diff --git a/subtask_8822.py b/subtask_8822.py index 530b1fd..ee3602e 100644 --- a/subtask_8822.py +++ b/subtask_8822.py @@ -1112,7 +1112,7 @@ class put_hanoi2(): time.sleep(0.5) logger.info("放中平台") if utils.direction is tlabel.RMARK: - by_cmd.send_position_axis_z(30, 140) + by_cmd.send_position_axis_z(30, 150) time.sleep(2) by_cmd.send_position_axis_x(1, 150) time.sleep(2) @@ -1124,7 +1124,7 @@ class put_hanoi2(): time.sleep(1) pass else: - by_cmd.send_position_axis_z(30, 140) + by_cmd.send_position_axis_z(30, 150) time.sleep(2) by_cmd.send_position_axis_x(1, 40) time.sleep(2) @@ -1173,7 +1173,7 @@ class put_hanoi2(): time.sleep(0.5) logger.info("放小平台") if utils.direction is tlabel.RMARK: - by_cmd.send_position_axis_z(30, 200) # 170 #190(new) + by_cmd.send_position_axis_z(30, 210) # 170 #190(new) time.sleep(1.5) by_cmd.send_position_axis_x(1, 150) time.sleep(2) @@ -1189,7 +1189,7 @@ class put_hanoi2(): # car_stop() pass else: - by_cmd.send_position_axis_z(30, 200) + by_cmd.send_position_axis_z(30, 210) time.sleep(1.5) by_cmd.send_position_axis_x(1, 40) time.sleep(2) @@ -1610,44 +1610,26 @@ class kick_ass(): 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) + + # 先移动到第一个人的地方 + by_cmd.send_distance_x(10, self.pos_gap1) + time.sleep(1.5) if self.target_person == 1: - 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() + # target_distance = self.pos_gap1 + pass else: - 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() + # 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 - 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) @@ -1669,6 +1651,8 @@ class kick_ass(): # by_cmd.send_speed_x(25) # time.sleep(4) pass + + def nexec(self): pass def after(self): diff --git a/subtask_raw.py b/subtask_raw.py index a986fb1..ff32c6d 100644 --- a/subtask_raw.py +++ b/subtask_raw.py @@ -557,7 +557,7 @@ class put_block(): ret, box = filter.get(tlabel.HOSPITAL) if ret > 0: width = box[0][2] - box[0][0] - if width > 135: + if width > 130: #135 return True return False def exec(self): @@ -568,7 +568,7 @@ class put_block(): by_cmd.send_distance_x(10, 100) by_cmd.send_position_axis_z(30, 0) time.sleep(0.5) - by_cmd.send_position_axis_x(1, 50) # 20 + by_cmd.send_position_axis_x(1, 30) # 20 time.sleep(1) by_cmd.send_angle_claw(63) time.sleep(1) @@ -593,7 +593,7 @@ class put_block(): time.sleep(1) by_cmd.send_position_axis_z(30, 0) time.sleep(0.5) - by_cmd.send_position_axis_x(1, 50) + by_cmd.send_position_axis_x(1, 30) time.sleep(1.5) by_cmd.send_angle_claw(45) time.sleep(1) @@ -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) @@ -1018,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, 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) @@ -1030,7 +1030,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) @@ -1078,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, 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) @@ -1090,7 +1090,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) @@ -1109,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, 140) + by_cmd.send_position_axis_z(30, 150) time.sleep(2) by_cmd.send_position_axis_x(1, 150) time.sleep(2) @@ -1121,7 +1121,7 @@ class put_hanoi2(): time.sleep(1) pass else: - by_cmd.send_position_axis_z(30, 140) + by_cmd.send_position_axis_z(30, 150) time.sleep(2) by_cmd.send_position_axis_x(1, 40) time.sleep(2) @@ -1139,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, 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) @@ -1151,7 +1151,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) @@ -1170,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, 200) # 170 #190(new) + by_cmd.send_position_axis_z(30, 210) # 170 #190(new) time.sleep(1.5) by_cmd.send_position_axis_x(1, 150) time.sleep(2) @@ -1186,7 +1186,7 @@ class put_hanoi2(): # car_stop() pass else: - by_cmd.send_position_axis_z(30, 200) + by_cmd.send_position_axis_z(30, 210) time.sleep(1.5) by_cmd.send_position_axis_x(1, 40) time.sleep(2) @@ -1537,6 +1537,14 @@ class move_area2(): if len(resp_commands) == 0: return action_list = resp_commands + # 先检查一下 action 是否生成正确，如果不正确直接跳过 + actions_keys = self.action_dict.keys() + try: + for action in action_list: + if not (action.get('action') in actions_keys): + return + except: + return # 进入停车区域 by_cmd.send_distance_y(10, 450) time.sleep((450 * 5 / 1000) + 0.5) @@ -1607,44 +1615,26 @@ class kick_ass(): 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) + + # 先移动到第一个人的地方 + by_cmd.send_distance_x(10, self.pos_gap1) + time.sleep(1.5) if self.target_person == 1: - 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() + # target_distance = self.pos_gap1 + pass else: - 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() + # 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 - 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)