From fffff764f75202a4327edf94116c8b0ba459af79 Mon Sep 17 00:00:00 2001
From: sanatd33 <saisanat@gmail.com>
Date: Sun, 7 Apr 2024 19:37:12 -0400
Subject: [PATCH] added dribbling to pivot line

---
 .../soccer/planning/planner/pivot_path_planner.cpp    | 11 -----------
 .../src/soccer/strategy/agent/position/pivot_test.cpp |  6 ++----
 2 files changed, 2 insertions(+), 15 deletions(-)

diff --git a/soccer/src/soccer/planning/planner/pivot_path_planner.cpp b/soccer/src/soccer/planning/planner/pivot_path_planner.cpp
index c10d9b591e..0e7c09a8fc 100644
--- a/soccer/src/soccer/planning/planner/pivot_path_planner.cpp
+++ b/soccer/src/soccer/planning/planner/pivot_path_planner.cpp
@@ -67,14 +67,6 @@ Trajectory PivotPathPlanner::plan(const PlanRequest& request) {
         request.world_state->get_robot(true, request.shell_id).pose.position());
     double target_angle = pivot_point.angle_to(final_position);
     double angle_change = fix_angle_radians(target_angle - start_angle);
-    SPDLOG_INFO("Start Position: ({}, {})", request.world_state->get_robot(true, request.shell_id).pose.position().x(), request.world_state->get_robot(true, request.shell_id).pose.position().y());
-    SPDLOG_INFO("Pivot Point: ({}, {})", pivot_point.x(), pivot_point.y());
-    SPDLOG_INFO("Pivot Target: ({}, {})", pivot_target.x(), pivot_target.y());
-    SPDLOG_INFO("Final Position: ({}, {})", final_position.x(), final_position.y());
-    SPDLOG_INFO("Start Angle: {}", start_angle);
-    SPDLOG_INFO("Target Angle: {}", target_angle);
-    SPDLOG_INFO("Angle Change: {}", angle_change);
-    SPDLOG_INFO("----------------");
 
     cached_angle_change_ = angle_change;
 
@@ -113,10 +105,7 @@ bool PivotPathPlanner::is_done() const {
     if (!cached_angle_change_.has_value()) {
         return false;
     }
-    SPDLOG_INFO("checking is done, {}", cached_angle_change_.value());
     bool val = abs(cached_angle_change_.value()) < degrees_to_radians(IS_DONE_ANGLE_CHANGE_THRESH);
-    SPDLOG_INFO("Result: {}", val);
-    SPDLOG_INFO("----------------");
     return val;
 }
 
diff --git a/soccer/src/soccer/strategy/agent/position/pivot_test.cpp b/soccer/src/soccer/strategy/agent/position/pivot_test.cpp
index 77f9a0f84a..6b8ea98b24 100644
--- a/soccer/src/soccer/strategy/agent/position/pivot_test.cpp
+++ b/soccer/src/soccer/strategy/agent/position/pivot_test.cpp
@@ -8,7 +8,6 @@ std::optional<RobotIntent> Pivot::derived_get_task(RobotIntent intent) {
     // Get next state, and if different, reset clock
     State new_state = next_state();
     if (new_state != current_state_) {
-        SPDLOG_INFO("New State: {}", new_state);
         curr_pt_ = last_world_state_->get_robot(true, robot_id_).pose.position();
     }
     current_state_ = new_state;
@@ -23,7 +22,6 @@ std::string Pivot::get_current_state() {
 
 Pivot::State Pivot::next_state() {
     // handle transitions between current state
-    SPDLOG_INFO("Current State: {}", current_state_);
     switch (current_state_) {
         case OUR_GOAL: {
             if (check_is_done()) {
@@ -53,17 +51,17 @@ Pivot::State Pivot::next_state() {
 std::optional<RobotIntent> Pivot::state_to_task(RobotIntent intent) {
     switch (current_state_) {
         case OUR_GOAL: {
-            SPDLOG_INFO("COMMAND: Pivot to our goal");
             planning::LinearMotionInstant target{field_dimensions_.our_goal_loc()};
             auto pivot_cmd = planning::MotionCommand{"line_pivot", target, planning::FaceTarget{}, false, last_world_state_->ball.position};
             intent.motion_command = pivot_cmd;
+            intent.dribbler_speed = 255.0;
             return intent;
         }
         case OPP_GOAL: {
-            SPDLOG_INFO("COMMAND: Pivot to opp goal");
             planning::LinearMotionInstant target{field_dimensions_.their_goal_loc()};
             auto pivot_cmd = planning::MotionCommand{"line_pivot", target, planning::FaceTarget{}, false, last_world_state_->ball.position};
             intent.motion_command = pivot_cmd;
+            intent.dribbler_speed = 255.0;
             return intent;           
         }
         case IDLE: {