From 6acb5045d3794a5cfc8fdc6bebf97040a2ab05e2 Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Wed, 21 Feb 2024 08:29:19 +0900
Subject: [PATCH 1/2] =?UTF-8?q?=E3=83=81=E3=83=A5=E3=83=BC=E3=83=8B?=
 =?UTF-8?q?=E3=83=B3=E3=82=B0?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 crane_bringup/launch/play_switcher.launch.xml | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/crane_bringup/launch/play_switcher.launch.xml b/crane_bringup/launch/play_switcher.launch.xml
index 294c5acf0..dbb8d6c3c 100644
--- a/crane_bringup/launch/play_switcher.launch.xml
+++ b/crane_bringup/launch/play_switcher.launch.xml
@@ -47,9 +47,9 @@
 
   <node pkg="crane_sender" exec="sim_sender_node" output="screen">
     <param name="no_movement" value="false"/>
-    <param name="theta_kp" value="5.0"/>
+    <param name="theta_kp" value="3.0"/>
     <param name="theta_ki" value="0.0"/>
-    <param name="theta_kd" value="1.0"/>
+    <param name="theta_kd" value="0.1"/>
   </node>
 
   <!--    <node pkg="crane_sender" exec="ibis_sender_node" output="screen">-->

From 24ab7e48c9b16e77154155e9cc202de8adf3a4ad Mon Sep 17 00:00:00 2001
From: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>
Date: Wed, 21 Feb 2024 08:29:42 +0900
Subject: [PATCH 2/2] =?UTF-8?q?PlaySwitcher=E5=86=85=E3=81=A7=E5=89=8D?=
 =?UTF-8?q?=E5=9B=9E=E3=82=B3=E3=83=9E=E3=83=B3=E3=83=89=E9=81=B7=E7=A7=BB?=
 =?UTF-8?q?=E6=99=82=E3=81=AE=E7=8A=B6=E6=85=8B=E8=A8=98=E9=8C=B2=E3=81=8C?=
 =?UTF-8?q?=E6=8A=9C=E3=81=91=E3=81=A6=E3=81=84=E3=81=9F=E3=81=AE=E3=82=92?=
 =?UTF-8?q?=E4=BF=AE=E6=AD=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 crane_play_switcher/src/play_switcher.cpp | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/crane_play_switcher/src/play_switcher.cpp b/crane_play_switcher/src/play_switcher.cpp
index 7f7308ca2..b60a05173 100644
--- a/crane_play_switcher/src/play_switcher.cpp
+++ b/crane_play_switcher/src/play_switcher.cpp
@@ -132,7 +132,11 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
       play_situation_msg.command == PlaySituation::OUR_PENALTY_START) {
       if (0.05 <= (last_command_changed_state.ball_position - world_model->ball.pos).norm()) {
         next_play_situation = PlaySituation::INPLAY;
-        inplay_command_info.reason = "INPLAY判定：敵ボールが少なくとも0.05m動いた";
+        inplay_command_info.reason =
+          "INPLAY判定：敵ボールが少なくとも0.05m動いた(移動量: " +
+          std::to_string(
+            (last_command_changed_state.ball_position - world_model->ball.pos).norm()) +
+          "m)";
       }
     }
 
@@ -175,6 +179,9 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
     RCLCPP_INFO(
       get_logger(), "PREV_CMD_TIME: %f", (now() - last_command_changed_state.stamp).seconds());
 
+    last_command_changed_state.stamp = now();
+    last_command_changed_state.ball_position = world_model->ball.pos;
+
     // パブリッシュはコマンド更新時のみ
     play_situation_pub->publish(play_situation_msg);
   }