diff --git a/rj_constants/include/rj_constants/constants.hpp b/rj_constants/include/rj_constants/constants.hpp index 199101e80c1..8a11ff79f79 100644 --- a/rj_constants/include/rj_constants/constants.hpp +++ b/rj_constants/include/rj_constants/constants.hpp @@ -25,6 +25,9 @@ constexpr float kRobotHeight = 0.150f; constexpr float kRobotMouthWidth = 0.0635f; constexpr float kRobotMouthRadius = 0.078f; +// Constant for ball deceleration on field +constexpr float kBallDecel{-0.4f}; + /** constants for dot patterns */ constexpr float kDotsSmallOffset = 0.035; constexpr float kDotsLargeOffset = 0.054772; diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp index 689e50eda7d..017eca53a11 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/offense.cpp @@ -87,8 +87,8 @@ Offense::State Offense::update_state() { } std::optional Offense::state_to_task(RobotIntent intent) { + float dist{0.0f}; SPDLOG_INFO(current_state_); - if (current_state_ == IDLING) { // Do nothing auto empty_motion_cmd = planning::MotionCommand{}; @@ -103,14 +103,19 @@ std::optional Offense::state_to_task(RobotIntent intent) { target_robot_id = 2; rj_geometry::Point target_robot_pos = last_world_state_->get_robot(true, target_robot_id).pose.position(); + rj_geometry::Point this_robot_pos = + last_world_state_->get_robot(true, this->robot_id_).pose.position(); planning::LinearMotionInstant target{target_robot_pos}; auto line_kick_cmd = planning::MotionCommand{"line_kick", target}; intent.motion_command = line_kick_cmd; intent.shoot_mode = RobotIntent::ShootMode::KICK; // NOTE: Check we can actually use break beams intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; - // TODO: Adjust the kick speed based on distance - intent.kick_speed = 4.0; + // Adjusts kick speed based on distance. Refer to + // TIGERS Mannheim eTDP from 2019 for details + // See also passer.py in rj_gameplay + dist = target_robot_pos.dist_to(this_robot_pos); + intent.kick_speed = std::sqrt((std::pow(kFinalBallSpeed, 2)) - (2 * kBallDecel * dist)); intent.is_active = true; return intent; } else if (current_state_ == PREPARING_SHOT) { diff --git a/soccer/src/soccer/strategy/agent/position/offense.hpp b/soccer/src/soccer/strategy/agent/position/offense.hpp index 2c13dacef8f..0bb1f5e641d 100644 --- a/soccer/src/soccer/strategy/agent/position/offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/offense.hpp @@ -11,6 +11,7 @@ #include "planning/instant.hpp" #include "position.hpp" #include "rj_common/time.hpp" +#include "rj_constants/constants.hpp" #include "rj_geometry/geometry_conversions.hpp" #include "rj_geometry/point.hpp" @@ -36,6 +37,9 @@ class Offense : public Position { private: bool kicking_{true}; + // These variables are for calculating ball speed when passing + static constexpr float kFinalBallSpeed{0.0f}; + std::optional derived_get_task(RobotIntent intent) override; // TODO (Kevin): strategy design pattern for BallHandler/Receiver