Optimize some code.

rm-controls · Dec 7, 2024 · c4208e0 · c4208e0
1 parent bdc18f6
commit c4208e0
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 17 deletions.
diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h
@@ -20,7 +20,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
 
 protected:
   void changeSpeedMode(SpeedMode speed_mode);
-  double getDynamicScale(double base_scale, double amplitude, double period, double phase);
+  double getDynamicScale(const double base_scale, const double amplitude, const double period, const 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;

diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp
@@ -87,6 +87,21 @@ void ChassisGimbalShooterCoverManual::updatePc(const rm_msgs::DbusData::ConstPtr
   ChassisGimbalShooterManual::updatePc(dbus_data);
   gimbal_cmd_sender_->setRate(-dbus_data->m_x * gimbal_scale_,
                               cover_command_sender_->getState() ? 0.0 : dbus_data->m_y * gimbal_scale_);
+  if (is_gyro_)
+  {
+    if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
+      if (x_scale_ != 0.0 || y_scale_ != 0.0)
+        vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_, gyro_speed_limit_);
+      else
+        vel_cmd_sender_->setAngularZVel(1.0, gyro_speed_limit_);
+    else if (x_scale_ != 0.0 || y_scale_ != 0.0)
+      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(
+          getDynamicScale(sin_gyro_base_scale_, sin_gyro_amplitude_, sin_gyro_period_, sin_gyro_phase_));
+  }
 }
 
 void ChassisGimbalShooterCoverManual::checkReferee()
@@ -153,22 +168,6 @@ void ChassisGimbalShooterCoverManual::sendCommand(const ros::Time& time)
   }
   ChassisGimbalShooterManual::sendCommand(time);
   cover_command_sender_->sendCommand(time);
-  if (state_ == PC && is_gyro_)
-  {
-    if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR)
-      if (x_scale_ != 0.0 || y_scale_ != 0.0)
-        vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_, gyro_speed_limit_);
-      else
-        vel_cmd_sender_->setAngularZVel(1.0, gyro_speed_limit_);
-    else if (x_scale_ != 0.0 || y_scale_ != 0.0)
-      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(
-          getDynamicScale(sin_gyro_base_scale_, sin_gyro_amplitude_, sin_gyro_period_, sin_gyro_phase_));
-    vel_cmd_sender_->sendCommand(time);
-  }
 }
 
 void ChassisGimbalShooterCoverManual::rightSwitchDownRise()