From 2b14e3f99e1d82952411597ad5bd20982c5b6c48 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Mon, 6 Jul 2026 23:48:00 +0800 Subject: [PATCH] =?UTF-8?q?7.6=E7=BB=88=E7=BB=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- elastic-layout-debug.json | 1362 +++++++++++++++++ src/main/java/frc/robot/Constants.java | 23 +- .../frc/robot/controls/DriveControls.java | 97 +- .../robot/subsystems/ConveyorSubsystem.java | 18 +- .../subsystems/IntakeRollerSubsystem.java | 20 +- .../robot/subsystems/ShooterSubsystem.java | 35 +- .../frc/robot/subsystems/Superstructure.java | 4 +- 7 files changed, 1461 insertions(+), 98 deletions(-) create mode 100644 elastic-layout-debug.json diff --git a/elastic-layout-debug.json b/elastic-layout-debug.json new file mode 100644 index 0000000..1ae265d --- /dev/null +++ b/elastic-layout-debug.json @@ -0,0 +1,1362 @@ +{ + "version": 1.0, + "grid_size": 128, + "tabs": [ + { + "name": "Match Debug", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Field", + "x": 768.0, + "y": 0.0, + "width": 768.0, + "height": 384.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Field", + "period": 0.06, + "field_game": "Reefscape", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": -180.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "Auto Chooser", + "x": 768.0, + "y": 384.0, + "width": 384.0, + "height": 128.0, + "type": "ComboBox Chooser", + "properties": { + "topic": "/SmartDashboard/Auto Chooser", + "period": 0.06, + "sort_options": false + } + }, + { + "title": "Wanted State", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Superstructure/wantedState", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "System State", + "x": 256.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Superstructure/systemState", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Time In State", + "x": 512.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Superstructure/timeInState", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Can Shoot", + "x": 0.0, + "y": 128.0, + "width": 192.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Superstructure/canShoot", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Shooter At Speed", + "x": 192.0, + "y": 128.0, + "width": 192.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Superstructure/shooterAtSpeed", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Drive Aimed", + "x": 384.0, + "y": 128.0, + "width": 192.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Superstructure/driveAimed", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Drive Stationary", + "x": 576.0, + "y": 128.0, + "width": 192.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Superstructure/driveStationary", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Heading Error", + "x": 0.0, + "y": 256.0, + "width": 256.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Superstructure/driveHeadingError", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Flywheel Offset", + "x": 256.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Superstructure/flywheelSpeedOffsetRps", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Backboard Offset", + "x": 512.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Superstructure/backboardPositionOffset", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Hub Active", + "x": 1152.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/GameData2026/HubActive", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Hub Status", + "x": 1280.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/GameData2026/HubStatus", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Match Time", + "x": 768.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/GameData2026/MatchTime", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Hub Changes In", + "x": 1024.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/GameData2026/SecondsUntilHubChanges", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Alliance", + "x": 1280.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/GameData2026/Alliance", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Mechanisms", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Flywheel Speed", + "x": 0.0, + "y": 0.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/flywheelSpeed", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Flywheel Target", + "x": 384.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/flywheelTargetSpeed", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Flywheel At Speed", + "x": 640.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shooter/flywheelAtSpeed", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Flywheel Difference", + "x": 384.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/flywheelSpeedDifference", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Shooter Conveyor Target RPS", + "x": 640.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/shooterConveyorTargetRps", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Shooter Conveyor Speed", + "x": 0.0, + "y": 256.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/conveyorSpeed", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Shooter Conveyor Current", + "x": 384.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/statorCurrent", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Backboard Position", + "x": 896.0, + "y": 0.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/backboardCurrentRate", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Backboard Target", + "x": 1280.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardTargetRate", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Backboard At Target", + "x": 1280.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shooter/isBackboardAtTarget", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Backboard PID Output", + "x": 896.0, + "y": 256.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/backboardPIDOutput", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Intake Position", + "x": 0.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/IntakeRotater/position", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Intake Target", + "x": 256.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/IntakeRotater/targetPosition", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Intake At Goal", + "x": 512.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/IntakeRotater/atGoal", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Intake Output Voltage", + "x": 640.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/IntakeRotater/outputVoltage", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Intake Roller Left", + "x": 0.0, + "y": 640.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/IntakeRoller/leftVelocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Intake Roller Right", + "x": 256.0, + "y": 640.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/IntakeRoller/rightVelocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Intake Velocity Difference", + "x": 512.0, + "y": 640.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/IntakeRoller/velocityDifference", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Intake Roller Target RPS", + "x": 0.0, + "y": 768.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/IntakeRoller/targetSpeedRps", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Main Conveyor Velocity", + "x": 896.0, + "y": 512.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Conveyor/velocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Main Conveyor Target RPS", + "x": 1280.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Conveyor/targetSpeedRps", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Aim Pose", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Field", + "x": 768.0, + "y": 0.0, + "width": 768.0, + "height": 384.0, + "type": "Field", + "properties": { + "topic": "/SmartDashboard/Field", + "period": 0.06, + "field_game": "Reefscape", + "robot_width": 0.85, + "robot_length": 0.85, + "show_other_objects": true, + "show_trajectories": true, + "field_rotation": -180.0, + "robot_color": 4294198070, + "trajectory_color": 4294967295, + "show_robot_outside_widget": true + } + }, + { + "title": "Shooter Camera", + "x": 0.0, + "y": 0.0, + "width": 384.0, + "height": 256.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/limelight-shooter", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "Intaker Camera", + "x": 384.0, + "y": 0.0, + "width": 384.0, + "height": 256.0, + "type": "Camera Stream", + "properties": { + "topic": "/CameraPublisher/limelight-intaker", + "period": 0.06, + "rotation_turns": 0 + } + }, + { + "title": "Distance To Hub", + "x": 0.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AutoControl/distanceToHub", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Angle To Hub", + "x": 256.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AutoControl/angleDifferenceToHub", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Auto Rotation Rate", + "x": 512.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AutoControl/autoRotationRate", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Distance To Pass", + "x": 0.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AutoControl/distanceToPassball", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Angle To Pass", + "x": 256.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AutoControl/angleDifferenceToPassball", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Actual VX", + "x": 0.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AutoControl/ActualVX", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Actual VY", + "x": 256.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AutoControl/ActualVY", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Actual VOmega", + "x": 512.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AutoControl/ActualVOmega", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Drive Mode", + "x": 768.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Drive/DriveMode", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Field Centric", + "x": 1024.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Drive/IsFieldCentric", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Yaw From State", + "x": 1152.0, + "y": 384.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Drive/YawFromState", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "MetaTag2 Available", + "x": 768.0, + "y": 512.0, + "width": 128.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Positioning/MetaTag2Available", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Using Limelight", + "x": 896.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Positioning/Usingll", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "Pose Distance", + "x": 768.0, + "y": 640.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Positioning/PoseDistance", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Rotation Difference", + "x": 1024.0, + "y": 640.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Positioning/RotationDifference", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "MT2 Distance Difference", + "x": 0.0, + "y": 640.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Positioning/MetaTag2DistanceDifference", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "MT2 X Difference", + "x": 384.0, + "y": 640.0, + "width": 192.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Positioning/MetaTag2XDifference", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "MT2 Y Difference", + "x": 576.0, + "y": 640.0, + "width": 192.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Positioning/MetaTag2YDifference", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + } + ] + } + }, + { + "name": "Tuning", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Drive Enable", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/Drive/Enable", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Drive kP", + "x": 128.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/Drive/kP", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Drive kI", + "x": 256.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/Drive/kI", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Drive kD", + "x": 384.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/Drive/kD", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Drive kV", + "x": 512.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/Drive/kV", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Flywheel Enable", + "x": 0.0, + "y": 192.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/flywheel/flywheelEnable", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Flywheel Run", + "x": 128.0, + "y": 192.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/flywheel/flywheelEnableRunning", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Flywheel Run RPS", + "x": 256.0, + "y": 192.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelRunningSpeedRPS", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Flywheel kP", + "x": 512.0, + "y": 192.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelkP", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Flywheel kV", + "x": 640.0, + "y": 192.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelkV", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Flywheel kI", + "x": 768.0, + "y": 192.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelkI", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Flywheel kD", + "x": 896.0, + "y": 192.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelkD", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Flywheel kA", + "x": 1024.0, + "y": 192.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelkA", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Flywheel kS", + "x": 1152.0, + "y": 192.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelkS", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Shooter Conv Enable", + "x": 0.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/conveyor/conveyorEnable", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Shooter Conv Run", + "x": 128.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/conveyor/conveyorEnableRunning", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Shooter Conv Run RPS", + "x": 256.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/conveyor/conveyorRunningSpeedRPS", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Shooter Conv kP", + "x": 512.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/conveyor/conveyorkP", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Shooter Conv kV", + "x": 640.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/conveyor/conveyorkV", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Shooter Conv kI", + "x": 768.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/conveyor/conveyorkI", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Shooter Conv kD", + "x": 896.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/conveyor/conveyorkD", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Shooter Conv kA", + "x": 1024.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/conveyor/conveyorkA", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Shooter Conv kS", + "x": 1152.0, + "y": 384.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/conveyor/conveyorkS", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Backboard Enable", + "x": 0.0, + "y": 576.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/backboard/backboardEnable", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Backboard Run", + "x": 128.0, + "y": 576.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/backboard/backboardEnableRunning", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Backboard Run RPS", + "x": 256.0, + "y": 576.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/backboard/backboardRunningSpeedRPS", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Backboard kP", + "x": 512.0, + "y": 576.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/backboard/backboardkP", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Backboard kV", + "x": 640.0, + "y": 576.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/backboard/backboardkV", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Backboard kS", + "x": 768.0, + "y": 576.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/backboard/backboardkS", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Backboard kI", + "x": 896.0, + "y": 576.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/backboard/backboardkI", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Backboard kD", + "x": 1024.0, + "y": 576.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/backboard/backboardkD", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Backboard kA", + "x": 1152.0, + "y": 576.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/backboard/backboardkA", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + } + } + ] +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d2acd0e..c819a48 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -18,10 +18,6 @@ public final class Constants { public static final int rightConveyorID = 45; public static final int backboardMotorID = 44; - public static final double flywheelVoltsPerRps = 0.12; - public static final double flywheelIdleVoltage = 0.0; - public static final double shooterConveyorVoltage = 6.0; - public static final double shooterConveyorReverseVoltage = -4.0; public static final double flywheelSpeedToleranceRps = 5.0; public static final double defaultFlywheelTargetRps = 64.0; @@ -121,8 +117,8 @@ public final class Constants { public static final int leftIntakeRotaterID = 48; public static final int rightIntakeRotaterID = 49; - public static final double intakeRollerVoltage = 6.0; - public static final double outtakeRollerVoltage = -5.0; + public static final double intakeRollerIntakeSpeedRps = 30.0; + public static final double intakeRollerOuttakeSpeedRps = -30.0; public static final double intakeRotaterLoweredPosition = -7.6; public static final double intakeRotaterShootAssistUpPosition = -2.5; @@ -134,9 +130,10 @@ public final class Constants { public static final double intakeRotaterUpLimit = 0.0; public static final double intakeRotaterDownLimit = -7.6; - public static final Slot0Configs intakeGetterSlot0Configs = new Slot0Configs() + public static final Slot0Configs intakeRollerSlot0Configs = new Slot0Configs() .withKP(0.00).withKI(0.00).withKD(0.00) .withKV(0.00).withKA(0.00).withKS(0.00); + public static final Slot0Configs intakeGetterSlot0Configs = intakeRollerSlot0Configs; public static final Slot0Configs intakeRotaterSlot0Configs = new Slot0Configs() .withKP(0.00).withKI(0.00).withKD(0.00) @@ -149,14 +146,18 @@ public final class Constants { public static class Conveyor { public static final int mainConveyorID = 50; - public static final double feedVoltage = 6.0; - public static final double reverseVoltage = -5.0; + public static final double feedSpeedRps = 30.0; + public static final double reverseSpeedRps = -30.0; + + public static final Slot0Configs mainConveyorSlot0Configs = new Slot0Configs() + .withKP(0.00).withKI(0.00).withKD(0.00) + .withKV(0.00).withKA(0.00).withKS(0.00); } public static class Superstructure { // 状态机时间和门控容差。真车基础动作验证后，需要现场调这些值。 - public static final double shooterFeedPercent = 1.0; - public static final double shooterReversePercent = -0.6; + public static final double shooterFeedSpeedRps = Shooter.conveyorSpeed; + public static final double shooterReverseSpeedRps = -Shooter.conveyorSpeed * 0.6; public static final double shootTimeoutSeconds = 1.0; public static final double aimToleranceDegrees = 1.5; public static final double stationarySpeedToleranceMetersPerSecond = 0.5; diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index 44805a0..0cbac6c 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -36,23 +36,23 @@ public class DriveControls { private final double maxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); private final double maxAngularRate = RotationsPerSecond.of(0.75).in(edu.wpi.first.units.Units.RadiansPerSecond); - // 榛樿閫熷害姣斾緥锛堝钩鏃跺紑杞︼級 + // 默认速度比例，普通驾驶使用。 private double normalDriveScale = 0.3; private double normalTurnScale = 0.7; - // 鍔犻€熸ā寮忔瘮渚嬶紙鎸変綇 RB锛? + // 加速模式比例，按住 RB 时使用。 private double boostDriveScale = 1.0; private double boostTurnScale = 1.0; - //autoaim妯″紡 + // 自动瞄准时降低平移速度。 private double slowDriveScale =0.1; private boolean boostEnabled = false; - private final SlewRateLimiter vxLimiter = new SlewRateLimiter(5.0); // m/s^2 绛夋晥锛堣皟锛? + private final SlewRateLimiter vxLimiter = new SlewRateLimiter(5.0); // m/s^2 等效加速度限制 private final SlewRateLimiter vyLimiter = new SlewRateLimiter(5.0); - private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 绛夋晥锛堣皟锛? + private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 等效角加速度限制 - // ====== 绫绘垚鍛樺彉閲忛噷鏂板锛氳褰曚笂涓€娆¤緭鍑?====== + // 记录上一周期输出，用于只限制加速、不限制减速。 private double lastVx = 0.0; private double lastVy = 0.0; private double lastOmega = 0.0; @@ -64,7 +64,7 @@ public class DriveControls { boostEnabled = enabled; } - // 鏂板锛氬綋鍓嶆槸鍚︿娇鐢ㄥ満鍦板潗鏍囩郴锛堥粯璁?true锛? + // 当前是否使用场地坐标系，默认启用。 private boolean fieldCentricEnabled = true; private final SwerveRequest.FieldCentric fieldCentric = @@ -101,7 +101,7 @@ public class DriveControls { return fieldCentricEnabled; } - /** 搴曣洏鐨勯粯璁ら┚椹跺懡浠わ紙teleop 鏈熼棿鎸佺画鎵ц锛?*/ + /** 底盘默认驾驶命令，在 teleop 期间持续执行。 */ public Command defaultDriveCommand() { return drivetrain.applyRequest(() -> { WantedState wantedState = superstructure.getWantedState(); @@ -168,27 +168,24 @@ public class DriveControls { } }, () -> fieldCentricEnabled ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC); } - /** - * - * 杈撳嚭绉诲姩鍒拌窛绂讳袱涓洰鏍囦箣闂磋緝杩戠殑閭ｄ釜鐐圭殑浣嶇Щ - * @return 浣嶇Щ 鏈夋璐? - */ + // 返回当前位置到两个目标点中更近目标的位移。 + // 返回到更近目标的带符号位移。 public double calculateDifferenceToTwoTarget(double current, double target1, double target2){ double x1 = target1 - current; double x2 = target2 - current; return Math.abs(x1) < Math.abs(x2) ? x1 : x2; } public double[] calculateDistanceAndRotationToHub() { - Pose2d currentPose = drivetrain.getState().Pose; // 鑾峰彇鏈哄櫒浜哄綋鍓嶇殑浣嶇疆鍜岃搴? + Pose2d currentPose = drivetrain.getState().Pose; // 当前机器人位姿 double targetX = isRedAlliance ? Constants.Field.RedHubPositionX : Constants.Field.BlueHubPositionX; double targetY = isRedAlliance ? Constants.Field.RedHubPositionY : Constants.Field.BlueHubPositionY; - // 璁＄畻鐩爣瑙掑害锛堢浉瀵逛簬鍦哄湴鍧愭爣绯伙級 + // 计算目标角度，相对于场地坐标系。 double deltaX = targetX - currentPose.getX(); double deltaY = targetY - currentPose.getY(); - double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 杞崲涓哄害 + double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为角度 - // 褰撳墠鏈哄櫒浜鸿搴? + // 当前机器人朝向。 double currentAngle = currentPose.getRotation().getDegrees(); double angleDifference = targetAngle - currentAngle; // Normalize angle difference to the range [-180, 180] @@ -198,26 +195,26 @@ public class DriveControls { angleDifference += 360; } - // 璁＄畻璺濈 + // 计算距离。 double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY); - // 璁＄畻璺濈鍜屾棆杞搴? + // 返回距离和旋转角度误差。 double[] distanceAndRotation = new double[]{distance, angleDifference}; - // 杩斿洖璺濈鍜岃搴﹀樊 + // 距离单位为米，角度单位为度。 return distanceAndRotation; } public double[] calculateDistanceAndRotationToPassBall() { - Pose2d currentPose = drivetrain.getState().Pose; // 鑾峰彇鏈哄櫒浜哄綋鍓嶇殑浣嶇疆鍜岃搴? - double targetX = isRedAlliance ? Constants.Field.RedPassingBallPosX : Constants.Field.BluePassingBallPosX; // 鐩爣 X 鍧愭爣 - double targetY1 = Constants.Field.PassingBallPosY1; // 鐩爣 Y 鍧愭爣 - double targetY2 = Constants.Field.PassingBallPosY2; // 鐩爣 Y 鍧愭爣 + Pose2d currentPose = drivetrain.getState().Pose; // 当前机器人位姿 + double targetX = isRedAlliance ? Constants.Field.RedPassingBallPosX : Constants.Field.BluePassingBallPosX; // 传球目标 X 坐标 + double targetY1 = Constants.Field.PassingBallPosY1; // 传球目标 Y 坐标 + double targetY2 = Constants.Field.PassingBallPosY2; // 传球目标 Y 坐标 - // 璁＄畻鐩爣瑙掑害锛堢浉瀵逛簬鍦哄湴鍧愭爣绯伙級 + // 计算目标角度，相对于场地坐标系。 double deltaX = targetX - currentPose.getX(); double deltaY = calculateDifferenceToTwoTarget(currentPose.getY(), targetY1, targetY2); - double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 杞崲涓哄害 + double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为角度 - // 褰撳墠鏈哄櫒浜鸿搴? + // 当前机器人朝向。 double currentAngle = currentPose.getRotation().getDegrees(); double angleDifference = targetAngle - currentAngle; // Normalize angle difference to the range [-180, 180] @@ -227,12 +224,12 @@ public class DriveControls { angleDifference += 360; } - // 璁＄畻璺濈 + // 计算距离。 double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY); - // 璁＄畻璺濈鍜屾棆杞搴? + // 返回距离和旋转角度误差。 double[] distanceAndRotation = new double[]{distance, angleDifference}; - // 杩斿洖璺濈鍜岃搴﹀樊 + // 距离单位为米，角度单位为度。 return distanceAndRotation; } @@ -260,26 +257,26 @@ public class DriveControls { } else { vomega = error * Constants.AutoPositioning.TurningkP; } - // 闄愬埗鏈€澶ц閫熷害 + // 限制最大角速度。 vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega)); return vomega; } - /** - * 鍙檺鍒垛€滃姞閫熲€濓紙骞呭€煎彉澶э級锛屼笉闄愬埗鈥滃噺閫熲€濓紙骞呭€煎彉灏忥級 - * 杩欐牱鏉炬潌/鍒硅溅浼氭洿鍙婃椂锛屽悓鏃?boost 浠嶇劧鑳芥姂鍒剁數娴佸啿鍑? + /** + * 只限制加速，也就是输出幅值变大时；不限制减速。 + * 这样松杆或刹车能更及时，同时 boost 仍能抑制电流冲击。 */ private double limitAccelOnly(double target, double last, SlewRateLimiter limiter) { - // 濡傛灉姝ｅ湪鍑忛€燂紙骞呭€煎彉灏忥級锛岀洿鎺ユ斁琛屽埌鐩爣鍊? + // 如果正在减速，直接放行到目标值。 if (Math.abs(target) < Math.abs(last)) { - limiter.reset(target); // 鍚屾 limiter 鍐呴儴鐘舵€侊紝闃叉涓嬩竴娆＄獊鍙? - return target; + limiter.reset(target); // 同步 limiter 内部状态，防止下一次突跳。 + return target; } - // 鍚﹀垯鏄姞閫燂紝浣跨敤 limiter + // 否则是加速，使用 limiter。 return limiter.calculate(target); } - /** 绱ф€ュ仠姝細娓呴浂闄愰€熷櫒锛岃閫熷害杈撳嚭绔嬪埢褰掗浂 */ + /** 紧急停止：清零限速器，让速度输出立刻归零。 */ public void emergencyStop() { vxLimiter.reset(0.0); vyLimiter.reset(0.0); @@ -317,13 +314,13 @@ public class DriveControls { return maxAngularRate; } public void setTeamColors() { - Optional ally = DriverStation.getAlliance(); // 鑾峰彇闃熶紞棰滆壊 + Optional ally = DriverStation.getAlliance(); // 获取联盟颜色 if (ally.isPresent()) { - // 鏍规嵁闃熶紞棰滆壊璁剧疆甯冨皵鍊? + // 根据联盟颜色更新自动瞄准目标。 if (ally.get() == Alliance.Red) { - isRedAlliance = true; // 绾㈤槦 + isRedAlliance = true; // 红队 } else if (ally.get() == Alliance.Blue) { - isRedAlliance = false; // 钃濋槦 + isRedAlliance = false; // 蓝队 } } else { isRedAlliance = true; @@ -369,20 +366,20 @@ public class DriveControls { switch (phase) { case 0: - vx = amplitude; // 鍓? + vx = amplitude; // 前 vy = 0.0; break; case 1: vx = 0.0; - vy = amplitude; // 鍙? + vy = amplitude; // 左 break; case 2: - vx = -amplitude; // 鍚? + vx = -amplitude; // 后 vy = 0.0; break; case 3: vx = 0.0; - vy = -amplitude; // 宸? + vy = -amplitude; // 右 break; } @@ -404,11 +401,11 @@ public class DriveControls { public void pushDistanceData(){ double[] distanceAndRotation = calculateDistanceAndRotationToHub(); - // 灏嗚窛绂诲€煎啓鍏?NetworkTable + // 将距离数据写入 NetworkTable。 autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]); autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]); - // 鑷姩闃舵榛樿椹鹃┒鍛戒护涓嶈繍琛岋紝杩欓噷鍚屾牱淇濇寔鐩爣鏈濆悜鏈€鏂帮紝 - // 璁╄嚜鍔ㄧ▼搴忛噷鐨勫皠鍑婚棬鎺?isAimed() 鏈夋晥銆? + // 自动阶段默认驾驶命令不运行，这里同样保持目标朝向最新， + // 让自动程序里的射击门控 isAimed() 有效。 drivetrain.setTargetHeading( drivetrain.getRotation().plus(Rotation2d.fromDegrees(distanceAndRotation[1]))); } diff --git a/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java b/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java index efef84f..7e86379 100644 --- a/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java @@ -1,6 +1,6 @@ package frc.robot.subsystems; -import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.networktables.NetworkTable; @@ -13,27 +13,31 @@ import frc.robot.Constants; */ public class ConveyorSubsystem extends SubsystemBase { public final TalonFX mainConveyor; - private final VoltageOut voltageRequest = new VoltageOut(0); + private final VelocityVoltage velocityRequest = new VelocityVoltage(0); private final NetworkTable table = NetworkTableInstance.getDefault().getTable("Conveyor"); + private double targetSpeedRps = 0.0; public ConveyorSubsystem() { mainConveyor = new TalonFX(Constants.Conveyor.mainConveyorID); + mainConveyor.getConfigurator().apply(Constants.Conveyor.mainConveyorSlot0Configs); } public void feedToShooter() { - setVoltage(Constants.Conveyor.feedVoltage); + setSpeedByRPS(Constants.Conveyor.feedSpeedRps); } public void reverse() { - setVoltage(Constants.Conveyor.reverseVoltage); + setSpeedByRPS(Constants.Conveyor.reverseSpeedRps); } public void stop() { - setVoltage(0.0); + setSpeedByRPS(0.0); } - public void setVoltage(double volts) { - mainConveyor.setControl(voltageRequest.withOutput(volts)); + public void setSpeedByRPS(double targetSpeedRps) { + this.targetSpeedRps = targetSpeedRps; + mainConveyor.setControl(velocityRequest.withVelocity(targetSpeedRps)); } @Override public void periodic() { table.getEntry("velocity").setDouble(mainConveyor.getVelocity().getValueAsDouble()); + table.getEntry("targetSpeedRps").setDouble(targetSpeedRps); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeRollerSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeRollerSubsystem.java index e28a997..87dc227 100644 --- a/src/main/java/frc/robot/subsystems/IntakeRollerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeRollerSubsystem.java @@ -1,11 +1,10 @@ package frc.robot.subsystems; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.MotorAlignmentValue; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -21,31 +20,35 @@ public class IntakeRollerSubsystem extends SubsystemBase { private final TalonFX leftIntakeRoller; private final TalonFX rightIntakeRoller; - private final VoltageOut voltageRequest = new VoltageOut(0); + private final VelocityVoltage velocityRequest = new VelocityVoltage(0); private final Follower rightFollower; private final NetworkTable table = NetworkTableInstance.getDefault().getTable("IntakeRoller"); + private double targetSpeedRps = 0.0; public IntakeRollerSubsystem() { leftIntakeRoller = new TalonFX(Constants.Intaker.leftIntakeRollerID); rightIntakeRoller = new TalonFX(Constants.Intaker.rightIntakeRollerID); + leftIntakeRoller.getConfigurator().apply(Constants.Intaker.intakeRollerSlot0Configs); + rightIntakeRoller.getConfigurator().apply(Constants.Intaker.intakeRollerSlot0Configs); rightFollower = new Follower(leftIntakeRoller.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50); followLeft(); } public void intake() { - setVoltage(Constants.Intaker.intakeRollerVoltage); + setSpeedByRPS(Constants.Intaker.intakeRollerIntakeSpeedRps); } public void outtake() { - setVoltage(Constants.Intaker.outtakeRollerVoltage); + setSpeedByRPS(Constants.Intaker.intakeRollerOuttakeSpeedRps); } public void stop() { - setVoltage(0); + setSpeedByRPS(0.0); } - public void setVoltage(double left) { - leftIntakeRoller.setControl(voltageRequest.withOutput(left)); + public void setSpeedByRPS(double targetSpeedRps) { + this.targetSpeedRps = targetSpeedRps; + leftIntakeRoller.setControl(velocityRequest.withVelocity(targetSpeedRps)); followLeft(); } @@ -64,5 +67,6 @@ public class IntakeRollerSubsystem extends SubsystemBase { table.getEntry("leftVelocity").setDouble(leftIntakeRoller.getVelocity().getValueAsDouble()); table.getEntry("rightVelocity").setDouble(rightIntakeRoller.getVelocity().getValueAsDouble()); table.getEntry("velocityDifference").setDouble(getVelocityDifference()); + table.getEntry("targetSpeedRps").setDouble(targetSpeedRps); } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 3d3ba0c..5db3aa0 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -4,7 +4,6 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFXS; import com.ctre.phoenix6.signals.InvertedValue; @@ -39,8 +38,8 @@ public class ShooterSubsystem extends SubsystemBase { public final Encoder backboardEncoder; public final ProfiledPIDController backboardPID; - private final VoltageOut flywheelVoltageRequest = new VoltageOut(0); - private final VoltageOut conveyorVoltageRequest = new VoltageOut(0); + private final VelocityVoltage flywheelVelocityRequest = new VelocityVoltage(0); + private final VelocityVoltage conveyorVelocityRequest = new VelocityVoltage(0); private final VelocityVoltage backboardVelocityRequest = new VelocityVoltage(0); private final Follower flywheelFollower; @@ -49,7 +48,7 @@ public class ShooterSubsystem extends SubsystemBase { private final NetworkTable shooterNetworkTable; private double targetFlywheelRps = 0.0; - private double shooterConveyorPercent = 0.0; + private double shooterConveyorTargetRps = 0.0; public ShooterSubsystem() { leftFlywheel = new TalonFX(Constants.Shooter.leftFlywheelID); @@ -108,11 +107,13 @@ public class ShooterSubsystem extends SubsystemBase { public void setFlywheelVelocity(double targetRps) { // 右飞轮是 follower，因此对外只暴露一个飞轮目标速度。 - // 实际控制仍是左飞轮电压开环，右飞轮反向跟随左飞轮。 + // 左飞轮使用 VelocityVoltage 速度闭环，右飞轮反向跟随左飞轮。 + setFlywheelSpeedByRPS(targetRps); + } + + public void setFlywheelSpeedByRPS(double targetRps) { targetFlywheelRps = Math.abs(targetRps); - double requestedVoltage = targetFlywheelRps * Constants.Shooter.flywheelVoltsPerRps; - requestedVoltage = MathUtil.clamp(requestedVoltage, -12.0, 12.0); - leftFlywheel.setControl(flywheelVoltageRequest.withOutput(requestedVoltage)); + leftFlywheel.setControl(flywheelVelocityRequest.withVelocity(targetFlywheelRps)); setRightFollowLeft(); } @@ -122,14 +123,8 @@ public class ShooterSubsystem extends SubsystemBase { setFlywheelVelocity(targetRps); } - public void setFlywheelVoltage(double volts) { - targetFlywheelRps = 0.0; - leftFlywheel.setControl(flywheelVoltageRequest.withOutput(MathUtil.clamp(volts, -12.0, 12.0))); - setRightFollowLeft(); - } - public void stopFlywheel() { - setFlywheelVoltage(Constants.Shooter.flywheelIdleVoltage); + setFlywheelSpeedByRPS(0.0); } public boolean atSpeed() { @@ -165,14 +160,14 @@ public class ShooterSubsystem extends SubsystemBase { /* shooter conveyor */ /* ====================== */ - public void runShooterConveyor(double percent) { - shooterConveyorPercent = MathUtil.clamp(percent, -1.0, 1.0); - leftConveyor.setControl(conveyorVoltageRequest.withOutput(shooterConveyorPercent * 12.0)); + public void runShooterConveyorByRPS(double targetRps) { + shooterConveyorTargetRps = targetRps; + leftConveyor.setControl(conveyorVelocityRequest.withVelocity(targetRps)); setRightFollowLeft(); } public void stopShooterConveyor() { - runShooterConveyor(0.0); + runShooterConveyorByRPS(0.0); } public double getConveyorStatorCurrent() { @@ -273,7 +268,7 @@ public class ShooterSubsystem extends SubsystemBase { shooterNetworkTable.getEntry("flywheelSpeedDifference").setDouble(getFlywheelVelocityDifference()); shooterNetworkTable.getEntry("flywheelTargetSpeed").setDouble(targetFlywheelRps); shooterNetworkTable.getEntry("flywheelAtSpeed").setBoolean(atSpeed()); - shooterNetworkTable.getEntry("shooterConveyorPercent").setDouble(shooterConveyorPercent); + shooterNetworkTable.getEntry("shooterConveyorTargetRps").setDouble(shooterConveyorTargetRps); shooterNetworkTable.getEntry("conveyorSpeed").setDouble(leftConveyor.getVelocity().getValueAsDouble()); shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition()); shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 59496b8..8660463 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -282,7 +282,7 @@ public class Superstructure extends SubsystemBase { // 避免球接触飞轮导致的掉速中断喂球。 intakeRotater.shootAssist(); conveyor.feedToShooter(); - shooter.runShooterConveyor(Constants.Superstructure.shooterFeedPercent); + shooter.runShooterConveyorByRPS(Constants.Superstructure.shooterFeedSpeedRps); if (timeInState() > Constants.Superstructure.shootTimeoutSeconds) { shotCompletedThisRequest = true; @@ -306,7 +306,7 @@ public class Superstructure extends SubsystemBase { setSystemState(SystemState.EJECTING); intakeRoller.outtake(); conveyor.reverse(); - shooter.runShooterConveyor(Constants.Superstructure.shooterReversePercent); + shooter.runShooterConveyorByRPS(Constants.Superstructure.shooterReverseSpeedRps); shooter.stopFlywheel(); shooter.holdBackboardAt(0.0); }