From 6bf00c95174ffa86ed4a7a26db32233100b54947 Mon Sep 17 00:00:00 2001 From: 321aurora <2508260166@qq.com> Date: Mon, 5 May 2025 23:54:28 +0800 Subject: [PATCH 1/4] Add a function for hero to reset friction speed and override standard ctrlQ. --- include/rm_manual/chassis_gimbal_shooter_cover_manual.h | 1 + src/chassis_gimbal_shooter_cover_manual.cpp | 6 ++++++ src/chassis_gimbal_shooter_manual.cpp | 2 ++ 3 files changed, 9 insertions(+) diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h index e2a8a48c..eada418c 100644 --- a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h @@ -48,6 +48,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual void zPress() override { } + void ctrlQPress() override; virtual void ctrlZPress(); virtual void ctrlZRelease() diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index 9f9598ff..2870a337 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -362,4 +362,10 @@ void ChassisGimbalShooterCoverManual::ctrlRRelease() shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); } +void ChassisGimbalShooterCoverManual::ctrlQPress() +{ + shooter_calibration_->reset(); + gimbal_calibration_->reset(); +} + } // namespace rm_manual diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index a5c7fc0a..fdcd6340 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -703,6 +703,8 @@ void ChassisGimbalShooterManual::ctrlQPress() { shooter_calibration_->reset(); gimbal_calibration_->reset(); + if (gimbal_cmd_sender_->getMsg()->mode != rm_msgs::GimbalCmd::TRAJ) + shooter_cmd_sender_->resetExtraWheelSpeed(); up_change_position_ = false; low_change_position_ = false; need_change_position_ = false; From d9ede1a8c1055e82dc0b79fc4c9f52add5655c18 Mon Sep 17 00:00:00 2001 From: 321aurora <2508260166@qq.com> Date: Mon, 2 Jun 2025 11:03:49 +0800 Subject: [PATCH 2/4] Fix the bug for deploy mode. --- src/chassis_gimbal_shooter_manual.cpp | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index fdcd6340..2355eaca 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -235,6 +235,7 @@ void ChassisGimbalShooterManual::remoteControlTurnOff() up_change_position_ = false; low_change_position_ = false; need_change_position_ = false; + deployed_ = false; } void ChassisGimbalShooterManual::remoteControlTurnOn() @@ -256,6 +257,7 @@ void ChassisGimbalShooterManual::robotDie() up_change_position_ = false; low_change_position_ = false; need_change_position_ = false; + deployed_ = false; } void ChassisGimbalShooterManual::chassisOutputOn() @@ -317,6 +319,15 @@ void ChassisGimbalShooterManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbu traj_pitch_ += traj_scale_ * gimbal_cmd_sender_->getMsg()->rate_pitch * ros::Duration(0.01).toSec(); gimbal_cmd_sender_->setGimbalTraj(traj_yaw_, traj_pitch_); } + if ((gimbal_cmd_sender_->getMsg()->mode == rm_msgs::GimbalCmd::TRAJ && deployed_) && + std::sqrt(std::pow(vel_cmd_sender_->getMsg()->linear.x, 2) + std::pow(vel_cmd_sender_->getMsg()->linear.y, 2)) > + 0.0) + { + setChassisMode(rm_msgs::ChassisCmd::FOLLOW); + gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); + deployed_ = false; + shooter_cmd_sender_->setDeployState(false); + } } void ChassisGimbalShooterManual::rightSwitchDownRise() @@ -615,7 +626,7 @@ void ChassisGimbalShooterManual::vPress() void ChassisGimbalShooterManual::zPress() { - if (chassis_cmd_sender_->getMsg()->mode != rm_msgs::ChassisCmd::RAW && !is_gyro_) + if (chassis_cmd_sender_->getMsg()->mode != rm_msgs::ChassisCmd::RAW && !deployed_) { double roll{}, pitch{}, yaw{}; try @@ -628,25 +639,23 @@ void ChassisGimbalShooterManual::zPress() } gimbal_cmd_sender_->setGimbalTrajFrameId("base_link"); gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ); - traj_yaw_ = yaw, traj_pitch_ = -0.40; + traj_yaw_ = yaw, traj_pitch_ = -0.585; gimbal_cmd_sender_->setGimbalTraj(traj_yaw_, traj_pitch_); setChassisMode(rm_msgs::ChassisCmd::DEPLOY); - shooter_cmd_sender_->deploySpeed(); + shooter_cmd_sender_->setDeployState(true); deployed_ = true; } else { setChassisMode(rm_msgs::ChassisCmd::FOLLOW); gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); - shooter_cmd_sender_->exitDeploySpeed(); + shooter_cmd_sender_->setDeployState(false); deployed_ = false; } } void ChassisGimbalShooterManual::shiftPress() { - if (chassis_cmd_sender_->getMsg()->mode != rm_msgs::ChassisCmd::FOLLOW && is_gyro_) - setChassisMode(rm_msgs::ChassisCmd::FOLLOW); chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST); } @@ -703,8 +712,7 @@ void ChassisGimbalShooterManual::ctrlQPress() { shooter_calibration_->reset(); gimbal_calibration_->reset(); - if (gimbal_cmd_sender_->getMsg()->mode != rm_msgs::GimbalCmd::TRAJ) - shooter_cmd_sender_->resetExtraWheelSpeed(); + adjust_image_transmission_ = false; up_change_position_ = false; low_change_position_ = false; need_change_position_ = false; From f8e8e1186ff267b5cc78d4da368f9182a95e3bec Mon Sep 17 00:00:00 2001 From: 321aurora <2508260166@qq.com> Date: Mon, 2 Jun 2025 16:51:06 +0800 Subject: [PATCH 3/4] Fix the bug of the direction for pitch. --- src/chassis_gimbal_manual.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/chassis_gimbal_manual.cpp b/src/chassis_gimbal_manual.cpp index 53f1ccc7..65fdeff9 100644 --- a/src/chassis_gimbal_manual.cpp +++ b/src/chassis_gimbal_manual.cpp @@ -56,7 +56,7 @@ void ChassisGimbalManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) void ChassisGimbalManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) { ManualBase::updatePc(dbus_data); - gimbal_cmd_sender_->setRate(-dbus_data->m_x * gimbal_scale_, dbus_data->m_y * gimbal_scale_); + gimbal_cmd_sender_->setRate(-dbus_data->m_x * gimbal_scale_, -dbus_data->m_y * gimbal_scale_); if (gimbal_cmd_sender_->getMsg()->mode == rm_msgs::GimbalCmd::RATE) chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw); else From 9591dd707bdc0b7dfc1b1373f6d6e69797b9ab37 Mon Sep 17 00:00:00 2001 From: 321aurora <2508260166@qq.com> Date: Mon, 2 Jun 2025 17:05:08 +0800 Subject: [PATCH 4/4] Delete some unused. --- include/rm_manual/chassis_gimbal_shooter_cover_manual.h | 1 - src/chassis_gimbal_shooter_cover_manual.cpp | 6 ------ 2 files changed, 7 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h index eada418c..e2a8a48c 100644 --- a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h @@ -48,7 +48,6 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual void zPress() override { } - void ctrlQPress() override; virtual void ctrlZPress(); virtual void ctrlZRelease() diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index 2870a337..9f9598ff 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -362,10 +362,4 @@ void ChassisGimbalShooterCoverManual::ctrlRRelease() shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY); } -void ChassisGimbalShooterCoverManual::ctrlQPress() -{ - shooter_calibration_->reset(); - gimbal_calibration_->reset(); -} - } // namespace rm_manual