diff --git a/data/config.toml b/data/config.toml index 828d5617..6019fe33 100644 --- a/data/config.toml +++ b/data/config.toml @@ -40,17 +40,39 @@ ip = '127.0.0.1' port = 10302 [soccer] +chip_tune_coef = 2.5 def_ball_distance_coef = 0.69999999999999996 def_max_extension_area = 1100.0 def_prediction_time = 0.5 def_tight_start_angle = 30.0 gk_tight_start_angle = 15.0 +kick_tune_coef = 1.0 +mark_distance = 250.0 mark_in_stop = true one_touch_beta = 0.40000000000000002 one_touch_gamma = 0.14000000000000001 one_touch_shoot_k = 4000.0 -kick_tune_coef = 1.0 -chip_tune_coef = 2.5 + + [soccer.velocity_profile.aroom] + acceleration = 2160.0 + deceleration = 2700.0 + speed = 900.0 + + [soccer.velocity_profile.kharaki] + acceleration = 4010.0 + deceleration = 4860.0 + speed = 2200.0 + + [soccer.velocity_profile.mamooli] + acceleration = 3500.0 + deceleration = 3850.0 + speed = 1800.0 + + [soccer.velocity_profile.sooski] + acceleration = 1620.0 + deceleration = 2700.0 + speed = 450.0 + [[soccer.robot_physical_status]] has_chip_kick = true has_direct_kick = true @@ -130,7 +152,7 @@ chip_tune_coef = 2.5 [[soccer.robot_physical_status]] has_chip_kick = false - has_direct_kick = true + has_direct_kick = false has_dribbler = false id = 11 is_3D_printed = true diff --git a/data/strategy.ims b/data/strategy.ims index a2108fba..41d8d9c5 100644 Binary files a/data/strategy.ims and b/data/strategy.ims differ diff --git a/source/common b/source/common index b6767872..92a798f9 160000 --- a/source/common +++ b/source/common @@ -1 +1 @@ -Subproject commit b67678729b52f15d80b5b83712a0a3b5f6765fd7 +Subproject commit 92a798f9f3855d572f033c5093c70b7e249c9976 diff --git a/source/sender/command.h b/source/sender/command.h index 85520541..d50bf10c 100644 --- a/source/sender/command.h +++ b/source/sender/command.h @@ -12,13 +12,13 @@ struct Command Common::Angle current_angle; Common::Angle target_angle; - int shoot = 0; - int chip = 0; - int dribbler = 0; + float shoot = 0; + float chip = 0; + float dribbler = 0; Command() = default; - Command(const Protos::Immortals::Command &t_command) + explicit Command(const Protos::Immortals::Command &t_command) { vision_id = t_command.vision_id(); diff --git a/source/sender/nrf.cpp b/source/sender/nrf.cpp index 2615512f..ccd5ca06 100644 --- a/source/sender/nrf.cpp +++ b/source/sender/nrf.cpp @@ -50,8 +50,6 @@ static float getCalibratedShootPow(float t_raw_shoot, const Common::Vec2 &t_coef return 0; } - t_raw_shoot = std::clamp(t_raw_shoot, 0.0f, 6500.f) / 1000.f; - float calib_shoot = t_coeffs.x * t_raw_shoot + t_coeffs.y; calib_shoot = std::clamp(calib_shoot, 0.0f, 100.0f); @@ -102,15 +100,19 @@ void Nrf::queueCommand(const Command &command) convert_float_to_2x_buff(data + 9, command.current_angle.deg()); if (command.shoot > 0) { + const float raw_shoot = std::clamp(command.shoot, 0.0f, 6500.f) / 1000.f; + const float calibrated_shoot = getCalibratedShootPow(raw_shoot, shoot_coeffs[command.vision_id]); - data[11] = static_cast (getCalibratedShootPow(Common::config().soccer.kick_tune_coef * command.shoot, - shoot_coeffs[command.vision_id])); + data[11] = static_cast(calibrated_shoot); data[12] = 0x00; } else if (command.chip > 0) { + const float raw_chip = std::clamp(command.chip, 0.0f, 150.f); + const float calibrated_chip = getCalibratedShootPow(raw_chip, chip_coeffs[command.vision_id]); + data[11] = 0x00; - data[12] = getCalibratedShootPow(Common::config().soccer.chip_tune_coef * command.chip, chip_coeffs[command.vision_id]); + data[12] = static_cast(calibrated_chip); } else { diff --git a/source/soccer/ai.h b/source/soccer/ai.h index 42f412cd..28477d4c 100644 --- a/source/soccer/ai.h +++ b/source/soccer/ai.h @@ -188,7 +188,7 @@ class Ai void gkHi(int t_robot_num); void runningDef(int t_robot_num, Common::Vec2 t_target, Common::Vec2 *t_defend_target); void defenceWall(int t_robot_num, bool t_kick_off = false); - void attacker(int t_robot_num, Common::Angle t_angle, float t_kick, int t_chip, bool t_kiss, bool t_dribbler); + void attacker(int t_robot_num, Common::Angle t_angle, float t_kick, int t_chip, bool t_kiss, bool t_dribbler, bool precise = false); void waitForPass(int t_robot_num, bool t_chip = false, const Common::Vec2 *t_target = nullptr, Common::Vec2 *t_stat_pos = nullptr); void waitForOmghi(int t_robot_num); diff --git a/source/soccer/assignment/cost_static.cpp b/source/soccer/assignment/cost_static.cpp index 20ebc345..4cc7a143 100644 --- a/source/soccer/assignment/cost_static.cpp +++ b/source/soccer/assignment/cost_static.cpp @@ -14,7 +14,7 @@ int Ai::staticRoleCost(const int t_robot_idx, const Assignment &t_assignment) co if (t_assignment.needs_chip != physical_status.has_chip_kick) caps_cost += 5000; - int switch_cost = t_robot_idx == *t_assignment.role ? 0 : 500; + int switch_cost = t_robot_idx == *t_assignment.role ? 0 : 200; return dis_cost + caps_cost + switch_cost; } diff --git a/source/soccer/assignment/create_mids.cpp b/source/soccer/assignment/create_mids.cpp index 38e8e6e3..e29cc437 100644 --- a/source/soccer/assignment/create_mids.cpp +++ b/source/soccer/assignment/create_mids.cpp @@ -73,7 +73,11 @@ void Ai::createMidAssignments() const bool shoot = !m_is_defending; const bool chip = !m_is_defending && role == &m_mid5; - createStaticAssignment(role, Assignment::Priority::Low, shoot, chip); + const bool important_dmf = role == &m_mid5 && (m_ref_state.ourBallPlacement() || m_ref_state.ourFreeKick()); + const Assignment::Priority priority = + important_dmf ? Assignment::Priority::High : Assignment::Priority::Low; + + createStaticAssignment(role, priority, shoot, chip); } } } diff --git a/source/soccer/dss/dss.cpp b/source/soccer/dss/dss.cpp index 6cd9a9e8..2d1c2bbc 100644 --- a/source/soccer/dss/dss.cpp +++ b/source/soccer/dss/dss.cpp @@ -27,9 +27,9 @@ bool Dss::collisionWithOwn(const Common::RobotState &state_a, const Common::Vec2 const Common::RobotState &state_b, const Common::Vec2 &cmd_b) const { const Trajectory traj_a = - Trajectory::MakeTrajectory(state_a, cmd_a, m_profile.max_dec, 1.0f / Common::config().vision.vision_frame_rate); + Trajectory::MakeTrajectory(state_a, cmd_a, m_profile.deceleration, 1.0f / Common::config().vision.vision_frame_rate); const Trajectory traj_b = - Trajectory::MakeTrajectory(state_b, cmd_b, m_profile.max_dec, 1.0f / Common::config().vision.vision_frame_rate); + Trajectory::MakeTrajectory(state_b, cmd_b, m_profile.deceleration, 1.0f / Common::config().vision.vision_frame_rate); return Parabolic::HaveOverlap(traj_a.acc, traj_b.acc, Common::field().robot_radius * 2.f) || Parabolic::HaveOverlap(traj_a.dec, traj_b.dec, Common::field().robot_radius * 2.f) || @@ -40,9 +40,9 @@ bool Dss::collisionWithOwn(const Common::RobotState &state_a, const Common::Vec2 bool Dss::collisionWithOpp(const Common::RobotState &state_own, const Common::Vec2 &cmd_own, const Common::RobotState &state_opp) const { - const Trajectory traj_own = Trajectory::MakeTrajectory(state_own, cmd_own, m_profile.max_dec, + const Trajectory traj_own = Trajectory::MakeTrajectory(state_own, cmd_own, m_profile.deceleration, 1.0f / Common::config().vision.vision_frame_rate); - const Trajectory traj_opp = Trajectory::MakeOpponentTrajectory(state_opp, m_profile.max_dec); + const Trajectory traj_opp = Trajectory::MakeOpponentTrajectory(state_opp, m_profile.deceleration); return Parabolic::HaveOverlap(traj_own.acc, traj_opp.dec, Common::field().robot_radius * 2.f) || Parabolic::HaveOverlap(traj_own.dec, traj_opp.dec, Common::field().robot_radius * 2.f) || @@ -53,7 +53,7 @@ bool Dss::collisionWithOpp(const Common::RobotState &state_own, const Common::Ve bool Dss::RobotHasStaticCollision(const Common::RobotState &state, const Common::Vec2 &cmd) const { const Trajectory traj = - Trajectory::MakeTrajectory(state, cmd, m_profile.max_dec, 1.0f / Common::config().vision.vision_frame_rate); + Trajectory::MakeTrajectory(state, cmd, m_profile.deceleration, 1.0f / Common::config().vision.vision_frame_rate); return Parabolic::HasStaticOverlap(traj.acc) || Parabolic::HasStaticOverlap(traj.dec) || Parabolic::HasStaticOverlap(traj.dec); @@ -132,7 +132,7 @@ void Dss::Reset() else { const float dec = - std::min(m_profile.max_dec, state.velocity.length() * Common::config().vision.vision_frame_rate); + std::min(m_profile.deceleration, state.velocity.length() * Common::config().vision.vision_frame_rate); computed_motions[robot_idx] = state.velocity.normalized() * (-dec); } } @@ -144,7 +144,7 @@ Common::Vec2 Dss::ComputeSafeMotion(const int robot_num, const Common::Vec2 &mot // TODO: in simulation setting a lower dec compared // to motion plan results in better avoidance. // Verify on the real field - m_profile.max_dec /= 2.0f; + m_profile.deceleration /= 2.0f; const Common::Vec2 target_a_cmd = GetAccFromMotion(robot_num, motion); const float target_a_cmd_mag = target_a_cmd.length(); @@ -162,7 +162,7 @@ Common::Vec2 Dss::ComputeSafeMotion(const int robot_num, const Common::Vec2 &mot else { const float dec = - std::min(m_profile.max_dec, state.velocity.length() * Common::config().vision.vision_frame_rate); + std::min(m_profile.deceleration, state.velocity.length() * Common::config().vision.vision_frame_rate); a_cmd = state.velocity.normalized() * (-dec); float error = ComputeError(target_a_cmd, a_cmd); diff --git a/source/soccer/errt/errt.cpp b/source/soccer/errt/errt.cpp index 147e29e6..512717b2 100644 --- a/source/soccer/errt/errt.cpp +++ b/source/soccer/errt/errt.cpp @@ -15,7 +15,7 @@ void Planner::init(const Common::Vec2 init, const Common::Vec2 final, const floa m_step_size = step; m_tree.reset(); - m_tree.AddNode(init_state, nullptr); + m_tree.addNode(init_state, nullptr); m_started_in_obs = g_obs_map.isInObstacle(init); } @@ -62,14 +62,14 @@ Node *Planner::extend(Node *s, Common::Vec2 &target) if (g_obs_map.collisionDetect(new_state, s->state)) return nullptr; - return m_tree.AddNode(new_state, s); + return m_tree.addNode(new_state, s); } void Planner::setWayPoints() { m_waypoints.clear(); - Node *n = m_tree.NearestNeighbour(final_state); + Node *n = m_tree.nearestNeighbour(final_state); m_waypoints.push_back(n->state); while (n->parent) @@ -85,28 +85,34 @@ void Planner::setWayPoints() Common::Vec2 Planner::plan() { + if (m_cached_waypoints.empty()) + { + Common::logWarning("cached waypoints are empty"); + } + // return final_state; if (!g_obs_map.collisionDetect(init_state, final_state)) { - m_tree.AddNode(final_state, m_tree.NearestNeighbour(final_state)); + // TODO: slice the path so that the cache contains valid waypoints + m_tree.addNode(final_state, m_tree.nearestNeighbour(final_state)); } else { for (int i = 0; ((i < m_max_nodes) && (!isReached())); i++) { Common::Vec2 r = chooseTarget(); - extend(m_tree.NearestNeighbour(r), r); + extend(m_tree.nearestNeighbour(r), r); } if ((isReached()) && (!g_obs_map.isInObstacle(final_state)) && - (final_state.distanceTo(m_tree.NearestNeighbour(final_state)->state) > 1)) + (final_state.distanceTo(m_tree.nearestNeighbour(final_state)->state) > 1)) { - m_tree.AddNode(final_state, m_tree.NearestNeighbour(final_state)); + m_tree.addNode(final_state, m_tree.nearestNeighbour(final_state)); } } setWayPoints(); - optimize_tree(); + optimizeTree(); if (m_started_in_obs || m_waypoints.size() <= 1) { @@ -118,7 +124,7 @@ Common::Vec2 Planner::plan() } } -void Planner::optimize_tree() +void Planner::optimizeTree() { for (size_t i = 0; i < m_waypoints.size() - 1; i++) { diff --git a/source/soccer/errt/errt.h b/source/soccer/errt/errt.h index 15f0b43f..9abcaf56 100644 --- a/source/soccer/errt/errt.h +++ b/source/soccer/errt/errt.h @@ -8,7 +8,7 @@ namespace Tyr::Soccer class Planner { public: - Planner(int t_max_nodes = 1000); + Planner(int t_max_nodes = 2000); ~Planner() = default; void init(Common::Vec2 init, Common::Vec2 final, float step); @@ -32,7 +32,7 @@ class Planner // TODO: move to setting float goal_target_prob = 0.1f; - float waypoint_target_prob = 0.5f; + float waypoint_target_prob = 0.8f; float acceptable_dis = 90.0f; Common::Random m_random; @@ -41,8 +41,8 @@ class Planner Common::Vec2 randomState() { - return Common::Vec2((m_random.get(-1.0f, 1.0f) * (Common::field().width + 250.0f)), - (m_random.get(-1.0f, 1.0f) * (Common::field().height + 250.0f))); + return Common::Vec2((m_random.get(-1.0f, 1.0f) * (Common::field().width + Common::field().boundary_width)), + (m_random.get(-1.0f, 1.0f) * (Common::field().height + Common::field().boundary_width))); } Common::Vec2 nearestFree(Common::Vec2 state); @@ -51,27 +51,27 @@ class Planner Node *extend(Node *s, Common::Vec2 &target); void setWayPoints(); - Common::Vec2 getWayPoint(const unsigned int i) + Common::Vec2 getWayPoint(const unsigned int i) const { return m_waypoints[i]; } - unsigned int getWayPointNum() + unsigned int getWayPointNum() const { return m_waypoints.size(); } bool isReached() { - return final_state.distanceTo(m_tree.NearestNeighbour(final_state)->state) <= acceptable_dis; + return final_state.distanceTo(m_tree.nearestNeighbour(final_state)->state) <= acceptable_dis; } - void optimize_tree(); + void optimizeTree(); }; inline Common::Vec2 Planner::chooseTarget() { - float r = m_random.get(0.0f, 1.0f); + const float r = m_random.get(0.0f, 1.0f); if (r <= goal_target_prob) { @@ -82,7 +82,6 @@ inline Common::Vec2 Planner::chooseTarget() const int idx = m_random.get(0, static_cast(m_cached_waypoints.size()) - 1); return m_cached_waypoints[idx]; } - else { return randomState(); diff --git a/source/soccer/errt/set_obstacles.cpp b/source/soccer/errt/set_obstacles.cpp index b888676a..2bcb48d4 100644 --- a/source/soccer/errt/set_obstacles.cpp +++ b/source/soccer/errt/set_obstacles.cpp @@ -2,12 +2,15 @@ namespace Tyr::Soccer { +// This is 500 mm in the rules, but we add some extra to avoid static constexpr float ballAreaRadius = 550.0f; // We allow errt points to be 250 mm outside the field, // so set this to some higher value static constexpr float penaltyAreaExtensionBehindGoal = 300.0f; -static constexpr float bigPenaltyAddition = 300.0f; + +// This is 200 mm in the rules, but we add some extra to avoid +static constexpr float bigPenaltyAddition = 220.0f; static float calculateRobotRadius(const Common::RobotState &state) { @@ -15,10 +18,21 @@ static float calculateRobotRadius(const Common::RobotState &state) return Common::field().robot_radius * (1.0f + extension_factor); } +static float calculateOtherRadius(const Common::RobotState &t_current, const Common::RobotState &t_other) +{ + static constexpr float kMaxExtension = 150.0f; + static constexpr float kSpeedToReachMaxExtension = 1500.0f; + + const Common::Vec2 velocity_diff = t_current.velocity - t_other.velocity; + const float extension = + std::clamp(velocity_diff.length() * kMaxExtension / kSpeedToReachMaxExtension, 0.0f, kMaxExtension); + return Common::field().robot_radius + extension; +} + void Ai::setObstacles(const int t_robot_num, const NavigationFlags t_flags) { const bool ourPenalty = t_robot_num != m_gk && !m_ref_state.ourBallPlacement(); - const bool oppPenalty = !m_ref_state.ourBallPlacement(); + const bool oppPenalty = !m_ref_state.ballPlacement(); const bool oppPenaltyBig = m_ref_state.freeKick() || m_ref_state.stop(); @@ -28,6 +42,7 @@ void Ai::setObstacles(const int t_robot_num, const NavigationFlags t_flags) if (t_flags & NavigationFlagsForceNoObstacles) { + Common::logWarning("Robot {} is navigating with no obstacles", t_robot_num); return; } @@ -37,7 +52,8 @@ void Ai::setObstacles(const int t_robot_num, const NavigationFlags t_flags) if ((m_own_robot[i].state().seen_state != Common::SeenState::CompletelyOut) && (i != t_robot_num) && (m_own_robot[i].state().vision_id != m_own_robot[t_robot_num].state().vision_id)) { - g_obs_map.addCircle({m_own_robot[i].state().position, current_robot_radius + Common::field().robot_radius}); + const float radius = calculateRobotRadius(m_own_robot[t_robot_num].state()); + g_obs_map.addCircle({m_own_robot[i].state().position, current_robot_radius + radius}); } } @@ -46,7 +62,10 @@ void Ai::setObstacles(const int t_robot_num, const NavigationFlags t_flags) { if (m_world_state.opp_robot[i].seen_state != Common::SeenState::CompletelyOut) { - const float radius = calculateRobotRadius(m_world_state.opp_robot[i]); + // Don't extend opp robots if we're the attacker + const float radius = t_robot_num == m_attack ? Common::field().robot_radius + : calculateOtherRadius(m_own_robot[t_robot_num].state(), + m_world_state.opp_robot[i]); g_obs_map.addCircle({m_world_state.opp_robot[i].position, radius + current_robot_radius}); } @@ -55,6 +74,8 @@ void Ai::setObstacles(const int t_robot_num, const NavigationFlags t_flags) float ball_radius = 0.0f; if ((t_flags & NavigationFlagsForceBallObstacle) || !m_ref_state.allowedNearBall()) ball_radius = ballAreaRadius; + else if (m_ref_state.ourBallPlacement() && t_robot_num != m_attack && t_robot_num != m_mid5) + ball_radius = ballAreaRadius; else if (t_flags & NavigationFlagsForceBallMediumObstacle) ball_radius = 230.0f; else if (t_flags & NavigationFlagsForceBallSmallObstacle) @@ -94,7 +115,7 @@ void Ai::setObstacles(const int t_robot_num, const NavigationFlags t_flags) if (oppPenaltyBig) { const float big_penalty_area_r = Common::field().penalty_area_depth + bigPenaltyAddition; - const float big_penalty_area_w = Common::field().penalty_area_width + bigPenaltyAddition; + const float big_penalty_area_w = Common::field().penalty_area_width + 2.0f * bigPenaltyAddition; const float big_penalty_area_half_width = big_penalty_area_w / 2.0f; const Common::Vec2 start{-m_side * (Common::field().width + penaltyAreaExtensionBehindGoal), diff --git a/source/soccer/errt/tree.h b/source/soccer/errt/tree.h index 10287989..c3800d6f 100644 --- a/source/soccer/errt/tree.h +++ b/source/soccer/errt/tree.h @@ -13,7 +13,7 @@ struct Node class Tree { public: - Tree(const size_t t_max_nodes) + explicit Tree(const size_t t_max_nodes) { m_nodes.reserve(t_max_nodes); } @@ -23,7 +23,7 @@ class Tree m_nodes.clear(); } - Node *AddNode(const Common::Vec2 s, Node *const parent) + Node *addNode(const Common::Vec2 s, Node *const parent) { if (m_nodes.size() < m_nodes.capacity()) { @@ -44,7 +44,7 @@ class Tree } } - Node *NearestNeighbour(const Common::Vec2 s) + Node *nearestNeighbour(const Common::Vec2 s) { if (m_nodes.empty()) return nullptr; @@ -61,15 +61,15 @@ class Tree } } - return GetNode(ans); + return getNode(ans); } - int tree_size() + int size() const { return m_nodes.size(); } - Node *GetNode(const int idx) + Node *getNode(const int idx) { return &m_nodes[idx]; } diff --git a/source/soccer/plays/corner_their_global.cpp b/source/soccer/plays/corner_their_global.cpp index d2215ae8..6751d699 100644 --- a/source/soccer/plays/corner_their_global.cpp +++ b/source/soccer/plays/corner_their_global.cpp @@ -48,7 +48,7 @@ void Ai::cornerTheirGlobal() } else { - mark(own, opp, 500); + mark(own, opp, Common::config().soccer.mark_distance); } } } diff --git a/source/soccer/plays/kickoff_their_one_wall.cpp b/source/soccer/plays/kickoff_their_one_wall.cpp index 8353d2a1..0ffa8a46 100644 --- a/source/soccer/plays/kickoff_their_one_wall.cpp +++ b/source/soccer/plays/kickoff_their_one_wall.cpp @@ -40,13 +40,13 @@ void Ai::kickoffTheirOneWall() { m_own_robot[m_mid5].face(oppGoal()); navigate(m_mid5, m_world_state.opp_robot[indexN].position.pointOnConnectingLine( - ownGoal(), (std::fabs(m_world_state.opp_robot[indexN].position.x) + 14) * 1.5)); + ownGoal(), (std::fabs(m_world_state.opp_robot[indexN].position.x) + 140) * 2.5)); } if (indexP != -1) { m_own_robot[m_mid1].face(oppGoal()); navigate(m_mid1, m_world_state.opp_robot[indexP].position.pointOnConnectingLine( - ownGoal(), (std::fabs(m_world_state.opp_robot[indexP].position.x) + 14) * 1.5)); + ownGoal(), (std::fabs(m_world_state.opp_robot[indexP].position.x) + 140) * 2.5)); } int zone_idx = 0; diff --git a/source/soccer/plays/normal_play_att.cpp b/source/soccer/plays/normal_play_att.cpp index d32f07f6..77ca84a5 100644 --- a/source/soccer/plays/normal_play_att.cpp +++ b/source/soccer/plays/normal_play_att.cpp @@ -45,7 +45,7 @@ void Ai::normalPlayAtt() const bool pass_angle_ok = (robot.state().position - m_world_state.ball.position) .normalized() - .dot((ownGoal() - m_world_state.ball.position).normalized()) < 0.75f; + .dot((ownGoal() - m_world_state.ball.position).normalized()) < 0.85f; Common::logDebug("mid {} pass angle ok: {}", *mid, pass_angle_ok); @@ -60,7 +60,14 @@ void Ai::normalPlayAtt() } Common::logDebug("open angle: {}", openAngle.magnitude.deg()); - if (openAngle.magnitude.deg() < 8 && (findKickerOpp(-1, 500.0f) == -1) && (suitable_mid != nullptr)) + + static bool ball_is_stationary = false; + const float speed_threshold = ball_is_stationary ? 750.0f : 250.0f; + ball_is_stationary = m_world_state.ball.velocity.length() < speed_threshold; + + const bool opp_attacker_in_range = findKickerOpp(-1, 1000.0f) != -1; + + if (openAngle.magnitude.deg() < 8 && ball_is_stationary && !opp_attacker_in_range && (suitable_mid != nullptr)) { Common::Angle passAngle = Common::Vec2(-m_side * 1700, Common::sign(-m_world_state.ball.position.y) * 1700.0f) @@ -70,9 +77,9 @@ void Ai::normalPlayAtt() if (suitable_mid) { passAngle = m_own_robot[*suitable_mid].state().position.angleWith(m_world_state.ball.position); - chip_pow = 50.f * m_own_robot[*suitable_mid].state().position.distanceTo(m_world_state.ball.position) / - 4000.0f; - chip_pow = std::min(50.f, chip_pow); + chip_pow = 15.f * m_own_robot[*suitable_mid].state().position.distanceTo(m_world_state.ball.position) / + 8000.0f; + chip_pow = std::min(15.f, chip_pow); } else { @@ -80,13 +87,32 @@ void Ai::normalPlayAtt() chip_pow = 0; } - attacker(m_attack, passAngle, 0, chip_pow, 0, 1); + attacker(m_attack, passAngle, 0, chip_pow, 0, 1, true); } else { - Common::Angle shootAngle; - shootAngle = Common::Angle::fromDeg(180.0f) + openAngle.center; + + static bool intersecting = false; + const float speed_threshold = intersecting ? 500.0f : 1000.0f; + + const float ball_dir_goal_dot = + m_world_state.ball.velocity.normalized().dot((oppGoal() - m_world_state.ball.position).normalized()); + if (m_world_state.ball.velocity.length() < speed_threshold || std::abs(ball_dir_goal_dot) > 0.75f) + { + intersecting = false; + + // target the goal center if the open angle is too small + if (openAngle.magnitude.deg() > 5.0f) + shootAngle = Common::Angle::fromDeg(180.0f) + openAngle.center; + else + shootAngle = (m_world_state.ball.position - oppGoal()).toAngle(); + } + else + { + intersecting = true; + shootAngle = m_world_state.ball.velocity.toAngle(); + } float shoot_pow = 6500.f; // 6.5m/s @@ -100,6 +126,10 @@ void Ai::normalPlayAtt() { shoot_pow = 1.f; } + else if (intersecting) + { + shoot_pow = 1.0f; + } attacker(m_attack, shootAngle, shoot_pow, 0, 0, 0); } diff --git a/source/soccer/plays/normal_play_def.cpp b/source/soccer/plays/normal_play_def.cpp index a1de9cd1..241b2091 100644 --- a/source/soccer/plays/normal_play_def.cpp +++ b/source/soccer/plays/normal_play_def.cpp @@ -37,7 +37,7 @@ void Ai::normalPlayDef() } else { - mark(own, opp, 500); + mark(own, opp, Common::config().soccer.mark_distance); } } @@ -56,7 +56,7 @@ void Ai::normalPlayDef() } else if (!goalBlocked(m_world_state.ball.position, 3000, 130)) { - shoot_pow = 50 - m_own_robot[m_attack].state().velocity.length() * 0.005f; + shoot_pow = 6500.f; chip_pow = 0; } else diff --git a/source/soccer/plays/penalty_us_shootout.cpp b/source/soccer/plays/penalty_us_shootout.cpp index bc99d121..a95e554c 100644 --- a/source/soccer/plays/penalty_us_shootout.cpp +++ b/source/soccer/plays/penalty_us_shootout.cpp @@ -17,7 +17,7 @@ void Ai::penaltyUsShootout() if (!m_ref_state.canKickBall()) { - // TODO: get behind the ball + circleBall(m_attack, oppGoal().angleWith(m_world_state.ball.position), 0, 0); Common::logInfo("step0 - Waiting for permission"); } else if (m_world_state.ball.position.distanceTo(oppGoal()) > 3000) @@ -27,7 +27,7 @@ void Ai::penaltyUsShootout() } else { - circleBall(m_attack, oppGoal().angleWith(m_world_state.ball.position), 60, 0); + circleBall(m_attack, oppGoal().angleWith(m_world_state.ball.position), 6000.0f, 0); Common::logInfo("step2 - Kick in the goal!!!!"); } } diff --git a/source/soccer/plays/place_ball.cpp b/source/soccer/plays/place_ball.cpp index 4a430fa5..b0bdd635 100644 --- a/source/soccer/plays/place_ball.cpp +++ b/source/soccer/plays/place_ball.cpp @@ -8,10 +8,7 @@ void Ai::ourPlaceBall() { calcIsDefending(); - // TODO: fix this #if 0 - m_is_defending = false; - m_assignments.clear(); createGkAssignment(); createDefAssignments(); @@ -50,7 +47,7 @@ void Ai::ourPlaceBall() if (m_func_state == -2) { - circleBall(m_attack, outFieldAng, 24, 0); + circleBall(m_attack, outFieldAng, 5000.0f, 0); // TODO: transition when m_dmf fits behind the ball if (last_state_ball_pos.distanceTo(m_world_state.ball.position) > 400) @@ -160,7 +157,7 @@ void Ai::ourPlaceBall() } else if (m_func_state == 1) { - circleBall(m_attack, move_angle, 18, 0); + circleBall(m_attack, move_angle, 5000.0f, 0); waitForPass(m_mid5, false, &m_own_robot[m_attack].state().position); if (last_state_ball_pos.distanceTo(m_world_state.ball.position) > 400) @@ -367,4 +364,5 @@ void Ai::ourPlaceBall() Common::debug().draw(m_ref_state.designated_position, Common::Color::red(), 20.0f); } + } // namespace Tyr::Soccer diff --git a/source/soccer/plays/stop.cpp b/source/soccer/plays/stop.cpp index 00be05fa..de7cb0fc 100644 --- a/source/soccer/plays/stop.cpp +++ b/source/soccer/plays/stop.cpp @@ -41,7 +41,7 @@ void Ai::stop() } else { - mark(own, opp, 500); + mark(own, opp, Common::config().soccer.mark_distance); } } diff --git a/source/soccer/plays/strategy.cpp b/source/soccer/plays/strategy.cpp index 2e67066d..075b2dc0 100644 --- a/source/soccer/plays/strategy.cpp +++ b/source/soccer/plays/strategy.cpp @@ -173,14 +173,8 @@ void Ai::strategy() for (int i = 0; i < strategy.roles.size(); i++) { - if (*m_strategy_ids[i] == m_gk || *m_strategy_ids[i] == m_def1 || *m_strategy_ids[i] == m_def2) - continue; - if (strategy.roles[i].path.size() == 0) - { - halt(*m_strategy_ids[i]); continue; - } if (*m_strategy_ids[i] == m_attack) { @@ -189,7 +183,7 @@ void Ai::strategy() if (strategy.roles[i].path[step[i]].type == Waypoint::Type::Position) { - shoot = strategy.roles[i].path[step[i]].tolerance; + shoot = strategy.roles[i].path[step[i]].tolerance * 100.0f; Common::logDebug("ATTACK: shoot: {}", shoot); } else diff --git a/source/soccer/plays/throwin_chip_shoot.cpp b/source/soccer/plays/throwin_chip_shoot.cpp index 50d0ca85..c284e471 100644 --- a/source/soccer/plays/throwin_chip_shoot.cpp +++ b/source/soccer/plays/throwin_chip_shoot.cpp @@ -16,15 +16,16 @@ void Ai::throwinChipShoot() gkHi(m_gk); defHi(m_def1, m_def2, nullptr); - m_own_robot[m_mid2].face(m_world_state.ball.position); - navigate(m_mid2, Common::Vec2(-m_side * 1500, Common::sign(m_world_state.ball.position.y) * 2500.0f), - VelocityProfile::mamooli(), NavigationFlagsForceBallObstacle); - m_one_touch_type[m_mid2] = OneTouchType::Shirje; + int zone_idx = 0; + for (const auto &mid : m_prioritized_mids) + { + if (mid == &m_mid5) + continue; - m_own_robot[m_mid1].face(m_world_state.ball.position); - navigate(m_mid1, Common::Vec2(-m_side * 3500, Common::sign(m_world_state.ball.position.y) * 2500.0f), - VelocityProfile::mamooli(), NavigationFlagsForceBallObstacle); - m_one_touch_type[m_mid1] = OneTouchType::Shirje; + m_own_robot[*mid].face(m_world_state.ball.position); + navigate(*mid, m_sorted_zones[zone_idx]->best_pos, VelocityProfile::mamooli(), NavigationFlagsForceBallObstacle); + ++zone_idx; + } if (m_timer.time().seconds() > 4) { diff --git a/source/soccer/robot/robot.cpp b/source/soccer/robot/robot.cpp index a7dee58a..89684286 100644 --- a/source/soccer/robot/robot.cpp +++ b/source/soccer/robot/robot.cpp @@ -4,11 +4,11 @@ namespace Tyr::Soccer { void Robot::shoot(const float pow) { - m_shoot = pow; + m_shoot = pow * Common::config().soccer.kick_tune_coef; } void Robot::chip(const float pow) { - m_chip = pow; + m_chip = pow * Common::config().soccer.chip_tune_coef; } void Robot::dribble(const float pow) @@ -33,10 +33,10 @@ Common::Vec2 Robot::computeMotion(const VelocityProfile &profile) Common::Vec2 diff = target.position - state().position; - Common::Vec2 tmp_max_speed = diff.abs().normalized() * profile.max_spd; + Common::Vec2 tmp_max_speed = diff.abs().normalized() * profile.speed; - const float acc = profile.max_acc / Common::config().vision.vision_frame_rate; - const float dec = profile.max_dec / Common::config().vision.vision_frame_rate; + const float acc = profile.acceleration / Common::config().vision.vision_frame_rate; + const float dec = profile.deceleration / Common::config().vision.vision_frame_rate; motion.x = pow(7.6f * dec * std::fabs(diff.x), 0.6f); motion.x *= Common::sign(diff.x); diff --git a/source/soccer/robot/velocity_profile.h b/source/soccer/robot/velocity_profile.h index 0347caf3..bdf24306 100644 --- a/source/soccer/robot/velocity_profile.h +++ b/source/soccer/robot/velocity_profile.h @@ -4,44 +4,34 @@ namespace Tyr::Soccer { struct VelocityProfile { - float max_spd; - float max_dec; - float max_acc; + float speed = 0.0f; + float deceleration = 0.0f; + float acceleration = 0.0f; - static constexpr VelocityProfile sooski() + VelocityProfile() = default; + + explicit VelocityProfile(const Common::Config::VelocityProfile &config) + : speed(config.speed), deceleration(config.deceleration), acceleration(config.acceleration) + {} + + static VelocityProfile sooski() { - return { - .max_spd = 450.0f, - .max_dec = 2700.0f, - .max_acc = 1620.0f, - }; + return VelocityProfile{Common::config().soccer.velocity_profile_sooski}; } - static constexpr VelocityProfile aroom() + static VelocityProfile aroom() { - return { - .max_spd = 900.0f, - .max_dec = 2700.0f, - .max_acc = 2160.0f, - }; + return VelocityProfile{Common::config().soccer.velocity_profile_aroom}; } - static constexpr VelocityProfile mamooli() + static VelocityProfile mamooli() { - return { - .max_spd = 3800.0f, - .max_dec = 4850.5f, - .max_acc = 3100.0f, - }; + return VelocityProfile{Common::config().soccer.velocity_profile_mamooli}; } - static constexpr VelocityProfile kharaki() + static VelocityProfile kharaki() { - return { - .max_spd = 3800.0f, - .max_dec = 5860.0f, - .max_acc = 3910.0f, - }; + return VelocityProfile{Common::config().soccer.velocity_profile_kharaki}; } }; diff --git a/source/soccer/skills/attacker.cpp b/source/soccer/skills/attacker.cpp index 44a1d38d..f93e0524 100644 --- a/source/soccer/skills/attacker.cpp +++ b/source/soccer/skills/attacker.cpp @@ -2,9 +2,8 @@ namespace Tyr::Soccer { -int lockAngleCounter = 0; -int elendil = 0; -Common::Vec2 Pelendil; +int lockAngleCounter = 0; +int elendil = 0; Common::Vec2 Ai::predictBallForwardAINew(const float t_time_ahead) { @@ -79,7 +78,7 @@ Common::Vec2 Ai::predictBallForwardAI(const float t_time_ahead) float Ai::calculateRobotReachTime(const int t_robot_num, const Common::Vec2 t_dest, const VelocityProfile t_profile) { const VelocityProfile vel_profile(t_profile); - return m_own_robot[t_robot_num].state().position.distanceTo(t_dest) / (vel_profile.max_spd * 0.5); + return m_own_robot[t_robot_num].state().position.distanceTo(t_dest) / (vel_profile.speed * 0.5); } float Ai::calculateBallRobotReachTime(const int t_robot_num, const VelocityProfile t_profile) @@ -103,9 +102,13 @@ float Ai::calculateBallRobotReachTime(const int t_robot_num, const VelocityProfi return predTFilt.current(); } -void Ai::attacker(const int t_robot_num, const Common::Angle t_angle, float t_kick, const int t_chip, const bool t_kiss, - const bool t_dribbler) +void Ai::attacker(const int t_robot_num, const Common::Angle t_angle, const float t_kick, const int t_chip, + const bool t_kiss, const bool t_dribbler, const bool precise) { + Common::debug().draw( + Common::LineSegment{m_world_state.ball.position, m_world_state.ball.position + t_angle.toUnitVec() * 1000.0f}, + Common::Color::blue()); + // t_kick=100; if (m_ref_state.restart() && (t_chip > 0)) { @@ -187,7 +190,7 @@ void Ai::attacker(const int t_robot_num, const Common::Angle t_angle, float t_ki else { r = 400.0f; - tetta = 32.0f; + tetta = 45.0f; // if ( !passedBall ) // r *= 1.5; } @@ -204,8 +207,10 @@ void Ai::attacker(const int t_robot_num, const Common::Angle t_angle, float t_ki else m_own_robot[t_robot_num].target.angle = t_angle - Common::Angle::fromDeg(180.0f); - Common::Angle hehe = m_predicted_ball.angleWith(m_own_robot[t_robot_num].state().position); - hehe = t_angle - hehe; + // move the ball forward a bit to compensate for the over predict we do on our robots + const Common::Vec2 ball_moved_towards_target = m_predicted_ball - t_angle.toUnitVec() * 100.0f; + Common::Angle hehe = ball_moved_towards_target.angleWith(m_own_robot[t_robot_num].state().position); + hehe = t_angle - hehe; if ((std::fabs(hehe.deg()) < tetta)) //|| ( m_circle_reached_behind_ball ) ) { @@ -219,8 +224,14 @@ void Ai::attacker(const int t_robot_num, const Common::Angle t_angle, float t_ki Common::Vec2 targetPoint; if (!m_ref_state.restart()) { - targetPoint = m_predicted_ball.circleAroundPoint( - t_angle, std::min((r / 1.6f), std::fabs(hehe.deg()) * 320.0f / (tetta * 1.2f))); + float normalized_hehe = std::fabs(hehe.deg()) / tetta; + normalized_hehe = std::pow(normalized_hehe, 0.3f); + targetPoint = m_predicted_ball.circleAroundPoint(t_angle, std::min(r, normalized_hehe * 265.0f)); + + if (!precise) + { + targetPoint -= t_angle.toUnitVec() * (1.0f - std::pow(normalized_hehe, 0.5f)) * 400.0f; + } } else { @@ -228,34 +239,42 @@ void Ai::attacker(const int t_robot_num, const Common::Angle t_angle, float t_ki t_angle, std::min(r, std::fabs(hehe.deg()) * 320.0f / (tetta))); } - Common::logDebug("elendil: {}", elendil); - Common::Angle hehe2 = m_predicted_ball.angleWith(m_own_robot[t_robot_num].state().position); - hehe2 = t_angle - hehe2; - bool el = ((hehe2.deg() < 5) && - (m_world_state.ball.position.distanceTo(m_own_robot[t_robot_num].state().position) < 100)); - if (el || (elendil > 0)) + // This part extends the target point towards the goal + if (precise) { + Common::debug().draw(targetPoint, Common::Color::maroon()); - targetPoint.x -= 150.0 * t_angle.cos(); - targetPoint.y -= 150.0 * t_angle.sin(); - targetPoint.x /= 1; - targetPoint.y /= 1; + Common::logDebug("elendil: {}", elendil); + Common::Angle hehe2 = m_predicted_ball.angleWith(m_own_robot[t_robot_num].state().position); + hehe2 = t_angle - hehe2; + Common::logDebug("hehe2: {}", hehe2.deg()); - Pelendil = targetPoint; + const bool el_in = + ((std::abs(hehe2.deg()) < 5.0f) && + (m_world_state.ball.position.distanceTo(m_own_robot[t_robot_num].state().position) < 100)); + const bool el_out = + ((std::abs(hehe2.deg()) > 10.0f) && + (m_world_state.ball.position.distanceTo(m_own_robot[t_robot_num].state().position) > 200)); - elendil--; - if (el) - { + if (el_in) elendil = 30; + if (el_out) + elendil = 0; + + if (elendil > 0) + { + elendil--; + // extend towards the shoot target + targetPoint -= t_angle.toUnitVec() * 200.0f; + } + else + { + // extend towards the point behind the ball + // targetPoint += (targetPoint - m_own_robot[t_robot_num].state().position) / 2.0f; } - navigate(t_robot_num, Pelendil, VelocityProfile::kharaki()); - } - else - { - targetPoint = (targetPoint * 3.0f) - m_own_robot[t_robot_num].state().position; - targetPoint /= 2.0f; - navigate(t_robot_num, targetPoint, VelocityProfile::kharaki()); } + + navigate(t_robot_num, targetPoint, VelocityProfile::kharaki()); } else navigate( diff --git a/source/soccer/skills/circle_ball.cpp b/source/soccer/skills/circle_ball.cpp index d9084931..43d2376a 100644 --- a/source/soccer/skills/circle_ball.cpp +++ b/source/soccer/skills/circle_ball.cpp @@ -39,7 +39,7 @@ void Ai::circleBall(const int t_robot_num, const Common::Angle t_tagret_angle, f Common::logDebug("STEPPPPP1"); m_own_robot[t_robot_num].face(m_world_state.ball.position); VelocityProfile profile = VelocityProfile::mamooli(); - profile.max_spd = 900.0f; + profile.speed = 900.0f; navigate(t_robot_num, m_world_state.ball.position, profile); Common::debug().draw(Common::Circle{m_world_state.ball.position, very_far_ball_dis - 90.0f}, diff --git a/source/soccer/skills/gk_hi.cpp b/source/soccer/skills/gk_hi.cpp index d882d004..1a329ee0 100644 --- a/source/soccer/skills/gk_hi.cpp +++ b/source/soccer/skills/gk_hi.cpp @@ -81,7 +81,7 @@ void Ai::gkHi(const int t_robot_num) attacker(t_robot_num, predicted_ball.angleWith(Common::Vec2(m_side * (Common::field().width + 110), 0)), 0, - 80, 0, 0); + 20, 0, 0); } else { diff --git a/source/soccer/skills/navigation.cpp b/source/soccer/skills/navigation.cpp index 74b11916..b26db05a 100644 --- a/source/soccer/skills/navigation.cpp +++ b/source/soccer/skills/navigation.cpp @@ -11,7 +11,7 @@ void Ai::navigate(const int t_robot_num, const Common::Vec2 t_dest, VelocityProf if (m_ref_state.shouldSlowDown()) { - t_profile.max_spd = std::min(t_profile.max_spd, 900.0f); + t_profile.speed = std::min(t_profile.speed, 900.0f); } if (m_own_robot[t_robot_num].state().position.distanceTo(t_dest) > 100.0f) diff --git a/source/soccer/skills/pass.cpp b/source/soccer/skills/pass.cpp index 1e380e1c..ec93443a 100644 --- a/source/soccer/skills/pass.cpp +++ b/source/soccer/skills/pass.cpp @@ -64,7 +64,7 @@ void Ai::waitForPass(const int t_robot_num, const bool t_chip, const Common::Vec vel_delta = 60 - vel_delta; Common::logDebug("ball vel: {}", vel_delta); //TODO: calc pass speed - m_own_robot[t_robot_num].shoot(4000.f); + m_own_robot[t_robot_num].shoot(6000.f); } } else diff --git a/source/soccer/skills/pass_omghi.cpp b/source/soccer/skills/pass_omghi.cpp index 7e8cc7c5..89a35d7a 100644 --- a/source/soccer/skills/pass_omghi.cpp +++ b/source/soccer/skills/pass_omghi.cpp @@ -39,7 +39,7 @@ void Ai::waitForOmghi(const int t_robot_num) Common::logDebug("sBAR: {}", sBAR); VelocityProfile profile = VelocityProfile::kharaki(); - profile.max_spd = sBAR * 45.0f; + profile.speed = sBAR * 45.0f; navigate(t_robot_num, target, profile); diff --git a/source/soccer/zone/pos_attack.cpp b/source/soccer/zone/pos_attack.cpp index 19339908..e9bbd9eb 100644 --- a/source/soccer/zone/pos_attack.cpp +++ b/source/soccer/zone/pos_attack.cpp @@ -4,6 +4,16 @@ namespace Tyr::Soccer { float Ai::staticPosScoreAttack(const Common::Vec2 t_pos) const { + // Reject if blocks the attack's shot on goal + const Common::Line pos_ball_line = Common::Line::fromTwoPoints(m_world_state.ball.position, t_pos); + const Common::Line goal_line = Common::Line::fromSegment(oppGoalLine()); + const auto goal_inter = pos_ball_line.intersect(goal_line); + + const float dot = (t_pos - m_world_state.ball.position).normalized().dot((oppGoal() - m_world_state.ball.position).normalized()); + + if (dot > 0 && goal_inter.has_value() && std::abs(goal_inter.value().y) < Common::field().goal_width / 2.0f) + return 0.0f; + float oppDisToGoal = t_pos.distanceTo(oppGoal()); Common::Angle t1Angel = t_pos.angleWith(oppGoalPostBottom()); @@ -24,9 +34,6 @@ float Ai::staticPosScoreAttack(const Common::Vec2 t_pos) const float ballToOppDis = m_world_state.ball.position.distanceTo(t_pos); - if (ballToOppDis < 400) - return -1; - float score_ball_dis; if (ballToOppDis < 2000) score_ball_dis = pow(ballToOppDis / 2000.0f, 2.0f); @@ -49,10 +56,17 @@ float Ai::staticPosScoreAttack(const Common::Vec2 t_pos) const score_open_angle = std::min(1.0f, std::max(0.0f, score_open_angle)); score_one_touch_angle = std::min(1.0f, std::max(0.0f, score_one_touch_angle)); - auto final_score_one_touch = score_one_touch_angle * std::min(score_ball_dis * score_goal_dis, score_open_angle); + const bool pass_angle_ok = (t_pos - m_world_state.ball.position) + .normalized() + .dot((ownGoal() - m_world_state.ball.position).normalized()) < 0.85f; + const float own_goal_angle_score = pass_angle_ok ? 1.0f : 0.0f; + + auto final_score_one_touch = own_goal_angle_score * score_one_touch_angle * std::min(score_ball_dis * score_goal_dis, score_open_angle); auto final_score_turn_shoot = std::min(score_ball_dis * score_goal_dis, score_open_angle); - auto score = std::max(final_score_one_touch, final_score_turn_shoot); + + + const float score = std::max(final_score_one_touch, final_score_turn_shoot); return score; } diff --git a/source/vision/filtered/filtered.h b/source/vision/filtered/filtered.h index eec43092..95677603 100644 --- a/source/vision/filtered/filtered.h +++ b/source/vision/filtered/filtered.h @@ -58,7 +58,7 @@ class Filtered Common::MedianFilter m_angle_filter[2][Common::Config::Common::kMaxRobots]; Common::Angle m_raw_angles[2][Common::Config::Common::kMaxRobots]; - static constexpr size_t kMaxHist = 6; + static constexpr size_t kMaxHist = 7; using CommandHistory = std::deque; std::unordered_map m_cmd_map; diff --git a/vcpkg-configuration.json b/vcpkg-configuration.json index fafe7982..eb41e712 100644 --- a/vcpkg-configuration.json +++ b/vcpkg-configuration.json @@ -8,7 +8,7 @@ { "kind": "git", "repository": "https://github.com/Immortals-Robotics/vcpkg-registry", - "baseline": "61b93585734da7b32fb1179864510951007db509", + "baseline": "5ad5f854664b7bb5adcf9f75584d5b3fecefbf5a", "packages": [ "homog2d", "imgui",