Maintain Rc wheel function.

rm-controls · Dec 1, 2024 · ea5e1cc · ea5e1cc
1 parent 9b081f5
commit ea5e1cc
Showing 1 changed file with 16 additions and 0 deletions.
diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp
@@ -155,6 +155,22 @@ 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()