Cameron carson/penalty position dribbling

soccer/src/soccer/strategy/agent/position/penalty_player.cpp

@@ -14,26 +14,39 @@ std::optional<RobotIntent> PenaltyPlayer::derived_get_task(RobotIntent intent) {
 
 PenaltyPlayer::State PenaltyPlayer::update_state() {
     switch (latest_state_) {
-        case LINE_UP: {
+        case START: {
             // if penalty playing and restart penalty in playstate we switch to shooting
             if (current_play_state_.is_ready() &&
                 (current_play_state_.is_penalty() || current_play_state_.is_kickoff())) {
-                return SHOOTING_START;
+                return SMALL_KICK;
             }
             break;
         }
-        case SHOOTING_START: {
+        case SMALL_KICK: {
+            if (distance_from_enemy_goal() < kDistanceToGoalThreshold) {
+                return LINE_UP;
+            }
+            break;
+        }
+        case LINE_UP: {
             if (check_is_done()) {
-                return SHOOTING;
+                return SHOOTING_START;
             }
             if (distance_to_ball() < kOwnBallRadius) {
+                return SHOOTING_START;
+            }
+            break;
+        }
+        case SHOOTING_START: {
+            if (check_is_done()) {
                 return SHOOTING;
             }
+
             break;
         }
         case SHOOTING: {
             if (check_is_done()) {
-                return LINE_UP;
+                return START;
             }
             break;
         }
@@ -43,14 +56,41 @@ PenaltyPlayer::State PenaltyPlayer::update_state() {
 
 std::optional<RobotIntent> PenaltyPlayer::state_to_task(RobotIntent intent) {
     switch (latest_state_) {
-        case LINE_UP: {
+        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
+            rj_geometry::Point target_pt{last_world_state_->ball.position.x(), y_pos};
+            rj_geometry::Point target_vel{0.0, 0.0};
+            // Face ball
+            planning::PathTargetFaceOption face_option{planning::FaceBall{}};
+
+            // Create Motion Command
+            planning::LinearMotionInstant goal{target_pt, target_vel};
+            intent.motion_command =
+                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");
+            rj_geometry::Point center_goal = field_dimensions_.their_goal_loc();
+            auto line_kick_cmd =
+                planning::MotionCommand{"line_kick", planning::LinearMotionInstant{center_goal}};
+
+            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
+
+            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");
             double y_pos = last_world_state_->ball.position.y();
-            // if ball is above goal, increase y_pos, else decrease
-            // if (y_pos - field_dimensions_.their_goal_loc().y() > 0) {
-            // y_pos += kRobotRadius - 0.15;
-            // } else {
-            y_pos -= kRobotRadius + 0.3;
-            // }
+            y_pos -= kRobotRadius + 0.1;
             rj_geometry::Point target_pt{last_world_state_->ball.position.x(), y_pos};
             rj_geometry::Point target_vel{0.0, 0.0};
             // Face ball
@@ -62,9 +102,12 @@ std::optional<RobotIntent> PenaltyPlayer::state_to_task(RobotIntent intent) {
             planning::LinearMotionInstant goal{target_pt, target_vel};
             intent.motion_command =
                 planning::MotionCommand{"path_target", goal, face_option, ignore_ball};
-            break;
+
+            return intent;
         }
-        case 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();
@@ -77,7 +120,9 @@ std::optional<RobotIntent> PenaltyPlayer::state_to_task(RobotIntent intent) {
 
             return intent;
         }
-        case 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_}};
 
@@ -87,7 +132,6 @@ std::optional<RobotIntent> PenaltyPlayer::state_to_task(RobotIntent intent) {
             intent.kick_speed = 4.0;
 
             return intent;
-            break;
         }
     }
 
@@ -124,11 +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
+    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.
     rj_geometry::Point best_shot = their_goal_pos;
     double best_distance = -1.0;
     rj_geometry::Point increment(0.05, 0);

soccer/src/soccer/strategy/agent/position/penalty_player.hpp

@@ -40,10 +40,12 @@ class PenaltyPlayer : public Position {
 private:
     std::optional<RobotIntent> derived_get_task(RobotIntent intent) override;
 
-    enum State { LINE_UP, SHOOTING_START, SHOOTING };
+    enum State { START, SMALL_KICK, LINE_UP, SHOOTING_START, SHOOTING };
 
     static constexpr double kOwnBallRadius{kRobotRadius + 0.1};
 
+    static constexpr double kDistanceToGoalThreshold{3.5};
+
     State update_state();
 
     /**
@@ -56,6 +58,12 @@ class PenaltyPlayer : public Position {
             last_world_state_->get_robot(true, robot_id_).pose.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 the target (within the goal) that would be the most clear shot
      */
@@ -77,7 +85,7 @@ class PenaltyPlayer : public Position {
     std::optional<RobotIntent> state_to_task(RobotIntent intent);
 
     // current state of Goalie (state machine)
-    State latest_state_ = LINE_UP;
+    State latest_state_ = START;
 };
 
 }  // namespace strategy