diff --git a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp index 3ddf350c8e..1eccb8522a 100644 --- a/soccer/src/soccer/strategy/agent/position/penalty_player.cpp +++ b/soccer/src/soccer/strategy/agent/position/penalty_player.cpp @@ -25,7 +25,7 @@ PenaltyPlayer::State PenaltyPlayer::update_state() { case SMALL_KICK: { if (distance_from_enemy_goal() < kDistanceToGoalThreshold) { return LINE_UP; - } + } break; } case LINE_UP: { @@ -56,10 +56,10 @@ PenaltyPlayer::State PenaltyPlayer::update_state() { std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { switch (latest_state_) { - case START: { // First, gets the robot to the ball to begin penalty dribbling-shooting + case START: { // First, gets the robot to the ball to begin penalty dribbling-shooting SPDLOG_INFO("START"); double y_pos = last_world_state_->ball.position.y(); - y_pos -= kRobotRadius + 0.3; // added the 0.01 as a buffer space + y_pos -= kRobotRadius + 0.3; // added the 0.01 as a buffer space rj_geometry::Point target_pt{last_world_state_->ball.position.x(), y_pos}; rj_geometry::Point target_vel{0.0, 0.0}; // Face ball @@ -71,8 +71,8 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { planning::MotionCommand{"path_target", goal, planning::FaceBall{}}; break; } - case SMALL_KICK: { //less of a kick, more of a "follow" ball closely - //SPDLOG_INFO("SMALL_KICK"); + case SMALL_KICK: { // less of a kick, more of a "follow" ball closely + // SPDLOG_INFO("SMALL_KICK"); rj_geometry::Point center_goal = field_dimensions_.their_goal_loc(); auto line_kick_cmd = planning::MotionCommand{"line_kick", planning::LinearMotionInstant{center_goal}}; @@ -80,13 +80,15 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { intent.motion_command = line_kick_cmd; intent.shoot_mode = RobotIntent::ShootMode::KICK; intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; - intent.kick_speed = 0.0; - // the point of making a 0 kick speed is to fake dribble since we cannot get the vaccum behavior to work + intent.kick_speed = 0.0; + // the point of making a 0 kick speed is to fake dribble since we cannot get the vaccum + // behavior to work return intent; } - case LINE_UP: { // gets the robot behind the ball with a certain distance (usually immediatly skipped if the robot is already close) - //SPDLOG_INFO("LINE_UP"); + case LINE_UP: { // gets the robot behind the ball with a certain distance (usually + // immediatly skipped if the robot is already close) + // SPDLOG_INFO("LINE_UP"); double y_pos = last_world_state_->ball.position.y(); y_pos -= kRobotRadius + 0.1; rj_geometry::Point target_pt{last_world_state_->ball.position.x(), y_pos}; @@ -103,8 +105,9 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { return intent; } - case SHOOTING_START: { // Positions the robot behind the ball at a certain angle so that it has a straight shot towards the goal - //SPDLOG_INFO("SHOOTING_START"); + case SHOOTING_START: { // Positions the robot behind the ball at a certain angle so that it + // has a straight shot towards the goal + // SPDLOG_INFO("SHOOTING_START"); target_ = calculate_best_shot(); rj_geometry::Point ball_position = last_world_state_->ball.position; auto current_pos = last_world_state_->get_robot(true, robot_id_).pose.position(); @@ -117,8 +120,9 @@ std::optional PenaltyPlayer::state_to_task(RobotIntent intent) { return intent; } - case SHOOTING: { // Kicks the ball with a now much higher speed (basically SMALL_KICK but power set to 4) - //SPDLOG_INFO("SHOOTING"); + case SHOOTING: { // Kicks the ball with a now much higher speed (basically SMALL_KICK but + // power set to 4) + // SPDLOG_INFO("SHOOTING"); auto line_kick_cmd = planning::MotionCommand{"line_kick", planning::LinearMotionInstant{target_}}; @@ -164,13 +168,16 @@ double PenaltyPlayer::distance_from_their_robots(rj_geometry::Point tail, rj_geometry::Point PenaltyPlayer::calculate_best_shot() const { // Goal location rj_geometry::Point their_goal_pos = field_dimensions_.their_goal_loc(); - double goal_width = field_dimensions_.goal_width(); // 1.0 meters // BALL SEEMS TO OCCASIONALLY MISS SHOT ON EDGE, CONSIDER CHANGING THIS? + double goal_width = + field_dimensions_.goal_width(); // 1.0 meters // BALL SEEMS TO OCCASIONALLY MISS SHOT ON + // EDGE, CONSIDER CHANGING THIS? // Ball location rj_geometry::Point ball_position = this->last_world_state_->ball.position; - // Iterates across 19 possible shot locations along the goal width in 0.05-meter increments. - // For each location, it calculates the clearance distance from opponent robots and updates the best shot position if a better (less obstructed) option is found. + // Iterates across 19 possible shot locations along the goal width in 0.05-meter increments. + // For each location, it calculates the clearance distance from opponent robots and updates the + // best shot position if a better (less obstructed) option is found. rj_geometry::Point best_shot = their_goal_pos; double best_distance = -1.0; rj_geometry::Point increment(0.05, 0); diff --git a/soccer/src/soccer/strategy/agent/position/penalty_player.hpp b/soccer/src/soccer/strategy/agent/position/penalty_player.hpp index e9c214ccf8..9b1febddc3 100644 --- a/soccer/src/soccer/strategy/agent/position/penalty_player.hpp +++ b/soccer/src/soccer/strategy/agent/position/penalty_player.hpp @@ -59,8 +59,9 @@ class PenaltyPlayer : public Position { }; double distance_from_enemy_goal() const { - return last_world_state_->get_robot(true, robot_id_).pose.position().dist_to( - field_dimensions_.their_goal_loc()); + return last_world_state_->get_robot(true, robot_id_) + .pose.position() + .dist_to(field_dimensions_.their_goal_loc()); }; /**