diff --git a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml index a4b85a3a..2dd12af4 100644 --- a/rmcs_ws/src/rmcs_bringup/config/sentry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/sentry.yaml @@ -22,7 +22,7 @@ rmcs_executor: - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::SteeringWheelController -> steering_wheel_controller - # - rmcs::navigation::Navigation -> rmcs_navigation + - rmcs::navigation::Navigation -> rmcs_navigation - rmcs::AutoAimRecorderComponent -> auto_aim_recorder - rmcs::AutoAimCapturerComponent -> auto_aim_capturer @@ -40,7 +40,7 @@ rmcs_navigation: auto_aim_capturer: ros__parameters: camera_name: "" - exposure_us: 2000.0 + exposure_us: 1000.0 gain: 8.0 framerate: 120.0 invert_image: true @@ -60,7 +60,7 @@ auto_aim_recorder: auto_aim_component: ros__parameters: manual_shoot: true - enable_rune: true + enable_rune: false camera_translation: [0.07128, 0.0, 0.0481] fire_control: bullet_speed: 22.5 @@ -68,7 +68,7 @@ auto_aim_component: offset_yaw: -0.3 offset_pitch: +0.9 attack_window: 80.0 - degraded_angle_speed: 12.0 + degraded_angle_speed: 10.0 window_hysteresis: 0.2 attack_preaim: false require_stable_command: false @@ -144,7 +144,7 @@ sentry_climber: stick_group: speed_drop: 30.0 speed_rise: 60.0 - rise_torque_limit: 1.2 + rise_torque_limit: 1.5 land_speed_begin: 100.0 land_speed_final: 10.0 land_duration: 0.5 @@ -158,7 +158,7 @@ sentry_climber: hold_torque: 0.01 block_hold: 0.05 align: - err: 0.20 + err: 0.18 w: 0.2 hold: 0.05 timeout: 15.0 @@ -168,7 +168,7 @@ sentry_climber: approach_vx: 1.2 deploy_vx: 0.3 dash_vx: 3.0 - retract_vx: 0.3 + retract_vx: 0.0 dash_min: 0.1 dash_duration: 0.8 stick_timeout: 8.0 @@ -181,6 +181,8 @@ sentry_climber: stick_timeout: 8.0 soft_timeout: 3.0 settle_timeout: 8.0 + leave_vx: 0.3 + leave_duration: 1.0 friction_wheel_controller: ros__parameters: @@ -247,11 +249,11 @@ steering_wheel_controller: no_load_power: 11.37 chassis_translation_kp: 20.0 - chassis_translation_ki: 0.001 + chassis_translation_ki: 0.0001 chassis_translation_kd: 0.00 chassis_angular_velocity_kp: 8.0 - chassis_angular_velocity_ki: 0.0 + chassis_angular_velocity_ki: 0.005 chassis_angular_velocity_kd: 1.0 steering_velocity_kp: 0.15 diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp index b4bad88f..f9e8e83a 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_power_controller.cpp @@ -30,6 +30,7 @@ class ChassisPowerController register_input("/chassis/power", chassis_power_); register_input("/chassis/supercap/voltage", supercap_voltage_); register_input("/chassis/supercap/enabled", supercap_enabled_); + register_input("/rmcs_navigation/enable_supercap", navigation_supercap_, false); register_input("/referee/chassis/power_limit", chassis_power_limit_referee_); register_input("/referee/chassis/buffer_energy", chassis_buffer_energy_referee_); @@ -110,7 +111,10 @@ class ChassisPowerController void update_control_power_limit() { double power_limit; - if (boost_mode_ && *supercap_enabled_) + const auto navigation_supercap_boost = + navigation_supercap_.ready() && *navigation_supercap_; + + if ((boost_mode_ || navigation_supercap_boost) && *supercap_enabled_) power_limit = rmcs_msgs::is_powered(*mode_) ? inf_ : *chassis_power_limit_referee_ + 80.0; else @@ -159,6 +163,7 @@ class ChassisPowerController InputInterface supercap_voltage_; InputInterface supercap_enabled_; + InputInterface navigation_supercap_; InputInterface chassis_power_limit_referee_; InputInterface chassis_buffer_energy_referee_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp index 3b60a0f5..04d3f955 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/sentry_climber.cpp @@ -60,6 +60,8 @@ class SentryClimber double stick_timeout; double soft_timeout; double settle_timeout; + double leave_vx; + double leave_duration; } land; double block_hold; @@ -126,6 +128,8 @@ class SentryClimber .stick_timeout = param_or("land.stick_timeout", 8.0), .soft_timeout = param_or("land.soft_timeout", 3.0), .settle_timeout = param_or("land.settle_timeout", 8.0), + .leave_vx = param_or("land.leave_vx", 0.5), + .leave_duration = param_or("land.leave_duration", 1.0), }, .block_hold = param_or("block_hold", 0.05), }; @@ -239,7 +243,21 @@ class SentryClimber OutputInterface chassis_track_direction; // 以履带方向为正向 OutputInterface chassis_climb_speed; // 正向为基准的速度值 - OutputInterface chassis_climb_status; // 事件进度 + OutputInterface + chassis_climb_status; // 事件进度: 0=空闲, 1=成功, -1=失败, (0,1)阶段小数 + + // /chassis/climber/status 阶段编码：(0, 0.55) 上台阶，[0.55, 1) 下台阶 + static constexpr double kStatusClimbAlign = 0.1; + static constexpr double kStatusClimbApproach = 0.2; + static constexpr double kStatusClimbDeploy = 0.3; + static constexpr double kStatusClimbDash = 0.4; + static constexpr double kStatusClimbRetract = 0.5; + static constexpr double kStatusLandAlign = 0.6; + static constexpr double kStatusLandDash = 0.7; + static constexpr double kStatusLandSettle = 0.75; + static constexpr double kStatusLandSoft = 0.8; + static constexpr double kStatusLandFinal = 0.9; + static constexpr double kStatusLandLeave = 0.95; struct SimpleComponent : public rmcs_executor::Component { std::function fn; @@ -390,7 +408,7 @@ class SentryClimber auto climb(double direction) -> CoSchduler::Task { using namespace std::chrono_literals; - *chassis_climb_status = 0.0; + *chassis_climb_status = kStatusClimbAlign; node::info("Climb start, direction={:.3f}", direction); *chassis_track_direction = direction; @@ -400,6 +418,7 @@ class SentryClimber stick_group->set_state(StickState::kHold); *chassis_climb_speed = 0.0; { + const auto t0 = std::chrono::steady_clock::now(); const auto timed_out = co_await context.wait_align( direction, config.align.err, config.align.w, seconds_to_duration(config.align.hold), seconds_to_duration(config.align.timeout)); @@ -409,9 +428,15 @@ class SentryClimber *chassis_climb_status = -1; co_return; } + const auto elapsed = + std::chrono::duration(std::chrono::steady_clock::now() - t0); + node::info( + "climb ALIGN done: err={:.3f}, took={:.3f}s", context.align_error(direction), + elapsed.count()); } // [] 冲向台阶，开启履带，让底盘沿着台阶边缘上升，直到倾斜到一定角度 + *chassis_climb_status = kStatusClimbApproach; track_group->set_state(TrackState::kRush); stick_group->set_state(StickState::kHold); *chassis_climb_speed = config.climb.approach_vx; @@ -425,6 +450,7 @@ class SentryClimber } // [] 伸出撑杆，同时慢速向台阶方向前进 + *chassis_climb_status = kStatusClimbDeploy; track_group->set_state(TrackState::kHold); stick_group->set_state(StickState::kDrop); *chassis_climb_speed = config.climb.deploy_vx; @@ -436,12 +462,14 @@ class SentryClimber } // [] 撑杆已完全伸出，全力冲上台阶，保持一定时间间隔 + *chassis_climb_status = kStatusClimbDash; track_group->set_state(TrackState::kHold); stick_group->set_state(StickState::kHold); *chassis_climb_speed = config.climb.dash_vx; co_await CoSchduler::Sleep{seconds_to_duration(config.climb.dash_duration)}; // [] 上台阶完毕，收回撑杆 + *chassis_climb_status = kStatusClimbRetract; track_group->set_state(TrackState::kHold); stick_group->set_state(StickState::kRise); *chassis_climb_speed = config.climb.retract_vx; @@ -463,7 +491,7 @@ class SentryClimber } auto land(double direction) -> CoSchduler::Task { - *chassis_climb_status = 0.0; + *chassis_climb_status = kStatusLandAlign; node::info("Land start, direction={:.3f}", direction); *chassis_track_direction = direction + std::numbers::pi; @@ -474,8 +502,8 @@ class SentryClimber *chassis_climb_speed = 0.0; { const auto timed_out = co_await context.wait_align( - direction, config.align.err, config.align.w, seconds_to_duration(config.align.hold), - seconds_to_duration(config.align.timeout)); + direction + std::numbers::pi, config.align.err, config.align.w, + seconds_to_duration(config.align.hold), seconds_to_duration(config.align.timeout)); if (timed_out) { node::warn("land ALIGN failed"); release_climber(); @@ -485,6 +513,7 @@ class SentryClimber } // [] 伸出撑杆，以较快速度冲下台阶 + *chassis_climb_status = kStatusLandDash; track_group->set_state(TrackState::kHold); stick_group->set_state(StickState::kDrop); *chassis_climb_speed = -config.land.dash_vx; @@ -496,6 +525,7 @@ class SentryClimber } // [] 保持撑杆伸出，直到撑杆从台阶落下，底盘倾角低于某个阈值，趋近水平 + *chassis_climb_status = kStatusLandSettle; track_group->set_state(TrackState::kHold); stick_group->set_state(StickState::kDrop); *chassis_climb_speed = -config.land.dash_vx; @@ -512,6 +542,7 @@ class SentryClimber co_await CoSchduler::Sleep{seconds_to_duration(config.land.land_delay)}; // [] 撑杆按照速度曲线收回，减少落地震动，并缓慢前进，让履带顺着台阶落下 + *chassis_climb_status = kStatusLandSoft; track_group->set_state(TrackState::kHold); stick_group->set_state(StickState::kLand); *chassis_climb_speed = -config.land.soft_vx; @@ -535,6 +566,7 @@ class SentryClimber } // [] 完全收回撑杆，结束下台阶 + *chassis_climb_status = kStatusLandFinal; track_group->set_state(TrackState::kFree); stick_group->set_state(StickState::kRise); *chassis_climb_speed = kNaN; @@ -545,6 +577,13 @@ class SentryClimber node::warn("land FINAL stick timeout, continue"); } + // [] 撑杆收回后，以一定速度向前（驶离台阶方向）运动一段时间 + *chassis_climb_status = kStatusLandLeave; + track_group->set_state(TrackState::kHold); + stick_group->set_state(StickState::kHold); + *chassis_climb_speed = -config.land.leave_vx; + co_await CoSchduler::Sleep{seconds_to_duration(config.land.leave_duration)}; + *chassis_climb_status = 1.0; release_climber(); }