From 4a8c9559b63ebd2273ae8601c45737693565a0a2 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Sun, 4 Aug 2024 20:29:10 +0800 Subject: [PATCH] Add slow mode for gyro's different speed. --- include/rm_manual/chassis_gimbal_manual.h | 1 + .../chassis_gimbal_shooter_cover_manual.h | 6 ++ src/chassis_gimbal_manual.cpp | 4 +- src/chassis_gimbal_shooter_cover_manual.cpp | 76 ++++++++++++------- 4 files changed, 58 insertions(+), 29 deletions(-) diff --git a/include/rm_manual/chassis_gimbal_manual.h b/include/rm_manual/chassis_gimbal_manual.h index 6c07ed8b..91f55491 100644 --- a/include/rm_manual/chassis_gimbal_manual.h +++ b/include/rm_manual/chassis_gimbal_manual.h @@ -64,6 +64,7 @@ class ChassisGimbalManual : public ManualBase double x_scale_{}, y_scale_{}; bool is_gyro_{ 0 }; double speed_change_scale_{ 1. }; + double gyro_speed_change_scale_{ 1. }; double gimbal_scale_{ 1. }; double gyro_move_reduction_{ 1. }; double gyro_rotate_reduction_{ 1. }; diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h index defce76b..031ca0a3 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); + 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; void checkReferee() override; @@ -38,6 +39,10 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual void aPressing() override; void sPressing() override; void dPressing() override; + void wRelease() override; + void aRelease() override; + void sRelease() override; + void dRelease() override; virtual void ctrlZPress(); virtual void ctrlZRelease() @@ -45,6 +50,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE); }; double low_speed_scale_{}, normal_speed_scale_{}; + double low_gyro_speed_scale_{}, normal_gyro_speed_scale_{}; double exit_buff_mode_duration_{}; double buff_gyro_rotate_limit_{}; rm_common::SwitchDetectionCaller* switch_buff_srv_{}; diff --git a/src/chassis_gimbal_manual.cpp b/src/chassis_gimbal_manual.cpp index efd171ec..50f28f41 100644 --- a/src/chassis_gimbal_manual.cpp +++ b/src/chassis_gimbal_manual.cpp @@ -230,9 +230,9 @@ void ChassisGimbalManual::setChassisMode(int mode) chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW); is_gyro_ = true; if (x_scale_ != 0.0 || y_scale_ != 0.0) - vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_); + vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_ * gyro_speed_change_scale_); else - vel_cmd_sender_->setAngularZVel(1.0); + vel_cmd_sender_->setAngularZVel(1.0 * gyro_speed_change_scale_); break; case rm_msgs::ChassisCmd::FOLLOW: chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW); diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index 4609313a..f222be47 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -21,6 +21,8 @@ ChassisGimbalShooterCoverManual::ChassisGimbalShooterCoverManual(ros::NodeHandle ros::NodeHandle chassis_nh(nh, "chassis"); normal_speed_scale_ = chassis_nh.param("normal_speed_scale", 1); low_speed_scale_ = chassis_nh.param("low_speed_scale", 0.30); + normal_gyro_speed_scale_ = chassis_nh.param("normal_gryo_speed_scale", 1); + low_gyro_speed_scale_ = chassis_nh.param("low_gyro_speed_scale", 0.30); nh.param("exit_buff_mode_duration", exit_buff_mode_duration_, 0.5); nh.param("buff_gyro_rotate_limit", buff_gyro_rotate_limit_, 6.0); @@ -43,6 +45,18 @@ void ChassisGimbalShooterCoverManual::changeSpeedMode(SpeedMode speed_mode) } } +void ChassisGimbalShooterCoverManual::changeGyroSpeedMode(SpeedMode speed_mode) +{ + if (speed_mode == LOW) + { + gyro_speed_change_scale_ = low_gyro_speed_scale_; + } + else if (speed_mode == NORMAL) + { + gyro_speed_change_scale_ = normal_gyro_speed_scale_; + } +} + void ChassisGimbalShooterCoverManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) { ChassisGimbalShooterManual::updatePc(dbus_data); @@ -165,29 +179,9 @@ void ChassisGimbalShooterCoverManual::ePress() void ChassisGimbalShooterCoverManual::cPress() { - if (is_gyro_) - { - setChassisMode(rm_msgs::ChassisCmd::FOLLOW); - } - else - { - setChassisMode(rm_msgs::ChassisCmd::RAW); - chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); - if (x_scale_ != 0.0 || y_scale_ != 0.0) - { - if (switch_buff_srv_->getTarget() == rm_msgs::StatusChangeRequest::SMALL_BUFF) - vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_, buff_gyro_rotate_limit_); - else - vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_); - } - else - { - if (switch_buff_srv_->getTarget() == rm_msgs::StatusChangeRequest::SMALL_BUFF) - vel_cmd_sender_->setAngularZVel(1.0, buff_gyro_rotate_limit_); - else - vel_cmd_sender_->setAngularZVel(1.0); - } - } + ChassisGimbalShooterManual::cPress(); + if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_) + changeGyroSpeedMode(LOW); } void ChassisGimbalShooterCoverManual::zPress() @@ -223,7 +217,7 @@ void ChassisGimbalShooterCoverManual::wPressing() switch_buff_type_srv_->callService(); switch_exposure_srv_->callService(); if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_) - vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, buff_gyro_rotate_limit_); + changeGyroSpeedMode(LOW); } } @@ -231,23 +225,51 @@ void ChassisGimbalShooterCoverManual::aPressing() { ChassisGimbalShooterManual::aPressing(); if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_) - vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, buff_gyro_rotate_limit_); + changeGyroSpeedMode(LOW); } void ChassisGimbalShooterCoverManual::sPressing() { ChassisGimbalShooterManual::sPressing(); if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_) - vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, buff_gyro_rotate_limit_); + changeGyroSpeedMode(LOW); } void ChassisGimbalShooterCoverManual::dPressing() { ChassisGimbalShooterManual::dPressing(); if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_) - vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, buff_gyro_rotate_limit_); + changeGyroSpeedMode(LOW); } +void ChassisGimbalShooterCoverManual::wRelease() +{ + ChassisGimbalShooterManual::wRelease(); + if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_) + changeGyroSpeedMode(LOW); +}; + +void ChassisGimbalShooterCoverManual::aRelease() +{ + ChassisGimbalShooterManual::aRelease(); + if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_) + changeGyroSpeedMode(LOW); +}; + +void ChassisGimbalShooterCoverManual::sRelease() +{ + ChassisGimbalShooterManual::sRelease(); + if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_) + changeGyroSpeedMode(LOW); +}; + +void ChassisGimbalShooterCoverManual::dRelease() +{ + ChassisGimbalShooterManual::dRelease(); + if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR && is_gyro_) + changeGyroSpeedMode(LOW); +}; + void ChassisGimbalShooterCoverManual::ctrlZPress() { if (!cover_command_sender_->getState())