diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index bd717de2..ff89def1 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -320,6 +320,12 @@ class ShooterCommandSender : public TimeStampCommandSenderBasegetSpeedLimit(); + msg_.wheel_speed = getWheelSpeedDes(); msg_.hz = heat_limit_->getShootFrequency(); TimeStampCommandSenderBase::sendCommand(time); } double getSpeed() { - switch (msg_.speed) + setSpeedDesAndWheelSpeedDes(); + return speed_des_; + } + double getWheelSpeedDes() + { + setSpeedDesAndWheelSpeedDes(); + return wheel_speed_des_ + total_extra_wheel_speed_; + } + void setSpeedDesAndWheelSpeedDes() + { + switch (heat_limit_->getSpeedLimit()) { case rm_msgs::ShootCmd::SPEED_10M_PER_SECOND: - return speed_10_; + { + speed_des_ = speed_10_; + wheel_speed_des_ = wheel_speed_10_; + break; + } case rm_msgs::ShootCmd::SPEED_15M_PER_SECOND: - return speed_15_; + { + speed_des_ = speed_15_; + wheel_speed_des_ = wheel_speed_15_; + break; + } case rm_msgs::ShootCmd::SPEED_16M_PER_SECOND: - return speed_16_; + { + speed_des_ = speed_16_; + wheel_speed_des_ = wheel_speed_16_; + break; + } case rm_msgs::ShootCmd::SPEED_18M_PER_SECOND: - return speed_18_; + { + speed_des_ = speed_18_; + wheel_speed_des_ = wheel_speed_18_; + break; + } case rm_msgs::ShootCmd::SPEED_30M_PER_SECOND: - return speed_30_; + { + speed_des_ = speed_30_; + wheel_speed_des_ = wheel_speed_30_; + break; + } } - return 0.; + } + void dropSpeed() + { + total_extra_wheel_speed_ -= extra_wheel_speed_once_; + } + void raiseSpeed() + { + total_extra_wheel_speed_ += extra_wheel_speed_once_; } void setArmorType(uint8_t armor_type) { @@ -404,9 +447,13 @@ class ShooterCommandSender : public TimeStampCommandSenderBase