diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h index 9d4f8ab4..3a37660f 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; @@ -27,14 +28,21 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual void rightSwitchDownRise() override; void rightSwitchMidRise() override; void rightSwitchUpRise() override; - void rPress() override; void ePress() override; + void cPress() override; void zPress(); void zRelease(); - void wPress() override; - void wPressing() override; void ctrlRPressing(); void ctrlRRelease() override; + void wPress() override; + void wPressing() override; + 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() @@ -43,6 +51,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual }; double low_speed_scale_{}, normal_speed_scale_{}; double exit_buff_mode_duration_{}; + double gyro_speed_limit_{}; rm_common::SwitchDetectionCaller* switch_buff_srv_{}; rm_common::SwitchDetectionCaller* switch_buff_type_srv_{}; rm_common::SwitchDetectionCaller* switch_exposure_srv_{}; diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h index 2fe98695..c9ce2e1d 100644 --- a/include/rm_manual/chassis_gimbal_shooter_manual.h +++ b/include/rm_manual/chassis_gimbal_shooter_manual.h @@ -88,10 +88,10 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual virtual void cPress(); virtual void bPress(); virtual void bRelease(); - virtual void rPress(); virtual void xRelease(); virtual void shiftPress(); virtual void shiftRelease(); + void rPress(); void qPress() { shooter_cmd_sender_->setShootFrequency(rm_common::HeatLimit::BURST); diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp index 0f984a03..73a92802 100644 --- a/src/chassis_gimbal_shooter_cover_manual.cpp +++ b/src/chassis_gimbal_shooter_cover_manual.cpp @@ -22,6 +22,7 @@ ChassisGimbalShooterCoverManual::ChassisGimbalShooterCoverManual(ros::NodeHandle normal_speed_scale_ = chassis_nh.param("normal_speed_scale", 1); low_speed_scale_ = chassis_nh.param("low_speed_scale", 0.30); nh.param("exit_buff_mode_duration", exit_buff_mode_duration_, 0.5); + nh.param("gyro_speed_limit", gyro_speed_limit_, 6.0); ctrl_z_event_.setEdge(boost::bind(&ChassisGimbalShooterCoverManual::ctrlZPress, this), boost::bind(&ChassisGimbalShooterCoverManual::ctrlZRelease, this)); @@ -42,6 +43,24 @@ void ChassisGimbalShooterCoverManual::changeSpeedMode(SpeedMode speed_mode) } } +void ChassisGimbalShooterCoverManual::changeGyroSpeedMode(SpeedMode speed_mode) +{ + if (speed_mode == LOW) + { + 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 (speed_mode == NORMAL) + { + if (x_scale_ != 0.0 || y_scale_ != 0.0) + vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_); + else + vel_cmd_sender_->setAngularZVel(1.0); + } +} + void ChassisGimbalShooterCoverManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data) { ChassisGimbalShooterManual::updatePc(dbus_data); @@ -133,21 +152,6 @@ void ChassisGimbalShooterCoverManual::rightSwitchUpRise() supply_ = false; } -void ChassisGimbalShooterCoverManual::rPress() -{ - if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) - { - chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE); - if (switch_buff_type_srv_->getTarget() == rm_msgs::StatusChangeRequest::SMALL_BUFF) - switch_buff_type_srv_->setTargetType(rm_msgs::StatusChangeRequest::BIG_BUFF); - else - switch_buff_type_srv_->setTargetType(rm_msgs::StatusChangeRequest::SMALL_BUFF); - switch_buff_type_srv_->callService(); - } - else - chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL); -} - void ChassisGimbalShooterCoverManual::ePress() { switch_buff_srv_->switchTargetType(); @@ -158,6 +162,30 @@ void ChassisGimbalShooterCoverManual::ePress() switch_detection_srv_->callService(); switch_buff_type_srv_->callService(); switch_exposure_srv_->callService(); + if (is_gyro_) + { + if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) + changeGyroSpeedMode(LOW); + else + changeGyroSpeedMode(NORMAL); + } +} + +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 (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) + changeGyroSpeedMode(LOW); + else + changeGyroSpeedMode(NORMAL); + } } void ChassisGimbalShooterCoverManual::zPress() @@ -193,6 +221,57 @@ void ChassisGimbalShooterCoverManual::wPressing() switch_buff_type_srv_->callService(); switch_exposure_srv_->callService(); } + if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) + vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, gyro_speed_limit_); +} + +void ChassisGimbalShooterCoverManual::aPressing() +{ + ChassisGimbalShooterManual::aPressing(); + if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) + vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, gyro_speed_limit_); +} + +void ChassisGimbalShooterCoverManual::sPressing() +{ + ChassisGimbalShooterManual::sPressing(); + if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) + vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, gyro_speed_limit_); +} + +void ChassisGimbalShooterCoverManual::dPressing() +{ + ChassisGimbalShooterManual::dPressing(); + if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) + vel_cmd_sender_->setAngularZVel(is_gyro_ ? gyro_rotate_reduction_ : 0, gyro_speed_limit_); +} + +void ChassisGimbalShooterCoverManual::wRelease() +{ + ChassisGimbalShooterManual::wRelease(); + if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) + vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0, gyro_speed_limit_); +} + +void ChassisGimbalShooterCoverManual::aRelease() +{ + ChassisGimbalShooterManual::aRelease(); + if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) + vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0, gyro_speed_limit_); +} + +void ChassisGimbalShooterCoverManual::sRelease() +{ + ChassisGimbalShooterManual::sRelease(); + if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) + vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0, gyro_speed_limit_); +} + +void ChassisGimbalShooterCoverManual::dRelease() +{ + ChassisGimbalShooterManual::dRelease(); + if (switch_buff_srv_->getTarget() != rm_msgs::StatusChangeRequest::ARMOR) + vel_cmd_sender_->setAngularZVel(is_gyro_ ? 1 : 0, gyro_speed_limit_); } void ChassisGimbalShooterCoverManual::ctrlZPress() diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp index 03fd56c5..b3cbc881 100644 --- a/src/chassis_gimbal_shooter_manual.cpp +++ b/src/chassis_gimbal_shooter_manual.cpp @@ -425,8 +425,6 @@ void ChassisGimbalShooterManual::bRelease() void ChassisGimbalShooterManual::rPress() { - if (camera_switch_cmd_sender_) - camera_switch_cmd_sender_->switchCamera(); if (scope_cmd_sender_) { use_scope_ = !scope_cmd_sender_->getState();