From 915b1b1af0b0ad109787cf0a26457c8a8d4076e2 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Thu, 9 Jul 2026 20:12:25 +0800 Subject: [PATCH] =?UTF-8?q?7.9=E6=99=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- elastic-layout-debug.json | 908 +++++++++++++++++- src/main/java/frc/robot/Constants.java | 49 +- src/main/java/frc/robot/RobotContainer.java | 7 +- .../frc/robot/controls/DriveControls.java | 6 +- .../frc/robot/controls/OperatorControls.java | 4 + .../subsystems/IntakeRotaterSubsystem.java | 20 +- .../robot/subsystems/LimelightSubsystem.java | 4 + .../robot/subsystems/ShooterSubsystem.java | 206 +++- .../frc/robot/subsystems/Superstructure.java | 113 ++- .../robot/tuning/ConfigTalonFXMotorTuner.java | 7 +- 10 files changed, 1193 insertions(+), 131 deletions(-) diff --git a/elastic-layout-debug.json b/elastic-layout-debug.json index cff8a4d..ecd3264 100644 --- a/elastic-layout-debug.json +++ b/elastic-layout-debug.json @@ -1821,28 +1821,41 @@ } }, { - "title": "Goal Position", + "title": "Position Enable", "x": 1024.0, "y": 0.0, "width": 256.0, "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/backboard/backboardPositionEnable", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Target Position", + "x": 1280.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shooter/backboardGoalPosition", + "topic": "/Tuning/backboard/backboardTargetPosition", "period": 0.06, "data_type": "double", - "show_submit_button": false + "show_submit_button": true } }, { - "title": "At Target", - "x": 1280.0, - "y": 0.0, + "title": "Position Active", + "x": 1024.0, + "y": 128.0, "width": 256.0, "height": 128.0, "type": "Boolean Box", "properties": { - "topic": "/Shooter/isBackboardAtTarget", + "topic": "/Tuning/backboard/backboardPositionEnableActive", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -1852,36 +1865,19 @@ } }, { - "title": "Position Offset", - "x": 1024.0, + "title": "Clamped Target", + "x": 1280.0, "y": 128.0, "width": 256.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Superstructure/backboardPositionOffset", + "topic": "/Tuning/backboard/backboardClampedTargetPosition", "period": 0.06, "data_type": "double", "show_submit_button": false } }, - { - "title": "Can Shoot", - "x": 1280.0, - "y": 128.0, - "width": 256.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": "Goal Error", "x": 0.0, @@ -2281,43 +2277,50 @@ "data_type": "double", "show_submit_button": false } - }, + } + ] + } + }, + { + "name": "Flywheel Velocity Tuning", + "grid_layout": { + "layouts": [], + "containers": [ { - "title": "Position Enable", + "title": "Apply Gains", "x": 0.0, - "y": 896.0, - "width": 256.0, + "y": 0.0, + "width": 128.0, "height": 128.0, "type": "Toggle Switch", "properties": { - "topic": "/Tuning/backboard/backboardPositionEnable", + "topic": "/Tuning/flywheel/flywheelEnable", "period": 0.06, "data_type": "boolean" } }, { - "title": "Target Position", - "x": 256.0, - "y": 896.0, - "width": 256.0, + "title": "Velocity Test", + "x": 128.0, + "y": 0.0, + "width": 128.0, "height": 128.0, - "type": "Text Display", + "type": "Toggle Switch", "properties": { - "topic": "/Tuning/backboard/backboardTargetPosition", + "topic": "/Tuning/flywheel/flywheelVelocityEnable", "period": 0.06, - "data_type": "double", - "show_submit_button": true + "data_type": "boolean" } }, { - "title": "Position Active", - "x": 512.0, - "y": 896.0, - "width": 256.0, + "title": "Test Active", + "x": 256.0, + "y": 0.0, + "width": 192.0, "height": 128.0, "type": "Boolean Box", "properties": { - "topic": "/Tuning/backboard/backboardPositionEnableActive", + "topic": "/Tuning/flywheel/flywheelVelocityEnableActive", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -2327,14 +2330,76 @@ } }, { - "title": "Commanded Target", - "x": 768.0, - "y": 896.0, + "title": "At Velocity", + "x": 448.0, + "y": 0.0, + "width": 192.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shooter/flywheelAtVelocity", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Opposed Follower", + "x": 640.0, + "y": 0.0, + "width": 192.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shooter/flywheelFollowerOpposed", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Target Velocity", + "x": 0.0, + "y": 128.0, "width": 256.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Tuning/backboard/backboardCommandedTargetPosition", + "topic": "/Tuning/flywheel/flywheelTargetVelocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Tolerance", + "x": 256.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelVelocityTolerance", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Commanded Target", + "x": 512.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelCommandedTargetVelocity", "period": 0.06, "data_type": "double", "show_submit_button": false @@ -2342,17 +2407,756 @@ }, { "title": "Clamped Target", - "x": 1024.0, - "y": 896.0, + "x": 768.0, + "y": 128.0, "width": 256.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Tuning/backboard/backboardClampedTargetPosition", + "topic": "/Tuning/flywheel/flywheelClampedTargetVelocity", "period": 0.06, "data_type": "double", "show_submit_button": false } + }, + { + "title": "Active Tolerance", + "x": 1024.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/flywheel/flywheelVelocityToleranceActive", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kP", + "x": 0.0, + "y": 320.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": "kI", + "x": 128.0, + "y": 320.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": "kD", + "x": 256.0, + "y": 320.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": "kV", + "x": 384.0, + "y": 320.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": "kA", + "x": 512.0, + "y": 320.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": "kS", + "x": 640.0, + "y": 320.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": "Target Velocity Now", + "x": 768.0, + "y": 320.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/flywheelTargetVelocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Velocity Error", + "x": 1024.0, + "y": 320.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/flywheelVelocityError", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Average Velocity", + "x": 0.0, + "y": 512.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/flywheelVelocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 8.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Target Velocity", + "x": 384.0, + "y": 512.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/flywheelTargetVelocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 8.0, + "color": 4283443184, + "line_width": 2.0 + } + }, + { + "title": "Velocity Error", + "x": 768.0, + "y": 512.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/flywheelVelocityError", + "period": 0.033, + "data_type": "double", + "time_displayed": 8.0, + "color": 4294198070, + "line_width": 2.0 + } + }, + { + "title": "Output Volts", + "x": 1152.0, + "y": 512.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/flywheelOutputVolts", + "period": 0.033, + "data_type": "double", + "time_displayed": 8.0, + "color": 4289374890, + "line_width": 2.0 + } + }, + { + "title": "Left Velocity", + "x": 0.0, + "y": 768.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/leftFlywheelVelocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 8.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Right Velocity", + "x": 384.0, + "y": 768.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/rightFlywheelVelocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 8.0, + "color": 4283443184, + "line_width": 2.0 + } + }, + { + "title": "Velocity Difference", + "x": 768.0, + "y": 768.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/flywheelVelocityDifference", + "period": 0.033, + "data_type": "double", + "time_displayed": 8.0, + "color": 4294198070, + "line_width": 2.0 + } + }, + { + "title": "Left Current", + "x": 1152.0, + "y": 768.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/leftFlywheelStatorCurrent", + "period": 0.033, + "data_type": "double", + "time_displayed": 8.0, + "color": 4289374890, + "line_width": 2.0 + } + }, + { + "title": "Right Current", + "x": 1152.0, + "y": 1024.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/rightFlywheelStatorCurrent", + "period": 0.033, + "data_type": "double", + "time_displayed": 8.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Target Volts Estimate", + "x": 0.0, + "y": 1024.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/flywheelTargetVoltage", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Left Output Volts", + "x": 256.0, + "y": 1024.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/leftFlywheelOutputVolts", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Right Output Volts", + "x": 512.0, + "y": 1024.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/rightFlywheelOutputVolts", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Left Output Volts Graph", + "x": 0.0, + "y": 1152.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/leftFlywheelOutputVolts", + "period": 0.033, + "data_type": "double", + "time_displayed": 8.0, + "color": 4283443184, + "line_width": 2.0 + } + }, + { + "title": "Right Output Volts Graph", + "x": 384.0, + "y": 1152.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/rightFlywheelOutputVolts", + "period": 0.033, + "data_type": "double", + "time_displayed": 8.0, + "color": 4289374890, + "line_width": 2.0 + } + }, + { + "title": "Velocity Offset", + "x": 768.0, + "y": 1024.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Superstructure/flywheelVelocityOffset", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Can Shoot", + "x": 1024.0, + "y": 1024.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 Ready", + "x": 1216.0, + "y": 1024.0, + "width": 192.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Superstructure/shooterAtVelocity", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + } + ] + } + }, + { + "name": "Shooter Conveyor Velocity Tuning", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Apply Gains", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/conveyor/conveyorEnable", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Velocity Test", + "x": 128.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/conveyor/conveyorVelocityEnable", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Test Active", + "x": 256.0, + "y": 0.0, + "width": 192.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Tuning/conveyor/conveyorVelocityEnableActive", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Subsystem Active", + "x": 448.0, + "y": 0.0, + "width": 192.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shooter/conveyorVelocityTuningActive", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Superstructure Holdoff", + "x": 640.0, + "y": 0.0, + "width": 192.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Superstructure/shooterConveyorTuningControlActive", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Target Velocity", + "x": 0.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/conveyor/conveyorTargetVelocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Commanded Target", + "x": 256.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/conveyor/conveyorCommandedTargetVelocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Clamped Target", + "x": 512.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/conveyor/conveyorClampedTargetVelocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Target Velocity Now", + "x": 768.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/conveyorTargetVelocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Velocity Error", + "x": 1024.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/conveyorVelocityError", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "kP", + "x": 0.0, + "y": 320.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": "kI", + "x": 128.0, + "y": 320.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": "kD", + "x": 256.0, + "y": 320.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": "kV", + "x": 384.0, + "y": 320.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": "kA", + "x": 512.0, + "y": 320.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": "kS", + "x": 640.0, + "y": 320.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": "Output Volts", + "x": 768.0, + "y": 320.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/shooterConveyorOutputVolts", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Stator Current", + "x": 1024.0, + "y": 320.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": "Velocity", + "x": 0.0, + "y": 512.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/conveyorVelocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 8.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Target Velocity", + "x": 384.0, + "y": 512.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/conveyorTargetVelocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 8.0, + "color": 4283443184, + "line_width": 2.0 + } + }, + { + "title": "Velocity Error", + "x": 768.0, + "y": 512.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/conveyorVelocityError", + "period": 0.033, + "data_type": "double", + "time_displayed": 8.0, + "color": 4294198070, + "line_width": 2.0 + } + }, + { + "title": "Output Volts", + "x": 1152.0, + "y": 512.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/shooterConveyorOutputVolts", + "period": 0.033, + "data_type": "double", + "time_displayed": 8.0, + "color": 4289374890, + "line_width": 2.0 + } + }, + { + "title": "Stator Current", + "x": 0.0, + "y": 768.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/statorCurrent", + "period": 0.033, + "data_type": "double", + "time_displayed": 8.0, + "color": 4278238420, + "line_width": 2.0 + } } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 542ae43..56f71f8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -18,30 +18,52 @@ public final class Constants { public static final int rightConveyorID = 37; public static final int backboardMotorID = 44; + public static final double flywheelNominalKv = 0.12; public static final double defaultFlywheelVoltage = 3.34; - public static final double flywheelReadyVoltageThreshold = 1.0; + public static final double defaultFlywheelVelocity = defaultFlywheelVoltage / flywheelNominalKv; + public static final double flywheelMaxVelocity = 120.0; + public static final double flywheelReadyVelocityThreshold = 5.0; + public static final double flywheelReadyVelocityTolerance = 2.0; public static final double flywheelReadyDelaySeconds = 0.5; // 距离(米) -> 飞轮电压(V) 射表。当前为占位点； // 真车试射后按实测数据加点即可，查表自动线性插值并在两端取边界值。 public static final InterpolatingDoubleTreeMap flywheelVoltageByDistance = new InterpolatingDoubleTreeMap(); static { - flywheelVoltageByDistance.put(1.2, 5.82); - flywheelVoltageByDistance.put(2.5, 6.34); - flywheelVoltageByDistance.put(3.5, 6.73); - flywheelVoltageByDistance.put(5.0, 7.33); + flywheelVoltageByDistance.put(1.2, 2.0); + flywheelVoltageByDistance.put(2.5, 2.1); + flywheelVoltageByDistance.put(3.5, 2.2); + flywheelVoltageByDistance.put(5.0, 2.3); + } + + public static final InterpolatingDoubleTreeMap flywheelVelocityByDistance = new InterpolatingDoubleTreeMap(); + static { + flywheelVelocityByDistance.put(1.2, 2.0 / flywheelNominalKv); + flywheelVelocityByDistance.put(2.5, 2.1 / flywheelNominalKv); + flywheelVelocityByDistance.put(3.5, 2.2 / flywheelNominalKv); + flywheelVelocityByDistance.put(5.0, 2.3 / flywheelNominalKv); } public static final double shooterConveyorFeedVoltage = 3.0; public static final double shooterConveyorReverseVoltage = -2.0; + public static final double shooterConveyorNominalKv = 0.125; + public static final double shooterConveyorMaxVelocity = 12.0 / shooterConveyorNominalKv; + public static final double shooterConveyorFeedVelocity = + shooterConveyorFeedVoltage / shooterConveyorNominalKv; + public static final double shooterConveyorReverseVelocity = + shooterConveyorReverseVoltage / shooterConveyorNominalKv; public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs() - .withKP(0.0).withKI(0).withKD(0) - .withKV(0.099).withKA(0.0).withKS(0.0); + .withKP(0.21) + .withKI(0) + .withKD(0.004) + .withKS(0.25) + .withKA(0.0) + .withKV(flywheelNominalKv); public static final Slot0Configs conveyorSlot0Configs = new Slot0Configs() - .withKP(0.0).withKI(0).withKD(0) - .withKV(0.16).withKA(0.15).withKS(0); + .withKP(0.19).withKI(0).withKD(0.001) + .withKV(shooterConveyorNominalKv).withKA(0.0).withKS(0.07); public static final Slot0Configs backboardSlot0Configs = new Slot0Configs() .withKP(0.01).withKI(0).withKD(0) @@ -65,7 +87,7 @@ public final class Constants { } public static class AutoPositioning { - public static final double autoRotationkP = 0.08; + public static final double autoRotationkP = 0.07; public static final double autoRotationForBumpTargetDegrees = 45; public static final double autoRotationForTrenchTargetDegrees = 0; public static final boolean teleopOffsetEnabled = true; @@ -130,9 +152,6 @@ public final class Constants { public static final double intakeRotaterRaiseVoltage = 0.8; public static final double intakeRotaterLowerVoltage = 0.05; - public static final double intakeRotaterShootAssistPeriodSeconds = 0.4; - public static final double intakeRotaterShootAssistUpDutyCycle = 0.5; - 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); @@ -160,14 +179,18 @@ public final class Constants { // 状态机时间和门控容差。真车基础动作验证后，需要现场调这些值。 public static final double shooterFeedVoltage = Shooter.shooterConveyorFeedVoltage; public static final double shooterReverseVoltage = Shooter.shooterConveyorReverseVoltage; + public static final double shooterFeedVelocity = Shooter.shooterConveyorFeedVelocity; + public static final double shooterReverseVelocity = Shooter.shooterConveyorReverseVelocity; public static final double shootTimeoutSeconds = 1.0; public static final double aimToleranceDegrees = 1.5; public static final double stationarySpeedToleranceMetersPerSecond = 0.5; public static final double passBallFlywheelVoltage = Shooter.defaultFlywheelVoltage; + public static final double passBallFlywheelVelocity = Shooter.defaultFlywheelVelocity; public static final double passBallBackboardPosition = 175.0; // 射击时背板的目标位置（外接编码器计数）。 public static final double backboardShootPosition = 175.0; + public static final double backboardReadyPositionTolerance = 100.0; // 射击门控是否要求底盘接近静止。 public static final boolean stationaryGateEnabled = true; } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c404048..7755ec3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -181,8 +181,8 @@ public class RobotContainer { controllerupper.b().and(teleopOnly) .onTrue(operatorControls.requestEjectCommand()) .onFalse(operatorControls.requestIdleCommand()); - controllerupper.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVoltageOffset(-0.1))); - controllerupper.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVoltageOffset(0.1))); + controllerupper.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(-1.0))); + controllerupper.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(1.0))); controllerupper.povUp().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(100))); controllerupper.povDown().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(-100))); controllerupper.back().onTrue(Commands.runOnce(() -> operatorControls.resetOffsets())); @@ -200,12 +200,15 @@ public class RobotContainer { public void tunerPeriodic() { driveGainsTuner.periodic(); configFlywheelTuner.periodic(() -> shooterSubsystem.applyLeftConfigurationToRight()); + shooterSubsystem.debugFlywheelVelocityPeriodic(); configConveyorTuner.periodic(null); + shooterSubsystem.debugShooterConveyorVelocityPeriodic(); configBackboardTuner.periodic(); shooterSubsystem.debugBackboardTargetPeriodic(); configMainConveyorTuner.periodic(null); configIntakerRollerTuner.periodic(null); configIntakerRotaterTuner.periodic(null); + shooterSubsystem.setRightFlywheelFollowLeft(); shooterSubsystem.setRightConveyorFollowLeft(); } diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index 0cbac6c..1a12af4 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -244,7 +244,11 @@ public class DriveControls { // If angle difference is within threshold, don't rotate if (Math.abs(calDifference) > 1.0) { - return calDifference * Constants.AutoPositioning.autoRotationkP; + double rotationRate = calDifference * Constants.AutoPositioning.autoRotationkP; + if (Math.abs(rotationRate) < 0.24) { + return Math.copySign(0.24, rotationRate); + } + return rotationRate; } else { return 0; } diff --git a/src/main/java/frc/robot/controls/OperatorControls.java b/src/main/java/frc/robot/controls/OperatorControls.java index eecbed6..f81ec03 100644 --- a/src/main/java/frc/robot/controls/OperatorControls.java +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -54,6 +54,10 @@ public class OperatorControls { superstructure.adjustFlywheelVoltageOffset(offset); } + public void adjustFlywheelVelocityOffset(double offset) { + superstructure.adjustFlywheelVelocityOffset(offset); + } + public void adjustBackboardRateOffset(double offset) { superstructure.adjustBackboardPositionOffset(offset); } diff --git a/src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java index 4ca66cc..455da16 100644 --- a/src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java @@ -69,12 +69,20 @@ public class IntakeRotaterSubsystem extends SubsystemBase { } public void shootAssist() { - double period = Constants.Intaker.intakeRotaterShootAssistPeriodSeconds; - double phase = (Timer.getFPGATimestamp() % period) / period; - double volts = phase < Constants.Intaker.intakeRotaterShootAssistUpDutyCycle - ? Constants.Intaker.intakeRotaterRaiseVoltage - : Constants.Intaker.intakeRotaterLowerVoltage; - setVoltage(volts); + double cycleTime = Timer.getFPGATimestamp() % 1.7; + + if (cycleTime < 0.7) { + setVoltage(1); + } else if (cycleTime < 0.9) { + setVoltage(0.0); + } else { + setVoltage(0.03); + } + } + + public void postShootIdleAssist(double elapsedSeconds) { + double cycleTime = elapsedSeconds % 0.2; + setVoltage(cycleTime < 0.1 ? 0.03 : 0.0); } public void manualRaise() { diff --git a/src/main/java/frc/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/robot/subsystems/LimelightSubsystem.java index 7d0c338..c118983 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LimelightSubsystem.java @@ -51,4 +51,8 @@ public class LimelightSubsystem extends SubsystemBase { // 查 Constants 里的插值射表；真车试射后往表里加实测点即可。 return Constants.Shooter.flywheelVoltageByDistance.get(getDistanceToTarget()); } + + public double getShooterVelocityByDistance() { + return Constants.Shooter.flywheelVelocityByDistance.get(getDistanceToTarget()); + } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index efa0772..82bf278 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -3,6 +3,7 @@ package frc.robot.subsystems; 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; @@ -39,18 +40,25 @@ public class ShooterSubsystem extends SubsystemBase { public final Encoder backboardEncoder; public final ProfiledPIDController backboardPID; - private final VoltageOut leftFlywheelVoltageRequest = new VoltageOut(0); - private final VoltageOut rightFlywheelVoltageRequest = new VoltageOut(0); - private final VoltageOut conveyorVoltageRequest = new VoltageOut(0); + private final VelocityVoltage leftFlywheelVelocityRequest = new VelocityVoltage(0).withSlot(0); + private final VelocityVoltage conveyorVelocityRequest = new VelocityVoltage(0).withSlot(0); private final VoltageOut backboardVoltageRequest = new VoltageOut(0); + private final Follower flywheelFollower; private final Follower conveyorFollower; private final NetworkTable shooterNetworkTable; + private final NetworkTable flywheelTuningTable; + private final NetworkTable conveyorTuningTable; private final NetworkTable backboardTuningTable; private double flywheelOutputVolts = 0.0; - private double flywheelVoltageStartTimestamp = 0.0; + private double flywheelTargetVelocity = 0.0; + private double flywheelVelocityTolerance = Constants.Shooter.flywheelReadyVelocityTolerance; + private double flywheelVelocityReadyStartTimestamp = 0.0; + private boolean flywheelVelocityTuningActive = false; + private double shooterConveyorTargetVelocity = 0.0; + private boolean shooterConveyorVelocityTuningActive = false; private double shooterConveyorOutputVolts = 0.0; private double backboardOutputVolts = 0.0; private double backboardRawPidOutputVolts = 0.0; @@ -75,7 +83,9 @@ public class ShooterSubsystem extends SubsystemBase { leftFlywheel.getConfigurator().apply(motorOutputConfigs); leftConveyor.getConfigurator().apply(motorOutputConfigs); + flywheelFollower = new Follower(leftFlywheel.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50); conveyorFollower = new Follower(leftConveyor.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50); + setRightFlywheelFollowLeft(); setRightConveyorFollowLeft(); backboardMotor = new TalonFXS(Constants.Shooter.backboardMotorID); @@ -95,6 +105,13 @@ public class ShooterSubsystem extends SubsystemBase { backboardPID.setTolerance(10.0); shooterNetworkTable = NetworkTableInstance.getDefault().getTable("Shooter"); + flywheelTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/flywheel"); + flywheelTuningTable.getEntry("flywheelVelocityEnable").setDefaultBoolean(false); + flywheelTuningTable.getEntry("flywheelTargetVelocity").setDefaultDouble(Constants.Shooter.defaultFlywheelVelocity); + flywheelTuningTable.getEntry("flywheelVelocityTolerance").setDefaultDouble(Constants.Shooter.flywheelReadyVelocityTolerance); + conveyorTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/conveyor"); + conveyorTuningTable.getEntry("conveyorVelocityEnable").setDefaultBoolean(false); + conveyorTuningTable.getEntry("conveyorTargetVelocity").setDefaultDouble(Constants.Shooter.shooterConveyorFeedVelocity); backboardTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/backboard"); backboardTuningTable.getEntry("backboardPositionEnable").setDefaultBoolean(false); backboardTuningTable.getEntry("backboardTargetPosition").setDefaultDouble(0.0); @@ -109,6 +126,10 @@ public class ShooterSubsystem extends SubsystemBase { rightConveyor.setControl(conveyorFollower); } + public void setRightFlywheelFollowLeft() { + rightFlywheel.setControl(flywheelFollower); + } + public void applyLeftConfigurationToRight() { Slot0Configs configs = new Slot0Configs(); leftFlywheel.getConfigurator().refresh(configs); @@ -117,29 +138,51 @@ public class ShooterSubsystem extends SubsystemBase { public void setFlywheelVoltage(double outputVolts) { double clampedOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0); - if (Math.abs(clampedOutputVolts - flywheelOutputVolts) > 1e-6) { - flywheelVoltageStartTimestamp = Timer.getFPGATimestamp(); + setFlywheelVelocity(clampedOutputVolts / Constants.Shooter.flywheelNominalKv); + } + + public void setFlywheelVelocity(double targetVelocityRps) { + double clampedTargetVelocity = MathUtil.clamp( + targetVelocityRps, + -Constants.Shooter.flywheelMaxVelocity, + Constants.Shooter.flywheelMaxVelocity + ); + if (Math.abs(clampedTargetVelocity - flywheelTargetVelocity) > 1e-6) { + flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp(); } - flywheelOutputVolts = clampedOutputVolts; - leftFlywheel.setControl(leftFlywheelVoltageRequest.withOutput(flywheelOutputVolts)); - rightFlywheel.setControl(rightFlywheelVoltageRequest.withOutput(-flywheelOutputVolts)); + flywheelTargetVelocity = clampedTargetVelocity; + leftFlywheel.setControl(leftFlywheelVelocityRequest.withVelocity(flywheelTargetVelocity)); + setRightFlywheelFollowLeft(); } public void setFlywheelVoltageByDistance(double distance) { - double targetVolts = Constants.Shooter.flywheelVoltageByDistance.get(distance); - setFlywheelVoltage(targetVolts); + setFlywheelVelocityByDistance(distance); + } + + public void setFlywheelVelocityByDistance(double distance) { + double targetVelocity = Constants.Shooter.flywheelVelocityByDistance.get(distance); + setFlywheelVelocity(targetVelocity); } public void stopFlywheel() { - setFlywheelVoltage(0.0); + setFlywheelVelocity(0.0); + } + + public boolean atVelocity() { + if (Math.abs(flywheelTargetVelocity) < Constants.Shooter.flywheelReadyVelocityThreshold) { + flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp(); + return false; + } + if (Math.abs(getFlywheelVelocityError()) > flywheelVelocityTolerance) { + flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp(); + return false; + } + return Timer.getFPGATimestamp() - flywheelVelocityReadyStartTimestamp + >= Constants.Shooter.flywheelReadyDelaySeconds; } public boolean atVoltage() { - if (Math.abs(flywheelOutputVolts) < Constants.Shooter.flywheelReadyVoltageThreshold) { - return false; - } - return Timer.getFPGATimestamp() - flywheelVoltageStartTimestamp - >= Constants.Shooter.flywheelReadyDelaySeconds; + return atVelocity(); } public double getLeftFlywheelVelocity() { return leftFlywheel.getVelocity().getValueAsDouble(); @@ -157,22 +200,51 @@ public class ShooterSubsystem extends SubsystemBase { return Math.abs(getLeftFlywheelVelocity()) - Math.abs(getRightFlywheelVelocity()); } + public double getTargetFlywheelVelocity() { + return flywheelTargetVelocity; + } + + public double getFlywheelVelocityError() { + return Math.abs(flywheelTargetVelocity) - getAverageFlywheelVelocity(); + } + public double getTargetFlywheelVoltage() { - return flywheelOutputVolts; + return flywheelTargetVelocity * Constants.Shooter.flywheelNominalKv; } /* ====================== */ /* shooter conveyor */ /* ====================== */ - public void runShooterConveyorVoltage(double outputVolts) { - shooterConveyorOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0); - leftConveyor.setControl(conveyorVoltageRequest.withOutput(shooterConveyorOutputVolts)); + public void runShooterConveyorVelocity(double targetVelocityRps) { + shooterConveyorTargetVelocity = MathUtil.clamp( + targetVelocityRps, + -Constants.Shooter.shooterConveyorMaxVelocity, + Constants.Shooter.shooterConveyorMaxVelocity + ); + leftConveyor.setControl(conveyorVelocityRequest.withVelocity(shooterConveyorTargetVelocity)); setRightConveyorFollowLeft(); } + public void runShooterConveyorVoltage(double outputVolts) { + double clampedOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0); + runShooterConveyorVelocity(clampedOutputVolts / Constants.Shooter.shooterConveyorNominalKv); + } + public void stopShooterConveyor() { - runShooterConveyorVoltage(0.0); + runShooterConveyorVelocity(0.0); + } + + public double getShooterConveyorVelocity() { + return leftConveyor.getVelocity().getValueAsDouble(); + } + + public double getShooterConveyorTargetVelocity() { + return shooterConveyorTargetVelocity; + } + + public double getShooterConveyorVelocityError() { + return shooterConveyorTargetVelocity - getShooterConveyorVelocity(); } public double getConveyorStatorCurrent() { @@ -280,10 +352,36 @@ public class ShooterSubsystem extends SubsystemBase { return backboardPID.atGoal(); } + public double getBackboardGoalPosition() { + return backboardPID.getGoal().position; + } + + public double getBackboardPositionError() { + return getBackboardGoalPosition() - getBackboardPosition(); + } + + public boolean isBackboardNearTarget(double tolerance) { + return Math.abs(getBackboardPositionError()) < tolerance; + } + public boolean isBackboardPositionTuningActive() { return backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false); } + public boolean isFlywheelTuningControlActive() { + boolean voltageTunerEnabled = flywheelTuningTable.getEntry("flywheelEnable").getBoolean(false); + boolean voltageTunerRunning = flywheelTuningTable.getEntry("flywheelEnableRunning").getBoolean(false); + boolean velocityTunerEnabled = flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false); + return (voltageTunerEnabled && voltageTunerRunning) || velocityTunerEnabled; + } + + public boolean isShooterConveyorTuningControlActive() { + boolean voltageTunerEnabled = conveyorTuningTable.getEntry("conveyorEnable").getBoolean(false); + boolean voltageTunerRunning = conveyorTuningTable.getEntry("conveyorEnableRunning").getBoolean(false); + boolean velocityTunerEnabled = conveyorTuningTable.getEntry("conveyorVelocityEnable").getBoolean(false); + return (voltageTunerEnabled && voltageTunerRunning) || velocityTunerEnabled; + } + public void resetbackboardencoder() { backboardEncoder.reset(); backboardPID.reset(0); @@ -329,20 +427,82 @@ public class ShooterSubsystem extends SubsystemBase { } } + public void debugFlywheelVelocityPeriodic() { + flywheelVelocityTuningActive = flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false); + double requestedTargetVelocity = + flywheelTuningTable.getEntry("flywheelTargetVelocity").getDouble(Constants.Shooter.defaultFlywheelVelocity); + flywheelVelocityTolerance = Math.max( + 0.0, + flywheelTuningTable.getEntry("flywheelVelocityTolerance") + .getDouble(Constants.Shooter.flywheelReadyVelocityTolerance) + ); + + double clampedTargetVelocity = MathUtil.clamp( + requestedTargetVelocity, + -Constants.Shooter.flywheelMaxVelocity, + Constants.Shooter.flywheelMaxVelocity + ); + flywheelTuningTable.getEntry("flywheelVelocityEnableActive").setBoolean(flywheelVelocityTuningActive); + flywheelTuningTable.getEntry("flywheelCommandedTargetVelocity").setDouble(requestedTargetVelocity); + flywheelTuningTable.getEntry("flywheelClampedTargetVelocity").setDouble(clampedTargetVelocity); + flywheelTuningTable.getEntry("flywheelVelocityToleranceActive").setDouble(flywheelVelocityTolerance); + + if (flywheelVelocityTuningActive) { + setFlywheelVelocity(clampedTargetVelocity); + } + } + + public void debugShooterConveyorVelocityPeriodic() { + shooterConveyorVelocityTuningActive = + conveyorTuningTable.getEntry("conveyorVelocityEnable").getBoolean(false); + double requestedTargetVelocity = + conveyorTuningTable.getEntry("conveyorTargetVelocity") + .getDouble(Constants.Shooter.shooterConveyorFeedVelocity); + double clampedTargetVelocity = MathUtil.clamp( + requestedTargetVelocity, + -Constants.Shooter.shooterConveyorMaxVelocity, + Constants.Shooter.shooterConveyorMaxVelocity + ); + + conveyorTuningTable.getEntry("conveyorVelocityEnableActive") + .setBoolean(shooterConveyorVelocityTuningActive); + conveyorTuningTable.getEntry("conveyorCommandedTargetVelocity").setDouble(requestedTargetVelocity); + conveyorTuningTable.getEntry("conveyorClampedTargetVelocity").setDouble(clampedTargetVelocity); + + if (shooterConveyorVelocityTuningActive) { + runShooterConveyorVelocity(clampedTargetVelocity); + } + } + @Override public void periodic() { double backboardPosition = getBackboardPosition(); double backboardGoalPosition = backboardPID.getGoal().position; double backboardSetpointPosition = backboardPID.getSetpoint().position; + flywheelOutputVolts = leftFlywheel.getMotorVoltage().getValueAsDouble(); shooterNetworkTable.getEntry("flywheelVelocity").setDouble(getAverageFlywheelVelocity()); shooterNetworkTable.getEntry("leftFlywheelVelocity").setDouble(getLeftFlywheelVelocity()); shooterNetworkTable.getEntry("rightFlywheelVelocity").setDouble(getRightFlywheelVelocity()); shooterNetworkTable.getEntry("flywheelVelocityDifference").setDouble(getFlywheelVelocityDifference()); + shooterNetworkTable.getEntry("flywheelTargetVelocity").setDouble(getTargetFlywheelVelocity()); + shooterNetworkTable.getEntry("flywheelVelocityError").setDouble(getFlywheelVelocityError()); + shooterNetworkTable.getEntry("flywheelVelocityTolerance").setDouble(flywheelVelocityTolerance); + shooterNetworkTable.getEntry("flywheelAtVelocity").setBoolean(atVelocity()); + shooterNetworkTable.getEntry("flywheelVelocityTuningActive").setBoolean(flywheelVelocityTuningActive); + shooterNetworkTable.getEntry("flywheelFollowerOpposed").setBoolean(true); shooterNetworkTable.getEntry("flywheelTargetVoltage").setDouble(getTargetFlywheelVoltage()); shooterNetworkTable.getEntry("flywheelOutputVolts").setDouble(flywheelOutputVolts); shooterNetworkTable.getEntry("flywheelAtVoltage").setBoolean(atVoltage()); + shooterNetworkTable.getEntry("leftFlywheelOutputVolts").setDouble(leftFlywheel.getMotorVoltage().getValueAsDouble()); + shooterNetworkTable.getEntry("rightFlywheelOutputVolts").setDouble(rightFlywheel.getMotorVoltage().getValueAsDouble()); + shooterNetworkTable.getEntry("leftFlywheelStatorCurrent").setDouble(leftFlywheel.getStatorCurrent().getValueAsDouble()); + shooterNetworkTable.getEntry("rightFlywheelStatorCurrent").setDouble(rightFlywheel.getStatorCurrent().getValueAsDouble()); + shooterConveyorOutputVolts = leftConveyor.getMotorVoltage().getValueAsDouble(); shooterNetworkTable.getEntry("shooterConveyorOutputVolts").setDouble(shooterConveyorOutputVolts); - shooterNetworkTable.getEntry("conveyorVelocity").setDouble(leftConveyor.getVelocity().getValueAsDouble()); + shooterNetworkTable.getEntry("conveyorTargetVelocity").setDouble(getShooterConveyorTargetVelocity()); + shooterNetworkTable.getEntry("conveyorVelocity").setDouble(getShooterConveyorVelocity()); + shooterNetworkTable.getEntry("conveyorVelocityError").setDouble(getShooterConveyorVelocityError()); + shooterNetworkTable.getEntry("conveyorVelocityTuningActive").setBoolean(shooterConveyorVelocityTuningActive); shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(backboardPosition); shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardSetpointPosition); shooterNetworkTable.getEntry("backboardOutputVolts").setDouble(backboardOutputVolts); diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index ff64917..584036d 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -52,9 +52,10 @@ public class Superstructure extends SubsystemBase { private WantedState wantedState = WantedState.IDLE; private SystemState systemState = SystemState.STOWED; private double stateStartTimestamp = Timer.getFPGATimestamp(); - private double flywheelVoltageOffset = 0.0; + private double flywheelVelocityOffset = 0.0; private double backboardPositionOffset = 0.0; private boolean shotCompletedThisRequest = false; + private double postShootIdleAssistStartTimestamp = Double.NaN; public Superstructure( ShooterSubsystem shooter, @@ -77,6 +78,9 @@ public class Superstructure extends SubsystemBase { /* ====================== */ public void requestIdle() { + if (systemState == SystemState.SHOOTING) { + postShootIdleAssistStartTimestamp = Timer.getFPGATimestamp(); + } wantedState = WantedState.IDLE; } @@ -164,9 +168,10 @@ public class Superstructure extends SubsystemBase { public boolean canShoot() { boolean stationaryGate = !Constants.Superstructure.stationaryGateEnabled || isDriveStationary(); - return shooter.atVoltage() + return shooter.atVelocity() && drive.isAimed() - && stationaryGate; + && stationaryGate + && isBackboardReady(); } private boolean isDriveStationary() { @@ -175,12 +180,20 @@ public class Superstructure extends SubsystemBase { return translationSpeed <= Constants.Superstructure.stationarySpeedToleranceMetersPerSecond; } + private boolean isBackboardReady() { + return shooter.isBackboardNearTarget(Constants.Superstructure.backboardReadyPositionTolerance); + } + /* ====================== */ /* 现场微调 offset */ /* ====================== */ + public void adjustFlywheelVelocityOffset(double offsetVelocity) { + flywheelVelocityOffset += offsetVelocity; + } + public void adjustFlywheelVoltageOffset(double offsetVolts) { - flywheelVoltageOffset += offsetVolts; + adjustFlywheelVelocityOffset(offsetVolts / Constants.Shooter.flywheelNominalKv); } public void adjustBackboardPositionOffset(double offset) { @@ -188,7 +201,7 @@ public class Superstructure extends SubsystemBase { } public void resetOffsets() { - flywheelVoltageOffset = 0.0; + flywheelVelocityOffset = 0.0; backboardPositionOffset = 0.0; } @@ -227,16 +240,33 @@ public class Superstructure extends SubsystemBase { // 安全默认状态：球路不动，intaker 不主动动作，背板保持受控。 conveyor.stop(); intakeRoller.stop(); - intakeRotater.stop(); - shooter.stopFlywheel(); - shooter.stopShooterConveyor(); + if (!runPostShootIdleAssist()) { + intakeRotater.stop(); + } + stopFlywheelUnlessTuning(); + stopShooterConveyorUnlessTuning(); holdBackboardStowedUnlessTuning(); setSystemState(SystemState.STOWED); } + private boolean runPostShootIdleAssist() { + if (Double.isNaN(postShootIdleAssistStartTimestamp)) { + return false; + } + + double elapsed = Timer.getFPGATimestamp() - postShootIdleAssistStartTimestamp; + if (elapsed >= 0.6) { + postShootIdleAssistStartTimestamp = Double.NaN; + return false; + } + + intakeRotater.postShootIdleAssist(elapsed); + return true; + } + private void handleIntake() { - shooter.stopFlywheel(); - shooter.stopShooterConveyor(); + stopFlywheelUnlessTuning(); + stopShooterConveyorUnlessTuning(); holdBackboardStowedUnlessTuning(); // Intaker 比赛开始后保持下放，收球何时结束由操作员松开按键决定。 intakeRoller.intake(); @@ -247,35 +277,35 @@ public class Superstructure extends SubsystemBase { private void handleAimHub() { intakeRoller.stop(); conveyor.stop(); - shooter.stopFlywheel(); - shooter.stopShooterConveyor(); + stopFlywheelUnlessTuning(); + stopShooterConveyorUnlessTuning(); holdBackboardStowedUnlessTuning(); setSystemState(SystemState.STOWED); } private void handleShootHub() { - double targetVolts = limelight.getShooterVoltageByDistance() + flywheelVoltageOffset; + double targetVelocity = limelight.getShooterVelocityByDistance() + flywheelVelocityOffset; double backboardPosition = Constants.Superstructure.backboardShootPosition + backboardPositionOffset; - handlePreparedFeed(targetVolts, backboardPosition); + handlePreparedFeed(targetVelocity, backboardPosition); } private void handlePassBall() { - double targetVolts = Constants.Superstructure.passBallFlywheelVoltage + flywheelVoltageOffset; + double targetVelocity = Constants.Superstructure.passBallFlywheelVelocity + flywheelVelocityOffset; double backboardPosition = Constants.Superstructure.passBallBackboardPosition + backboardPositionOffset; - handlePreparedFeed(targetVolts, backboardPosition); + handlePreparedFeed(targetVelocity, backboardPosition); } - private void handlePreparedFeed(double targetVolts, double backboardPosition) { + private void handlePreparedFeed(double targetVelocity, double backboardPosition) { if (systemState != SystemState.PREP_SHOOT && systemState != SystemState.SHOOTING) { setSystemState(SystemState.PREP_SHOOT); } intakeRoller.stop(); conveyor.stop(); - shooter.stopShooterConveyor(); + stopShooterConveyorUnlessTuning(); // 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调， // 但不会绕过 Superstructure 状态机。 - shooter.setFlywheelVoltage(targetVolts); + shooter.setFlywheelVelocity(targetVelocity); shooter.holdBackboardAt(backboardPosition); if (systemState == SystemState.SHOOTING) { @@ -283,18 +313,13 @@ public class Superstructure extends SubsystemBase { // 避免球接触飞轮导致的掉速中断喂球。 intakeRotater.shootAssist(); conveyor.feedToShooter(); - shooter.runShooterConveyorVoltage(Constants.Superstructure.shooterFeedVoltage); - - if (timeInState() > Constants.Superstructure.shootTimeoutSeconds) { - shotCompletedThisRequest = true; - setSystemState(SystemState.PREP_SHOOT); - } + runShooterConveyorVelocityUnlessTuning(Constants.Superstructure.shooterFeedVelocity); return; } // 未开始喂球前，球路保持静止。 conveyor.stop(); - shooter.stopShooterConveyor(); + stopShooterConveyorUnlessTuning(); // 门控条件实时刷新：条件回落时状态同步回退，仪表盘能看到卡在哪一关。 if (canShoot()) { @@ -307,8 +332,8 @@ public class Superstructure extends SubsystemBase { setSystemState(SystemState.EJECTING); intakeRoller.outtake(); conveyor.reverse(); - shooter.runShooterConveyorVoltage(Constants.Superstructure.shooterReverseVoltage); - shooter.stopFlywheel(); + runShooterConveyorVelocityUnlessTuning(Constants.Superstructure.shooterReverseVelocity); + stopFlywheelUnlessTuning(); holdBackboardStowedUnlessTuning(); } @@ -318,12 +343,30 @@ public class Superstructure extends SubsystemBase { } } + private void stopFlywheelUnlessTuning() { + if (!shooter.isFlywheelTuningControlActive()) { + shooter.stopFlywheel(); + } + } + + private void runShooterConveyorVelocityUnlessTuning(double targetVelocity) { + if (!shooter.isShooterConveyorTuningControlActive()) { + shooter.runShooterConveyorVelocity(targetVelocity); + } + } + + private void stopShooterConveyorUnlessTuning() { + if (!shooter.isShooterConveyorTuningControlActive()) { + shooter.stopShooterConveyor(); + } + } + private void handleManual() { // 手动模式：状态机不再驱动机构，交给现场手动/调参工具； // 只保持飞轮与喂球停止，避免意外射出。 setSystemState(SystemState.MANUAL_OVERRIDE); - shooter.stopFlywheel(); - shooter.stopShooterConveyor(); + stopFlywheelUnlessTuning(); + stopShooterConveyorUnlessTuning(); conveyor.stop(); intakeRoller.stop(); } @@ -352,11 +395,19 @@ public class Superstructure extends SubsystemBase { table.getEntry("systemState").setString(systemState.name()); table.getEntry("timeInState").setDouble(timeInState()); table.getEntry("canShoot").setBoolean(canShoot()); + table.getEntry("shooterAtVelocity").setBoolean(shooter.atVelocity()); table.getEntry("shooterAtVoltage").setBoolean(shooter.atVoltage()); table.getEntry("driveAimed").setBoolean(drive.isAimed()); table.getEntry("driveHeadingError").setDouble(drive.getHeadingError()); table.getEntry("driveStationary").setBoolean(isDriveStationary()); - table.getEntry("flywheelVoltageOffset").setDouble(flywheelVoltageOffset); + table.getEntry("backboardReady").setBoolean(isBackboardReady()); + table.getEntry("backboardPositionError").setDouble(shooter.getBackboardPositionError()); + table.getEntry("backboardReadyTolerance").setDouble(Constants.Superstructure.backboardReadyPositionTolerance); + table.getEntry("flywheelVelocityOffset").setDouble(flywheelVelocityOffset); + table.getEntry("flywheelTuningControlActive").setBoolean(shooter.isFlywheelTuningControlActive()); + table.getEntry("shooterConveyorTuningControlActive") + .setBoolean(shooter.isShooterConveyorTuningControlActive()); + table.getEntry("flywheelVoltageOffset").setDouble(flywheelVelocityOffset * Constants.Shooter.flywheelNominalKv); table.getEntry("backboardPositionOffset").setDouble(backboardPositionOffset); } } diff --git a/src/main/java/frc/robot/tuning/ConfigTalonFXMotorTuner.java b/src/main/java/frc/robot/tuning/ConfigTalonFXMotorTuner.java index 447cd0c..825a176 100644 --- a/src/main/java/frc/robot/tuning/ConfigTalonFXMotorTuner.java +++ b/src/main/java/frc/robot/tuning/ConfigTalonFXMotorTuner.java @@ -1,6 +1,4 @@ package frc.robot.tuning; -import java.util.function.Function; - import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; @@ -25,6 +23,7 @@ public class ConfigTalonFXMotorTuner { private final BooleanSubscriber enableRuningSub; private final DoubleSubscriber runningVolts; private double lastKp = 0, lastKi = 0, lastKd = 0, lastKv = 0, lastKa = 0, lastKs = 0; + private boolean wasRunning = false; private final VoltageOut voltageRequest = new VoltageOut(0); public ConfigTalonFXMotorTuner(TalonFX motor_, String motorName_, Slot0Configs motorSlot0Configs_) { @@ -83,9 +82,11 @@ public class ConfigTalonFXMotorTuner { if (enableRuningSub.get()) {; double volts = MathUtil.clamp(runningVolts.get(), -12.0, 12.0); motor.setControl(voltageRequest.withOutput(volts)); + wasRunning = true; // System.out.println("running motor with volts: "+volts+" on motor: "+motorName); - }else{ + }else if (wasRunning){ motor.setControl(voltageRequest.withOutput(0)); + wasRunning = false; } }