From 9b081f586e0829e630fbc37405e36455082fa086 Mon Sep 17 00:00:00 2001
From: Rayy0527 <1668834312@qq.com>
Date: Sun, 24 Nov 2024 15:51:53 +0800
Subject: [PATCH] Add sin speed gyro.

---
 .../chassis_gimbal_shooter_cover_manual.h     |  5 ++++
 src/chassis_gimbal_shooter_cover_manual.cpp   | 29 +++++++++++++++++--
 2 files changed, 31 insertions(+), 3 deletions(-)

diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h
index cb4203e8..2e2ff694 100644
--- a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h
+++ b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h
@@ -20,6 +20,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
 
 protected:
   void changeSpeedMode(SpeedMode speed_mode);
+  double getDynamicScale(double base_scale, double amplitude, double period, double phase);
   void changeGyroSpeedMode(SpeedMode speed_mode);
   void updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
   void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
@@ -52,6 +53,10 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
   double low_speed_scale_{}, normal_speed_scale_{};
   double exit_buff_mode_duration_{};
   double gyro_speed_limit_{};
+  double sin_gyro_base_scale_{ 1. };
+  double sin_gyro_amplitude_{ 0. };
+  double sin_gyro_period_{ 1. };
+  double sin_gyro_phase_{ 0. };
   rm_common::SwitchDetectionCaller* switch_buff_srv_{};
   rm_common::SwitchDetectionCaller* switch_buff_type_srv_{};
   rm_common::SwitchDetectionCaller* switch_exposure_srv_{};
diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp
index 73a92802..0e5c1c74 100644
--- a/src/chassis_gimbal_shooter_cover_manual.cpp
+++ b/src/chassis_gimbal_shooter_cover_manual.cpp
@@ -23,6 +23,11 @@ ChassisGimbalShooterCoverManual::ChassisGimbalShooterCoverManual(ros::NodeHandle
   low_speed_scale_ = chassis_nh.param("low_speed_scale", 0.30);
   nh.param("exit_buff_mode_duration", exit_buff_mode_duration_, 0.5);
   nh.param("gyro_speed_limit", gyro_speed_limit_, 6.0);
+  ros::NodeHandle vel_nh(nh, "vel");
+  sin_gyro_base_scale_ = vel_nh.param("sin_gyro_base_scale", 1.0);
+  sin_gyro_amplitude_ = vel_nh.param("sin_gyro_amplitude", 0.0);
+  sin_gyro_period_ = vel_nh.param("sin_gyro_period", 1.0);
+  sin_gyro_phase_ = vel_nh.param("sin_gyro_phase", 0.0);
 
   ctrl_z_event_.setEdge(boost::bind(&ChassisGimbalShooterCoverManual::ctrlZPress, this),
                         boost::bind(&ChassisGimbalShooterCoverManual::ctrlZRelease, this));
@@ -42,7 +47,22 @@ void ChassisGimbalShooterCoverManual::changeSpeedMode(SpeedMode speed_mode)
     speed_change_scale_ = normal_speed_scale_;
   }
 }
-
+double ChassisGimbalShooterCoverManual::getDynamicScale(double base_scale, double amplitude, double period, double phase)
+{
+  ros::Time current_time = ros::Time::now();
+  double t = current_time.toSec();
+  double f = 2 * M_PI / period;
+  double dynamic_scale = base_scale + amplitude * sin(f * t + phase);
+  if (dynamic_scale < 0.0)
+  {
+    dynamic_scale = 0.0;
+  }
+  else if (dynamic_scale > 1.0)
+  {
+    dynamic_scale = 1.0;
+  }
+  return dynamic_scale;
+}
 void ChassisGimbalShooterCoverManual::changeGyroSpeedMode(SpeedMode speed_mode)
 {
   if (speed_mode == LOW)
@@ -55,9 +75,12 @@ void ChassisGimbalShooterCoverManual::changeGyroSpeedMode(SpeedMode speed_mode)
   else if (speed_mode == NORMAL)
   {
     if (x_scale_ != 0.0 || y_scale_ != 0.0)
-      vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_);
+      vel_cmd_sender_->setAngularZVel(
+          getDynamicScale(sin_gyro_base_scale_, sin_gyro_amplitude_, sin_gyro_period_, sin_gyro_phase_) *
+          gyro_rotate_reduction_);
     else
-      vel_cmd_sender_->setAngularZVel(1.0);
+      vel_cmd_sender_->setAngularZVel(
+          getDynamicScale(sin_gyro_base_scale_, sin_gyro_amplitude_, sin_gyro_period_, sin_gyro_phase_));
   }
 }