From 58f53c7c0347b4d9bd874ed28bc155a4420713f6 Mon Sep 17 00:00:00 2001 From: 1moule <1961466188@qq.com> Date: Tue, 28 May 2024 02:04:51 +0800 Subject: [PATCH 1/5] Set chassis follow vel des. --- src/chassis_gimbal_manual.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/chassis_gimbal_manual.cpp b/src/chassis_gimbal_manual.cpp index cd250d1c..b5427647 100644 --- a/src/chassis_gimbal_manual.cpp +++ b/src/chassis_gimbal_manual.cpp @@ -47,11 +47,14 @@ void ChassisGimbalManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) { ManualBase::updateRc(dbus_data); gimbal_cmd_sender_->setRate(-dbus_data->ch_l_x, -dbus_data->ch_l_y); + chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw); } + 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_); + chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw); } void ChassisGimbalManual::checkReferee() From d729e976fd779502a00ec2dd2947a6affd140f52 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 17 Jul 2024 11:39:35 +0800 Subject: [PATCH 2/5] Use dbusFeedforward for chassis only when gimbal in rate. --- src/chassis_gimbal_manual.cpp | 2 -- src/chassis_gimbal_shooter_manual.cpp | 8 ++++++++ 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/chassis_gimbal_manual.cpp b/src/chassis_gimbal_manual.cpp index b5427647..d7b4ffab 100644 --- a/src/chassis_gimbal_manual.cpp +++ b/src/chassis_gimbal_manual.cpp @@ -47,14 +47,12 @@ void ChassisGimbalManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) { ManualBase::updateRc(dbus_data); gimbal_cmd_sender_->setRate(-dbus_data->ch_l_x, -dbus_data->ch_l_y); - chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw); } 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_); - chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw); } void ChassisGimbalManual::checkReferee() diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index af95de4d..5ec57be4 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -251,6 +251,10 @@ void ChassisGimbalShooterManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbu if (shooter_cmd_sender_->getMsg()->mode != rm_msgs::ShootCmd::STOP) gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed()); + if (gimbal_cmd_sender_->getMsg()->mode == rm_msgs::GimbalCmd::RATE) + chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw); + else + chassis_cmd_sender_->setFollowVelDes(0); } void ChassisGimbalShooterManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) @@ -265,6 +269,10 @@ void ChassisGimbalShooterManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbu else if (chassis_power_ < 6.0 && chassis_cmd_sender_->getMsg()->mode == rm_msgs::ChassisCmd::FOLLOW) chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE); } + if (gimbal_cmd_sender_->getMsg()->mode == rm_msgs::GimbalCmd::RATE) + chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw); + else + chassis_cmd_sender_->setFollowVelDes(0); } void ChassisGimbalShooterManual::rightSwitchDownRise() From 485af9ba09645f9d22d474541b91f54b3d281164 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 17 Jul 2024 15:50:49 +0800 Subject: [PATCH 3/5] Fix format. --- src/chassis_gimbal_manual.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/chassis_gimbal_manual.cpp b/src/chassis_gimbal_manual.cpp index d7b4ffab..cd250d1c 100644 --- a/src/chassis_gimbal_manual.cpp +++ b/src/chassis_gimbal_manual.cpp @@ -48,7 +48,6 @@ void ChassisGimbalManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) ManualBase::updateRc(dbus_data); gimbal_cmd_sender_->setRate(-dbus_data->ch_l_x, -dbus_data->ch_l_y); } - void ChassisGimbalManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) { ManualBase::updatePc(dbus_data); From 12e61ec4ef666883ed5f058e035c8e0c75da2306 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 17 Jul 2024 16:17:08 +0800 Subject: [PATCH 4/5] Modify something unreasonable. --- src/chassis_gimbal_shooter_manual.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 5ec57be4..80eda252 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -254,7 +254,7 @@ void ChassisGimbalShooterManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbu if (gimbal_cmd_sender_->getMsg()->mode == rm_msgs::GimbalCmd::RATE) chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw); else - chassis_cmd_sender_->setFollowVelDes(0); + chassis_cmd_sender_->setFollowVelDes(0.); } void ChassisGimbalShooterManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) @@ -272,7 +272,7 @@ void ChassisGimbalShooterManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbu if (gimbal_cmd_sender_->getMsg()->mode == rm_msgs::GimbalCmd::RATE) chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw); else - chassis_cmd_sender_->setFollowVelDes(0); + chassis_cmd_sender_->setFollowVelDes(0.); } void ChassisGimbalShooterManual::rightSwitchDownRise() From f642aedff6643ed9a8279700d6a4de58d61c18ff Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 17 Jul 2024 20:40:51 +0800 Subject: [PATCH 5/5] Write the code to the parent class. --- src/chassis_gimbal_manual.cpp | 8 ++++++++ src/chassis_gimbal_shooter_manual.cpp | 8 -------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/chassis_gimbal_manual.cpp b/src/chassis_gimbal_manual.cpp index cd250d1c..71ac115c 100644 --- a/src/chassis_gimbal_manual.cpp +++ b/src/chassis_gimbal_manual.cpp @@ -47,11 +47,19 @@ void ChassisGimbalManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) { ManualBase::updateRc(dbus_data); gimbal_cmd_sender_->setRate(-dbus_data->ch_l_x, -dbus_data->ch_l_y); + if (gimbal_cmd_sender_->getMsg()->mode == rm_msgs::GimbalCmd::RATE) + chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw); + else + chassis_cmd_sender_->setFollowVelDes(0.); } 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_); + if (gimbal_cmd_sender_->getMsg()->mode == rm_msgs::GimbalCmd::RATE) + chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw); + else + chassis_cmd_sender_->setFollowVelDes(0.); } void ChassisGimbalManual::checkReferee() diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 80eda252..af95de4d 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -251,10 +251,6 @@ void ChassisGimbalShooterManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbu if (shooter_cmd_sender_->getMsg()->mode != rm_msgs::ShootCmd::STOP) gimbal_cmd_sender_->setBulletSpeed(shooter_cmd_sender_->getSpeed()); - if (gimbal_cmd_sender_->getMsg()->mode == rm_msgs::GimbalCmd::RATE) - chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw); - else - chassis_cmd_sender_->setFollowVelDes(0.); } void ChassisGimbalShooterManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) @@ -269,10 +265,6 @@ void ChassisGimbalShooterManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbu else if (chassis_power_ < 6.0 && chassis_cmd_sender_->getMsg()->mode == rm_msgs::ChassisCmd::FOLLOW) chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE); } - if (gimbal_cmd_sender_->getMsg()->mode == rm_msgs::GimbalCmd::RATE) - chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw); - else - chassis_cmd_sender_->setFollowVelDes(0.); } void ChassisGimbalShooterManual::rightSwitchDownRise()