From 1c39a3bc4201322a780185c908e5e5f334e82106 Mon Sep 17 00:00:00 2001 From: OnDYCLaptop Date: Tue, 7 Jul 2026 23:25:10 +0800 Subject: [PATCH] 7.7 --- elastic-layout-debug.json | 1395 ++++++++++++++--- elastic-layout.json | 56 +- src/main/java/frc/robot/Constants.java | 73 +- src/main/java/frc/robot/RobotContainer.java | 23 +- .../java/frc/robot/controls/Autocommand.java | 6 +- .../frc/robot/controls/OperatorControls.java | 8 +- .../subsystems/CommandSwerveDrivetrain.java | 4 +- .../robot/subsystems/ConveyorSubsystem.java | 21 +- .../subsystems/IntakeRollerSubsystem.java | 23 +- .../subsystems/IntakeRotaterSubsystem.java | 72 +- .../robot/subsystems/LimelightSubsystem.java | 4 +- .../robot/subsystems/ShooterSubsystem.java | 194 ++- .../frc/robot/subsystems/Superstructure.java | 31 +- .../robot/tuning/ConfigTalonFXMotorTuner.java | 19 +- .../tuning/ConfigTalonFXSMotorTuner.java | 1 - 15 files changed, 1518 insertions(+), 412 deletions(-) diff --git a/elastic-layout-debug.json b/elastic-layout-debug.json index 1ae265d..cff8a4d 100644 --- a/elastic-layout-debug.json +++ b/elastic-layout-debug.json @@ -101,14 +101,14 @@ } }, { - "title": "Shooter At Speed", + "title": "Shooter At Voltage", "x": 192.0, "y": 128.0, "width": 192.0, "height": 128.0, "type": "Boolean Box", "properties": { - "topic": "/Superstructure/shooterAtSpeed", + "topic": "/Superstructure/shooterAtVoltage", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -168,14 +168,14 @@ } }, { - "title": "Flywheel Offset", + "title": "Flywheel Voltage Offset", "x": 256.0, "y": 256.0, "width": 256.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Superstructure/flywheelSpeedOffsetRps", + "topic": "/Superstructure/flywheelVoltageOffset", "period": 0.06, "data_type": "double", "show_submit_button": false @@ -277,14 +277,14 @@ "layouts": [], "containers": [ { - "title": "Flywheel Speed", + "title": "Flywheel Velocity", "x": 0.0, "y": 0.0, "width": 384.0, "height": 256.0, "type": "Graph", "properties": { - "topic": "/Shooter/flywheelSpeed", + "topic": "/Shooter/flywheelVelocity", "period": 0.033, "data_type": "double", "time_displayed": 5.0, @@ -293,28 +293,28 @@ } }, { - "title": "Flywheel Target", + "title": "Flywheel Target Voltage", "x": 384.0, "y": 0.0, "width": 256.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shooter/flywheelTargetSpeed", + "topic": "/Shooter/flywheelTargetVoltage", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Flywheel At Speed", + "title": "Flywheel At Voltage", "x": 640.0, "y": 0.0, "width": 256.0, "height": 128.0, "type": "Boolean Box", "properties": { - "topic": "/Shooter/flywheelAtSpeed", + "topic": "/Shooter/flywheelAtVoltage", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -324,42 +324,42 @@ } }, { - "title": "Flywheel Difference", + "title": "Flywheel Velocity Difference", "x": 384.0, "y": 128.0, "width": 256.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shooter/flywheelSpeedDifference", + "topic": "/Shooter/flywheelVelocityDifference", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Shooter Conveyor Target RPS", + "title": "Shooter Conveyor Volts", "x": 640.0, "y": 128.0, "width": 256.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shooter/shooterConveyorTargetRps", + "topic": "/Shooter/shooterConveyorOutputVolts", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Shooter Conveyor Speed", + "title": "Shooter Conveyor Velocity", "x": 0.0, "y": 256.0, "width": 384.0, "height": 256.0, "type": "Graph", "properties": { - "topic": "/Shooter/conveyorSpeed", + "topic": "/Shooter/conveyorVelocity", "period": 0.033, "data_type": "double", "time_displayed": 5.0, @@ -459,28 +459,28 @@ } }, { - "title": "Intake Target", + "title": "Intake Output Volts", "x": 256.0, "y": 512.0, "width": 256.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/IntakeRotater/targetPosition", + "topic": "/IntakeRotater/outputVoltage", "period": 0.06, "data_type": "double", "show_submit_button": false } }, { - "title": "Intake At Goal", + "title": "Intake Running", "x": 512.0, "y": 512.0, "width": 128.0, "height": 128.0, "type": "Boolean Box", "properties": { - "topic": "/IntakeRotater/atGoal", + "topic": "/IntakeRotater/isRunning", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -546,19 +546,67 @@ } }, { - "title": "Intake Roller Target RPS", + "title": "Intake Roller Volts", "x": 0.0, "y": 768.0, "width": 256.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/IntakeRoller/targetSpeedRps", + "topic": "/IntakeRoller/targetOutputVolts", "period": 0.06, "data_type": "double", "show_submit_button": false } }, + { + "title": "Intake Roller Left Speed", + "x": 256.0, + "y": 768.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/IntakeRoller/leftVelocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Intake Roller Right Speed", + "x": 640.0, + "y": 768.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/IntakeRoller/rightVelocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Intake Rotater Speed", + "x": 1024.0, + "y": 768.0, + "width": 384.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/IntakeRotater/leftVelocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, { "title": "Main Conveyor Velocity", "x": 896.0, @@ -576,14 +624,28 @@ } }, { - "title": "Main Conveyor Target RPS", + "title": "Main Conveyor Volts", "x": 1280.0, "y": 512.0, "width": 256.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Conveyor/targetSpeedRps", + "topic": "/Conveyor/targetOutputVolts", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Flywheel Output Volts", + "x": 640.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/flywheelOutputVolts", "period": 0.06, "data_type": "double", "show_submit_button": false @@ -626,7 +688,7 @@ "height": 256.0, "type": "Camera Stream", "properties": { - "topic": "/CameraPublisher/limelight-shooter", + "topic": "/CameraPublisher/limelight-left", "period": 0.06, "rotation_turns": 0 } @@ -639,7 +701,7 @@ "height": 256.0, "type": "Camera Stream", "properties": { - "topic": "/CameraPublisher/limelight-intaker", + "topic": "/CameraPublisher/limelight-front", "period": 0.06, "rotation_turns": 0 } @@ -1010,98 +1072,14 @@ } }, { - "title": "Flywheel Run RPS", + "title": "Flywheel Run Volts", "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", + "topic": "/Tuning/flywheel/flywheelRunningVolts", "period": 0.06, "data_type": "double", "show_submit_button": true @@ -1134,98 +1112,14 @@ } }, { - "title": "Shooter Conv Run RPS", + "title": "Shooter Conv Run Volts", "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", + "topic": "/Tuning/conveyor/conveyorRunningVolts", "period": 0.06, "data_type": "double", "show_submit_button": true @@ -1354,6 +1248,1111 @@ "data_type": "double", "show_submit_button": true } + }, + { + "title": "Main Conv Enable", + "x": 0.0, + "y": 768.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/mainconveyor/mainconveyorEnable", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Main Conv Run", + "x": 128.0, + "y": 768.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/mainconveyor/mainconveyorEnableRunning", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Main Conv Run Volts", + "x": 256.0, + "y": 768.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/mainconveyor/mainconveyorRunningVolts", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Intake Roller Enable", + "x": 0.0, + "y": 960.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/intakerroller/intakerrollerEnable", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Intake Roller Run", + "x": 128.0, + "y": 960.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/intakerroller/intakerrollerEnableRunning", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Intake Roller Run Volts", + "x": 256.0, + "y": 960.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/intakerroller/intakerrollerRunningVolts", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Intake Roller Left Speed", + "x": 512.0, + "y": 960.0, + "width": 384.0, + "height": 128.0, + "type": "Graph", + "properties": { + "topic": "/IntakeRoller/leftVelocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Intake Roller Right Speed", + "x": 896.0, + "y": 960.0, + "width": 384.0, + "height": 128.0, + "type": "Graph", + "properties": { + "topic": "/IntakeRoller/rightVelocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Intake Roller Speed Diff", + "x": 1280.0, + "y": 960.0, + "width": 256.0, + "height": 128.0, + "type": "Graph", + "properties": { + "topic": "/IntakeRoller/velocityDifference", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Intake Rotater Enable", + "x": 0.0, + "y": 1152.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/intakerrotater/intakerrotaterEnable", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Intake Rotater Run", + "x": 128.0, + "y": 1152.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/intakerrotater/intakerrotaterEnableRunning", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Intake Rotater Run Volts", + "x": 256.0, + "y": 1152.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/intakerrotater/intakerrotaterRunningVolts", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Intake Rotater Left Speed", + "x": 512.0, + "y": 1152.0, + "width": 384.0, + "height": 128.0, + "type": "Graph", + "properties": { + "topic": "/IntakeRotater/leftVelocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Intake Rotater Right Speed", + "x": 896.0, + "y": 1152.0, + "width": 384.0, + "height": 128.0, + "type": "Graph", + "properties": { + "topic": "/IntakeRotater/rightVelocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Intake Rotater Speed Diff", + "x": 1280.0, + "y": 1152.0, + "width": 256.0, + "height": 128.0, + "type": "Graph", + "properties": { + "topic": "/IntakeRotater/velocityDifference", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + } + ] + } + }, + { + "name": "Intaker", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Roller Enable", + "x": 0.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/intakerroller/intakerrollerEnable", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Roller Run", + "x": 128.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/intakerroller/intakerrollerEnableRunning", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Roller Run Volts", + "x": 256.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/intakerroller/intakerrollerRunningVolts", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Roller Output Volts", + "x": 512.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/IntakeRoller/targetOutputVolts", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Roller Left Speed", + "x": 768.0, + "y": 0.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": "Roller Right Speed", + "x": 1024.0, + "y": 0.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": "Roller Speed Diff", + "x": 1280.0, + "y": 0.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": "Roller Left Speed Trend", + "x": 0.0, + "y": 128.0, + "width": 512.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/IntakeRoller/leftVelocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Roller Right Speed Trend", + "x": 512.0, + "y": 128.0, + "width": 512.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/IntakeRoller/rightVelocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Roller Speed Diff Trend", + "x": 1024.0, + "y": 128.0, + "width": 512.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/IntakeRoller/velocityDifference", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Rotater Enable", + "x": 0.0, + "y": 448.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/intakerrotater/intakerrotaterEnable", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Rotater Run", + "x": 128.0, + "y": 448.0, + "width": 128.0, + "height": 128.0, + "type": "Toggle Switch", + "properties": { + "topic": "/Tuning/intakerrotater/intakerrotaterEnableRunning", + "period": 0.06, + "data_type": "boolean" + } + }, + { + "title": "Rotater Run Volts", + "x": 256.0, + "y": 448.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/intakerrotater/intakerrotaterRunningVolts", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Rotater Output Volts", + "x": 512.0, + "y": 448.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": "Rotater Position", + "x": 768.0, + "y": 448.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": "Rotater Running", + "x": 1024.0, + "y": 448.0, + "width": 256.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/IntakeRotater/isRunning", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Rotater Speed Diff", + "x": 1280.0, + "y": 448.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/IntakeRotater/velocityDifference", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Rotater Left Speed Trend", + "x": 0.0, + "y": 576.0, + "width": 512.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/IntakeRotater/leftVelocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Rotater Right Speed Trend", + "x": 512.0, + "y": 576.0, + "width": 512.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/IntakeRotater/rightVelocity", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Rotater Speed Diff Trend", + "x": 1024.0, + "y": 576.0, + "width": 512.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/IntakeRotater/velocityDifference", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Rotater Position Trend", + "x": 0.0, + "y": 832.0, + "width": 512.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/IntakeRotater/position", + "period": 0.033, + "data_type": "double", + "time_displayed": 5.0, + "color": 4278238420, + "line_width": 2.0 + } + } + ] + } + }, + { + "name": "Backboard Debug", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "Position", + "x": 0.0, + "y": 0.0, + "width": 512.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/backboardCurrentRate", + "period": 0.033, + "data_type": "double", + "time_displayed": 6.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Setpoint Position", + "x": 512.0, + "y": 0.0, + "width": 512.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/backboardSetpointPosition", + "period": 0.033, + "data_type": "double", + "time_displayed": 6.0, + "color": 4294198070, + "line_width": 2.0 + } + }, + { + "title": "Goal Position", + "x": 1024.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardGoalPosition", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "At Target", + "x": 1280.0, + "y": 0.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": "Position Offset", + "x": 1024.0, + "y": 128.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": "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, + "y": 256.0, + "width": 512.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/backboardGoalError", + "period": 0.033, + "data_type": "double", + "time_displayed": 6.0, + "color": 4281966278, + "line_width": 2.0 + } + }, + { + "title": "PID Output", + "x": 512.0, + "y": 256.0, + "width": 512.0, + "height": 256.0, + "type": "Graph", + "properties": { + "topic": "/Shooter/backboardPIDOutput", + "period": 0.033, + "data_type": "double", + "time_displayed": 6.0, + "color": 4278238420, + "line_width": 2.0 + } + }, + { + "title": "Setpoint Velocity", + "x": 1024.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardSetpointVelocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Setpoint Error", + "x": 1280.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardSetpointError", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Raw PID Output", + "x": 1024.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardRawPIDOutput", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Clamped PID Output", + "x": 1280.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardClampedPIDOutput", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Output Saturated", + "x": 0.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shooter/backboardOutputSaturated", + "period": 0.06, + "data_type": "boolean", + "true_color": 4294198070, + "false_color": 4283215696, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Minimum Output", + "x": 256.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Shooter/backboardMinimumOutputActive", + "period": 0.06, + "data_type": "boolean", + "true_color": 4294198070, + "false_color": 4283215696, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Final Output Volts", + "x": 512.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardOutputVolts", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Max Output Volts", + "x": 768.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardMaxOutputVolts", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Motor Voltage", + "x": 1024.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardMotorVoltage", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Motor Current", + "x": 1280.0, + "y": 512.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardMotorStatorCurrent", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Motor Position", + "x": 0.0, + "y": 640.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardMotorPosition", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Motor Velocity", + "x": 256.0, + "y": 640.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardMotorVelocity", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Tolerance", + "x": 512.0, + "y": 640.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardPositionTolerance", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Shoot Position", + "x": 768.0, + "y": 640.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardShootPosition", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Pass Position", + "x": 1024.0, + "y": 640.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardPassPosition", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "System State", + "x": 1280.0, + "y": 640.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": "Backboard Enable", + "x": 0.0, + "y": 768.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": 768.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": 768.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": "Talon kP", + "x": 512.0, + "y": 768.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": "Talon kI", + "x": 640.0, + "y": 768.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": "Talon kD", + "x": 768.0, + "y": 768.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": "Talon kV", + "x": 896.0, + "y": 768.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": "Talon kS", + "x": 1024.0, + "y": 768.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": "Talon kA", + "x": 1152.0, + "y": 768.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 + } + }, + { + "title": "Min Output Volts", + "x": 1280.0, + "y": 768.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Shooter/backboardMinOutputVolts", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Position Enable", + "x": 0.0, + "y": 896.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": 256.0, + "y": 896.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/backboard/backboardTargetPosition", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Position Active", + "x": 512.0, + "y": 896.0, + "width": 256.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/Tuning/backboard/backboardPositionEnableActive", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Commanded Target", + "x": 768.0, + "y": 896.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/backboard/backboardCommandedTargetPosition", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Clamped Target", + "x": 1024.0, + "y": 896.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/Tuning/backboard/backboardClampedTargetPosition", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } } ] } diff --git a/elastic-layout.json b/elastic-layout.json index b933e4f..6e653bb 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -768,14 +768,14 @@ } }, { - "title": "flywheelRunningSpeedRPS", + "title": "flywheelRunningVolts", "x": 640.0, "y": 128.0, "width": 256.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Tuning/flywheel/flywheelRunningSpeedRPS", + "topic": "/Tuning/flywheel/flywheelRunningVolts", "period": 0.06, "data_type": "double", "show_submit_button": true @@ -845,27 +845,27 @@ "layouts": [], "containers": [ { - "title": "limelight-intaker", + "title": "limelight-front", "x": 0.0, "y": 0.0, "width": 256.0, "height": 256.0, "type": "Camera Stream", "properties": { - "topic": "/CameraPublisher/limelight-intaker", + "topic": "/CameraPublisher/limelight-front", "period": 0.06, "rotation_turns": 0 } }, { - "title": "limelight-shooter", + "title": "limelight-left", "x": 768.0, "y": 512.0, "width": 640.0, "height": 384.0, "type": "Camera Stream", "properties": { - "topic": "/CameraPublisher/limelight-shooter", + "topic": "/CameraPublisher/limelight-left", "period": 0.06, "rotation_turns": 0 } @@ -909,7 +909,7 @@ "height": 128.0, "type": "Boolean Box", "properties": { - "topic": "/Positioning/limelight-intaker/available", + "topic": "/Positioning/limelight-front/available", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -926,7 +926,7 @@ "height": 128.0, "type": "Boolean Box", "properties": { - "topic": "/Positioning/limelight-shooter/available", + "topic": "/Positioning/limelight-left/available", "period": 0.06, "data_type": "boolean", "true_color": 4283215696, @@ -1034,7 +1034,7 @@ "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Positioning/limelight-intaker/Distance", + "topic": "/Positioning/limelight-front/Distance", "period": 0.06, "data_type": "double", "show_submit_button": true @@ -1064,7 +1064,7 @@ "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Positioning/limelight-shooter/Distance", + "topic": "/Positioning/limelight-left/Distance", "period": 0.06, "data_type": "double", "show_submit_button": true @@ -1138,7 +1138,7 @@ "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Positioning/limelight-intaker/X", + "topic": "/Positioning/limelight-front/X", "period": 0.06, "data_type": "double", "show_submit_button": true @@ -1152,7 +1152,7 @@ "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Positioning/limelight-intaker/Y", + "topic": "/Positioning/limelight-front/Y", "period": 0.06, "data_type": "double", "show_submit_button": true @@ -1166,7 +1166,7 @@ "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Positioning/limelight-shooter/X", + "topic": "/Positioning/limelight-left/X", "period": 0.06, "data_type": "double", "show_submit_button": true @@ -1180,7 +1180,7 @@ "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Positioning/limelight-shooter/Y", + "topic": "/Positioning/limelight-left/Y", "period": 0.06, "data_type": "double", "show_submit_button": true @@ -1195,14 +1195,14 @@ "layouts": [], "containers": [ { - "title": "limelight-shooter", + "title": "limelight-left", "x": 1152.0, "y": 0.0, "width": 384.0, "height": 256.0, "type": "Camera Stream", "properties": { - "topic": "/CameraPublisher/limelight-shooter", + "topic": "/CameraPublisher/limelight-left", "period": 0.06, "rotation_turns": 0 } @@ -1224,14 +1224,14 @@ } }, { - "title": "conveyorSpeed", + "title": "conveyorVelocity", "x": 384.0, "y": 256.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shooter/conveyorSpeed", + "topic": "/Shooter/conveyorVelocity", "period": 0.06, "data_type": "double", "show_submit_button": true @@ -1271,14 +1271,14 @@ } }, { - "title": "flywheelSpeed", + "title": "flywheelVelocity", "x": 640.0, "y": 0.0, "width": 256.0, "height": 256.0, "type": "Graph", "properties": { - "topic": "/Shooter/flywheelSpeed", + "topic": "/Shooter/flywheelVelocity", "period": 0.033, "data_type": "double", "time_displayed": 5.0, @@ -1322,14 +1322,14 @@ } }, { - "title": "conveyorSpeed", + "title": "conveyorVelocity", "x": 384.0, "y": 0.0, "width": 256.0, "height": 256.0, "type": "Graph", "properties": { - "topic": "/Shooter/conveyorSpeed", + "topic": "/Shooter/conveyorVelocity", "period": 0.033, "data_type": "double", "time_displayed": 5.0, @@ -1338,14 +1338,14 @@ } }, { - "title": "flywheelSpeed", + "title": "flywheelVelocity", "x": 640.0, "y": 256.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/Shooter/flywheelSpeed", + "topic": "/Shooter/flywheelVelocity", "period": 0.033, "data_type": "double", "show_submit_button": true @@ -1380,14 +1380,14 @@ } }, { - "title": "flywheelTargetSpeed", + "title": "flywheelTargetVoltage", "x": 768.0, "y": 256.0, "width": 128.0, "height": 128.0, "type": "Text Display", "properties": { - "topic": "/ShooterControl/flywheelTargetSpeed", + "topic": "/Shooter/flywheelTargetVoltage", "period": 0.06, "data_type": "double", "show_submit_button": true @@ -1424,14 +1424,14 @@ } }, { - "title": "flywheelTargetSpeed", + "title": "flywheelTargetVoltage", "x": 640.0, "y": 384.0, "width": 256.0, "height": 256.0, "type": "Graph", "properties": { - "topic": "/ShooterControl/flywheelTargetSpeed", + "topic": "/Shooter/flywheelTargetVoltage", "period": 0.033, "data_type": "double", "time_displayed": 5.0, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c819a48..86c2f8d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -12,27 +12,28 @@ public final class Constants { public static class Shooter { // 机构 CAN ID 目前是占位值；季后赛最终接线确认后，只需要改这里。 - public static final int leftFlywheelID = 41; - public static final int rightFlywheelID = 42; - public static final int leftConveyorID = 43; - public static final int rightConveyorID = 45; + public static final int leftFlywheelID = 38; + public static final int rightFlywheelID = 39; + public static final int leftConveyorID = 40; + public static final int rightConveyorID = 37; public static final int backboardMotorID = 44; - public static final double flywheelSpeedToleranceRps = 5.0; - public static final double defaultFlywheelTargetRps = 64.0; + public static final double defaultFlywheelVoltage = 3.34; + public static final double flywheelReadyVoltageThreshold = 1.0; + public static final double flywheelReadyDelaySeconds = 0.5; - // 距离(米) -> 飞轮转速(RPS) 射表。当前为占位点； + // 距离(米) -> 飞轮电压(V) 射表。当前为占位点； // 真车试射后按实测数据加点即可，查表自动线性插值并在两端取边界值。 - public static final InterpolatingDoubleTreeMap flywheelRpsByDistance = new InterpolatingDoubleTreeMap(); + public static final InterpolatingDoubleTreeMap flywheelVoltageByDistance = new InterpolatingDoubleTreeMap(); static { - flywheelRpsByDistance.put(1.2, 58.8); - flywheelRpsByDistance.put(2.5, 64.0); - flywheelRpsByDistance.put(3.5, 68.0); - flywheelRpsByDistance.put(5.0, 74.0); + flywheelVoltageByDistance.put(1.2, 5.82); + flywheelVoltageByDistance.put(2.5, 6.34); + flywheelVoltageByDistance.put(3.5, 6.73); + flywheelVoltageByDistance.put(5.0, 7.33); } - public static final double conveyorSpeed = 150; - public static final double conveyorSpeedForAuto = 10; + public static final double shooterConveyorFeedVoltage = 3.0; + public static final double shooterConveyorReverseVoltage = -2.0; public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs() .withKP(0.0).withKI(0).withKD(0) @@ -47,13 +48,14 @@ public final class Constants { .withKV(0.03).withKA(0.0).withKS(0.010); public static class backboardPositionPID { - public static final double kP = 0.3; + public static final double kP = 0.007; public static final double kI = 0.0; public static final double kD = 0.0; } - public static final double backboardSpeedMax = 60; - public static final double backboardUpLimit = 3000.0; + public static final double backboardSpeedMax = 5.0; + public static final double backboardSpeedMin = 0.4; + public static final double backboardUpLimit = 2200.0; public static final double backboardDownLimit = 0.0; } @@ -93,8 +95,8 @@ public final class Constants { } public static class Limelight { - public static final String LIMELIGHT_NAME_Shooter = "limelight-shooter"; - public static final String LIMELIGHT_NAME_Intaker = "limelight-intaker"; + public static final String LIMELIGHT_NAME_Shooter = "limelight-left"; + public static final String LIMELIGHT_NAME_Intaker = "limelight-front"; public static final Boolean UsingMetaTag2 = true; public static final int locatePipelineIndex = 0; } @@ -112,24 +114,20 @@ public final class Constants { public static class Intaker { // 进球机构 CAN ID 同样是占位值，真实 ID 确认后集中在这里修改。 - public static final int leftIntakeRollerID = 47; - public static final int rightIntakeRollerID = 46; + public static final int leftIntakeRollerID = 20; + public static final int rightIntakeRollerID = 47; public static final int leftIntakeRotaterID = 48; - public static final int rightIntakeRotaterID = 49; + public static final int rightIntakeRotaterID = 43; - public static final double intakeRollerIntakeSpeedRps = 30.0; - public static final double intakeRollerOuttakeSpeedRps = -30.0; + public static final double intakeRollerIntakeVoltage = -6.0; + public static final double intakeRollerOuttakeVoltage = 3.0; + + public static final double intakeRotaterRaiseVoltage = 0.8; + public static final double intakeRotaterLowerVoltage = 0.05; - public static final double intakeRotaterLoweredPosition = -7.6; - public static final double intakeRotaterShootAssistUpPosition = -2.5; - public static final double intakeRotaterManualRaiseVoltage = 2.0; - public static final double intakeRotaterManualLowerVoltage = -2.0; public static final double intakeRotaterShootAssistPeriodSeconds = 0.4; public static final double intakeRotaterShootAssistUpDutyCycle = 0.5; - public static final double intakeRotaterUpLimit = 0.0; - public static final double intakeRotaterDownLimit = -7.6; - 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); @@ -141,13 +139,12 @@ public final class Constants { public static final double IntakeGetterSpeedforAuto = 0.5; // 这里给出可用的占位值，真车调好位置闭环后再收紧。 - public static final double positionToleranceRotations = 0.3; } public static class Conveyor { - public static final int mainConveyorID = 50; - public static final double feedSpeedRps = 30.0; - public static final double reverseSpeedRps = -30.0; + public static final int mainConveyorID = 42; + public static final double feedVoltage = 2.5; + public static final double reverseVoltage = -2.0; public static final Slot0Configs mainConveyorSlot0Configs = new Slot0Configs() .withKP(0.00).withKI(0.00).withKD(0.00) @@ -156,12 +153,12 @@ public final class Constants { public static class Superstructure { // 状态机时间和门控容差。真车基础动作验证后，需要现场调这些值。 - public static final double shooterFeedSpeedRps = Shooter.conveyorSpeed; - public static final double shooterReverseSpeedRps = -Shooter.conveyorSpeed * 0.6; + public static final double shooterFeedVoltage = Shooter.shooterConveyorFeedVoltage; + public static final double shooterReverseVoltage = Shooter.shooterConveyorReverseVoltage; 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 passBallFlywheelRps = Shooter.defaultFlywheelTargetRps; + public static final double passBallFlywheelVoltage = Shooter.defaultFlywheelVoltage; public static final double passBallBackboardPosition = 175.0; // 射击时背板的目标位置（外接编码器计数）。 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 52f59bd..c404048 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -82,6 +82,12 @@ public class RobotContainer { new ConfigTalonFXMotorTuner(shooterSubsystem.leftConveyor, "conveyor", Constants.Shooter.conveyorSlot0Configs); private ConfigTalonFXSMotorTuner configBackboardTuner = new ConfigTalonFXSMotorTuner(shooterSubsystem.backboardMotor, "backboard", Constants.Shooter.backboardSlot0Configs); + private ConfigTalonFXMotorTuner configMainConveyorTuner = + new ConfigTalonFXMotorTuner(conveyorSubsystem.mainConveyor, "mainconveyor", Constants.Conveyor.mainConveyorSlot0Configs); + private ConfigTalonFXMotorTuner configIntakerRollerTuner = + new ConfigTalonFXMotorTuner(intakeRollerSubsystem.leftIntakeRoller, "intakerroller", Constants.Intaker.intakeRollerSlot0Configs); + private ConfigTalonFXMotorTuner configIntakerRotaterTuner = + new ConfigTalonFXMotorTuner(intakeRotaterSubsystem.leftIntakeRotater, "intakerrotater", Constants.Intaker.intakeRotaterSlot0Configs); private final SendableChooser autoChooser; private final Trigger enableTrigger = new Trigger(DriverStation::isEnabled); @@ -175,8 +181,8 @@ public class RobotContainer { controllerupper.b().and(teleopOnly) .onTrue(operatorControls.requestEjectCommand()) .onFalse(operatorControls.requestIdleCommand()); - controllerupper.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelSpeedOffset(-1))); - controllerupper.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelSpeedOffset(1))); + controllerupper.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVoltageOffset(-0.1))); + controllerupper.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVoltageOffset(0.1))); controllerupper.povUp().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(100))); controllerupper.povDown().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(-100))); controllerupper.back().onTrue(Commands.runOnce(() -> operatorControls.resetOffsets())); @@ -196,7 +202,11 @@ public class RobotContainer { configFlywheelTuner.periodic(() -> shooterSubsystem.applyLeftConfigurationToRight()); configConveyorTuner.periodic(null); configBackboardTuner.periodic(); - shooterSubsystem.setRightFollowLeft(); + shooterSubsystem.debugBackboardTargetPeriodic(); + configMainConveyorTuner.periodic(null); + configIntakerRollerTuner.periodic(null); + configIntakerRotaterTuner.periodic(null); + shooterSubsystem.setRightConveyorFollowLeft(); } public void dashboardPeriodic() { @@ -221,5 +231,12 @@ public class RobotContainer { .onTrue(operatorControls.requestManualCommand()) .whileTrue(operatorControls.manualLowerIntakeRotaterCommand(intakeRotaterSubsystem)) .onFalse(operatorControls.requestIdleCommand()); + + controllerupper.y().and(teleopOnly) + .onTrue(Commands.sequence( + operatorControls.requestManualCommand(), + operatorControls.lowerIntakeRotaterForMatchCommand(intakeRotaterSubsystem), + operatorControls.requestIdleCommand() + )); } } diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java index 6033d82..2b37f66 100644 --- a/src/main/java/frc/robot/controls/Autocommand.java +++ b/src/main/java/frc/robot/controls/Autocommand.java @@ -28,7 +28,7 @@ public class Autocommand extends SequentialCommandGroup { NamedCommands.registerCommand("start_intake_timelimit", timedIntake(superstructure)); NamedCommands.registerCommand("stop_intake", idle(superstructure)); NamedCommands.registerCommand("intaker_down", lowerIntaker(intakeRotater)); - NamedCommands.registerCommand("intaker_up", idle(superstructure)); + NamedCommands.registerCommand("intaker_up", raiseIntaker(intakeRotater)); NamedCommands.registerCommand("shoot_hub", shootHub(superstructure)); NamedCommands.registerCommand("Shake", shake(driveControls)); NamedCommands.registerCommand("AutoAim", autoAim(driveControls)); @@ -54,6 +54,10 @@ public class Autocommand extends SequentialCommandGroup { return intakeRotater.lowerForMatchCommand(); } + public static Command raiseIntaker(IntakeRotaterSubsystem intakeRotater) { + return intakeRotater.raiseForMatchCommand(); + } + // 自动射击和手动射击使用同一套 gated shooting 流程。 public static Command shootHub(Superstructure superstructure) { return Commands.sequence( diff --git a/src/main/java/frc/robot/controls/OperatorControls.java b/src/main/java/frc/robot/controls/OperatorControls.java index 73c4f50..eecbed6 100644 --- a/src/main/java/frc/robot/controls/OperatorControls.java +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -50,8 +50,8 @@ public class OperatorControls { return superstructure.requestManualCommand(); } - public void adjustFlywheelSpeedOffset(double offset) { - superstructure.adjustFlywheelSpeedOffset(offset); + public void adjustFlywheelVoltageOffset(double offset) { + superstructure.adjustFlywheelVoltageOffset(offset); } public void adjustBackboardRateOffset(double offset) { @@ -74,6 +74,10 @@ public class OperatorControls { return Commands.runEnd(intakeRotater::manualLower, intakeRotater::stop, intakeRotater); } + public Command lowerIntakeRotaterForMatchCommand(IntakeRotaterSubsystem intakeRotater) { + return intakeRotater.lowerForMatchCommand(); + } + // PathPlanner 自动射击辅助：先预备到所有射击条件满足，再请求喂球， // 最后回到空闲状态。真正的“是否能射”判断仍然放在 Superstructure。 public Command autoShootToHubCommand() { diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 27878ab..b55e9b6 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -81,8 +81,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Intaker, 0); positioningNetworkTable = NetworkTableInstance.getDefault().getTable("Positioning"); driveNetworkTable = NetworkTableInstance.getDefault().getTable("Drive"); - ll_shooter_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-shooter"); - ll_intaker_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-intaker"); + ll_shooter_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-left"); + ll_intaker_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-front"); configueAutoBuilder(); } diff --git a/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java b/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java index 7e86379..aec95ef 100644 --- a/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java @@ -1,11 +1,12 @@ package frc.robot.subsystems; -import com.ctre.phoenix6.controls.VelocityVoltage; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.math.MathUtil; import frc.robot.Constants; /** @@ -13,31 +14,31 @@ import frc.robot.Constants; */ public class ConveyorSubsystem extends SubsystemBase { public final TalonFX mainConveyor; - private final VelocityVoltage velocityRequest = new VelocityVoltage(0); + private final VoltageOut voltageRequest = new VoltageOut(0); private final NetworkTable table = NetworkTableInstance.getDefault().getTable("Conveyor"); - private double targetSpeedRps = 0.0; + private double targetOutputVolts = 0.0; public ConveyorSubsystem() { mainConveyor = new TalonFX(Constants.Conveyor.mainConveyorID); mainConveyor.getConfigurator().apply(Constants.Conveyor.mainConveyorSlot0Configs); } public void feedToShooter() { - setSpeedByRPS(Constants.Conveyor.feedSpeedRps); + setVoltage(Constants.Conveyor.feedVoltage); } public void reverse() { - setSpeedByRPS(Constants.Conveyor.reverseSpeedRps); + setVoltage(Constants.Conveyor.reverseVoltage); } public void stop() { - setSpeedByRPS(0.0); + setVoltage(0.0); } - public void setSpeedByRPS(double targetSpeedRps) { - this.targetSpeedRps = targetSpeedRps; - mainConveyor.setControl(velocityRequest.withVelocity(targetSpeedRps)); + public void setVoltage(double outputVolts) { + targetOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0); + mainConveyor.setControl(voltageRequest.withOutput(targetOutputVolts)); } @Override public void periodic() { table.getEntry("velocity").setDouble(mainConveyor.getVelocity().getValueAsDouble()); - table.getEntry("targetSpeedRps").setDouble(targetSpeedRps); + table.getEntry("targetOutputVolts").setDouble(targetOutputVolts); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeRollerSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeRollerSubsystem.java index 87dc227..6c97e03 100644 --- a/src/main/java/frc/robot/subsystems/IntakeRollerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeRollerSubsystem.java @@ -1,10 +1,11 @@ package frc.robot.subsystems; 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.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; @@ -17,13 +18,13 @@ import frc.robot.Constants; * 本类只负责成对 TalonFX 输出和 follower 关系。 */ public class IntakeRollerSubsystem extends SubsystemBase { - private final TalonFX leftIntakeRoller; + public final TalonFX leftIntakeRoller; private final TalonFX rightIntakeRoller; - private final VelocityVoltage velocityRequest = new VelocityVoltage(0); + private final VoltageOut voltageRequest = new VoltageOut(0); private final Follower rightFollower; private final NetworkTable table = NetworkTableInstance.getDefault().getTable("IntakeRoller"); - private double targetSpeedRps = 0.0; + private double targetOutputVolts = 0.0; public IntakeRollerSubsystem() { leftIntakeRoller = new TalonFX(Constants.Intaker.leftIntakeRollerID); @@ -35,20 +36,20 @@ public class IntakeRollerSubsystem extends SubsystemBase { } public void intake() { - setSpeedByRPS(Constants.Intaker.intakeRollerIntakeSpeedRps); + setVoltage(Constants.Intaker.intakeRollerIntakeVoltage); } public void outtake() { - setSpeedByRPS(Constants.Intaker.intakeRollerOuttakeSpeedRps); + setVoltage(Constants.Intaker.intakeRollerOuttakeVoltage); } public void stop() { - setSpeedByRPS(0.0); + setVoltage(0.0); } - public void setSpeedByRPS(double targetSpeedRps) { - this.targetSpeedRps = targetSpeedRps; - leftIntakeRoller.setControl(velocityRequest.withVelocity(targetSpeedRps)); + public void setVoltage(double outputVolts) { + targetOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0); + leftIntakeRoller.setControl(voltageRequest.withOutput(targetOutputVolts)); followLeft(); } @@ -67,6 +68,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); + table.getEntry("targetOutputVolts").setDouble(targetOutputVolts); } } diff --git a/src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java index 2fa9511..4ca66cc 100644 --- a/src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java @@ -1,7 +1,6 @@ package frc.robot.subsystems; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.MotorAlignmentValue; @@ -18,19 +17,17 @@ import frc.robot.Constants; /** * Intaker rotation control for the 2026 fuel path. * - * The normal match posture is lowered. During shooting, the arm alternates - * between lowered and an upward assist position to help fuel enter the chamber. + * The rotater uses timed voltage actions only. Position is published for + * telemetry, but it is not used for closed-loop control. */ public class IntakeRotaterSubsystem extends SubsystemBase { - private final TalonFX leftIntakeRotater; + public final TalonFX leftIntakeRotater; private final TalonFX rightIntakeRotater; - private final PositionVoltage positionRequest = new PositionVoltage(0); private final VoltageOut voltageRequest = new VoltageOut(0); private final Follower rightFollower; private final NetworkTable table = NetworkTableInstance.getDefault().getTable("IntakeRotater"); - private double targetPosition = Constants.Intaker.intakeRotaterLoweredPosition; private double outputVoltage = 0.0; public IntakeRotaterSubsystem() { @@ -46,51 +43,70 @@ public class IntakeRotaterSubsystem extends SubsystemBase { } public void lowerForMatch() { - setPosition(Constants.Intaker.intakeRotaterLoweredPosition); + setVoltage(0.05); } public Command lowerForMatchCommand() { - return Commands.runOnce(this::lowerForMatch, this); + return Commands.sequence( + Commands.runOnce(() -> setVoltage(-1.5), this), + Commands.waitSeconds(0.7), + Commands.runOnce(() -> setVoltage(0.05), this), + Commands.waitSeconds(0.7), + Commands.runOnce(this::stop, this) + ); + } + + public void raiseForMatch() { + setVoltage(0.8); + } + + public Command raiseForMatchCommand() { + return Commands.sequence( + Commands.runOnce(this::raiseForMatch, this), + Commands.waitSeconds(0.5), + Commands.runOnce(this::stop, this) + ); } public void shootAssist() { double period = Constants.Intaker.intakeRotaterShootAssistPeriodSeconds; double phase = (Timer.getFPGATimestamp() % period) / period; - double target = phase < Constants.Intaker.intakeRotaterShootAssistUpDutyCycle - ? Constants.Intaker.intakeRotaterShootAssistUpPosition - : Constants.Intaker.intakeRotaterLoweredPosition; - setPosition(target); + double volts = phase < Constants.Intaker.intakeRotaterShootAssistUpDutyCycle + ? Constants.Intaker.intakeRotaterRaiseVoltage + : Constants.Intaker.intakeRotaterLowerVoltage; + setVoltage(volts); } public void manualRaise() { - setVoltage(Constants.Intaker.intakeRotaterManualRaiseVoltage); + setVoltage(Constants.Intaker.intakeRotaterRaiseVoltage); } public void manualLower() { - setVoltage(Constants.Intaker.intakeRotaterManualLowerVoltage); + setVoltage(Constants.Intaker.intakeRotaterLowerVoltage); } public void stop() { setVoltage(0.0); } - public boolean atGoal() { - return Math.abs(getPosition() - targetPosition) <= Constants.Intaker.positionToleranceRotations; + public boolean isRunning() { + return Math.abs(outputVoltage) > 1e-6; } public double getPosition() { return leftIntakeRotater.getPosition().getValueAsDouble(); } - private void setPosition(double rotations) { - targetPosition = MathUtil.clamp( - rotations, - Constants.Intaker.intakeRotaterDownLimit, - Constants.Intaker.intakeRotaterUpLimit - ); - outputVoltage = 0.0; - leftIntakeRotater.setControl(positionRequest.withPosition(targetPosition)); - followLeft(); + public double getLeftVelocity() { + return leftIntakeRotater.getVelocity().getValueAsDouble(); + } + + public double getRightVelocity() { + return rightIntakeRotater.getVelocity().getValueAsDouble(); + } + + private double getVelocityDifference() { + return Math.abs(getLeftVelocity()) - Math.abs(getRightVelocity()); } private void setVoltage(double volts) { @@ -106,8 +122,10 @@ public class IntakeRotaterSubsystem extends SubsystemBase { @Override public void periodic() { table.getEntry("position").setDouble(getPosition()); - table.getEntry("targetPosition").setDouble(targetPosition); - table.getEntry("atGoal").setBoolean(atGoal()); + table.getEntry("isRunning").setBoolean(isRunning()); table.getEntry("outputVoltage").setDouble(outputVoltage); + table.getEntry("leftVelocity").setDouble(getLeftVelocity()); + table.getEntry("rightVelocity").setDouble(getRightVelocity()); + table.getEntry("velocityDifference").setDouble(getVelocityDifference()); } } diff --git a/src/main/java/frc/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/robot/subsystems/LimelightSubsystem.java index 35518bc..7d0c338 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LimelightSubsystem.java @@ -47,8 +47,8 @@ public class LimelightSubsystem extends SubsystemBase { return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName).pose; } - public double getShooterSetpointByDistance() { + public double getShooterVoltageByDistance() { // 查 Constants 里的插值射表；真车试射后往表里加实测点即可。 - return Constants.Shooter.flywheelRpsByDistance.get(getDistanceToTarget()); + return Constants.Shooter.flywheelVoltageByDistance.get(getDistanceToTarget()); } } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 5db3aa0..2495d54 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -3,7 +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; import com.ctre.phoenix6.signals.InvertedValue; @@ -15,6 +15,7 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -23,8 +24,8 @@ import frc.robot.Constants; /** * Shooter 总成的硬件封装。 * - * 飞轮和 shooter 内部 conveyor 都使用电压开环输出， - * 右侧电机通过反向 follower 跟随左侧电机。 + * 飞轮和 shooter 内部 conveyor 使用 VoltageOut 开环电压输出； + * shooter 内部 conveyor 仍由右侧电机反向 follower 跟随左侧电机。 * 背板控制沿用旧代码中的电机、编码器和 PID 调参流程。 * 何时 spin up、何时喂球由 Superstructure 决定，本类不做比赛动作判断。 */ @@ -38,17 +39,24 @@ public class ShooterSubsystem extends SubsystemBase { public final Encoder backboardEncoder; public final ProfiledPIDController backboardPID; - private final VelocityVoltage flywheelVelocityRequest = new VelocityVoltage(0); - private final VelocityVoltage conveyorVelocityRequest = new VelocityVoltage(0); - private final VelocityVoltage backboardVelocityRequest = new VelocityVoltage(0); + private final VoltageOut leftFlywheelVoltageRequest = new VoltageOut(0); + private final VoltageOut rightFlywheelVoltageRequest = new VoltageOut(0); + private final VoltageOut conveyorVoltageRequest = new VoltageOut(0); + private final VoltageOut backboardVoltageRequest = new VoltageOut(0); - private final Follower flywheelFollower; private final Follower conveyorFollower; private final NetworkTable shooterNetworkTable; + private final NetworkTable backboardTuningTable; - private double targetFlywheelRps = 0.0; - private double shooterConveyorTargetRps = 0.0; + private double flywheelOutputVolts = 0.0; + private double flywheelVoltageStartTimestamp = 0.0; + private double shooterConveyorOutputVolts = 0.0; + private double backboardOutputVolts = 0.0; + private double backboardRawPidOutputVolts = 0.0; + private double backboardClampedPidOutputVolts = 0.0; + private boolean backboardOutputSaturated = false; + private boolean backboardMinimumOutputActive = false; public ShooterSubsystem() { leftFlywheel = new TalonFX(Constants.Shooter.leftFlywheelID); @@ -66,9 +74,8 @@ 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); - setRightFollowLeft(); + setRightConveyorFollowLeft(); backboardMotor = new TalonFXS(Constants.Shooter.backboardMotorID); backboardMotor.getConfigurator().apply(Constants.Shooter.backboardSlot0Configs); @@ -87,15 +94,17 @@ public class ShooterSubsystem extends SubsystemBase { backboardPID.setTolerance(10.0); shooterNetworkTable = NetworkTableInstance.getDefault().getTable("Shooter"); + backboardTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/backboard"); + backboardTuningTable.getEntry("backboardPositionEnable").setDefaultBoolean(false); + backboardTuningTable.getEntry("backboardTargetPosition").setDefaultDouble(0.0); } /* ====================== */ /* 飞轮 */ /* ====================== */ - public void setRightFollowLeft() { + public void setRightConveyorFollowLeft() { // 成对机构机械方向相反，因此右侧电机以反向 follower 跟随左侧电机。 - rightFlywheel.setControl(flywheelFollower); rightConveyor.setControl(conveyorFollower); } @@ -105,37 +114,32 @@ public class ShooterSubsystem extends SubsystemBase { rightFlywheel.getConfigurator().apply(configs); } - public void setFlywheelVelocity(double targetRps) { - // 右飞轮是 follower，因此对外只暴露一个飞轮目标速度。 - // 左飞轮使用 VelocityVoltage 速度闭环，右飞轮反向跟随左飞轮。 - setFlywheelSpeedByRPS(targetRps); + public void setFlywheelVoltage(double outputVolts) { + double clampedOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0); + if (Math.abs(clampedOutputVolts - flywheelOutputVolts) > 1e-6) { + flywheelVoltageStartTimestamp = Timer.getFPGATimestamp(); + } + flywheelOutputVolts = clampedOutputVolts; + leftFlywheel.setControl(leftFlywheelVoltageRequest.withOutput(flywheelOutputVolts)); + rightFlywheel.setControl(rightFlywheelVoltageRequest.withOutput(-flywheelOutputVolts)); } - public void setFlywheelSpeedByRPS(double targetRps) { - targetFlywheelRps = Math.abs(targetRps); - leftFlywheel.setControl(flywheelVelocityRequest.withVelocity(targetFlywheelRps)); - setRightFollowLeft(); - } - - public void setFlywheelVelocityByDistance(double distance) { - // 查 Constants 里的射表，两端自动取边界值。 - double targetRps = Constants.Shooter.flywheelRpsByDistance.get(distance); - setFlywheelVelocity(targetRps); + public void setFlywheelVoltageByDistance(double distance) { + double targetVolts = Constants.Shooter.flywheelVoltageByDistance.get(distance); + setFlywheelVoltage(targetVolts); } public void stopFlywheel() { - setFlywheelSpeedByRPS(0.0); + setFlywheelVoltage(0.0); } - public boolean atSpeed() { - // 虽然飞轮控制是电压开环，但仍读取 TalonFX 速度作为喂球门控。 - if (targetFlywheelRps <= 1.0) { + public boolean atVoltage() { + if (Math.abs(flywheelOutputVolts) < Constants.Shooter.flywheelReadyVoltageThreshold) { return false; } - return Math.abs(Math.abs(getLeftFlywheelVelocity()) - targetFlywheelRps) <= Constants.Shooter.flywheelSpeedToleranceRps - && Math.abs(Math.abs(getRightFlywheelVelocity()) - targetFlywheelRps) <= Constants.Shooter.flywheelSpeedToleranceRps; + return Timer.getFPGATimestamp() - flywheelVoltageStartTimestamp + >= Constants.Shooter.flywheelReadyDelaySeconds; } - public double getLeftFlywheelVelocity() { return leftFlywheel.getVelocity().getValueAsDouble(); } @@ -152,22 +156,22 @@ public class ShooterSubsystem extends SubsystemBase { return Math.abs(getLeftFlywheelVelocity()) - Math.abs(getRightFlywheelVelocity()); } - public double getTargetFlywheelRps() { - return targetFlywheelRps; + public double getTargetFlywheelVoltage() { + return flywheelOutputVolts; } /* ====================== */ /* shooter conveyor */ /* ====================== */ - public void runShooterConveyorByRPS(double targetRps) { - shooterConveyorTargetRps = targetRps; - leftConveyor.setControl(conveyorVelocityRequest.withVelocity(targetRps)); - setRightFollowLeft(); + public void runShooterConveyorVoltage(double outputVolts) { + shooterConveyorOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0); + leftConveyor.setControl(conveyorVoltageRequest.withOutput(shooterConveyorOutputVolts)); + setRightConveyorFollowLeft(); } public void stopShooterConveyor() { - runShooterConveyorByRPS(0.0); + runShooterConveyorVoltage(0.0); } public double getConveyorStatorCurrent() { @@ -178,13 +182,13 @@ public class ShooterSubsystem extends SubsystemBase { /* 背板 */ /* ====================== */ - public void setBackboardSpeedByRPS(double speed) { - if (speed > 0 && speed < 10) { - speed = 10; - } else if (speed < 0 && speed > -10) { - speed = -10; - } - backboardMotor.setControl(backboardVelocityRequest.withVelocity(speed)); + public void setBackboardVoltage(double outputVolts) { + backboardOutputVolts = MathUtil.clamp( + outputVolts, + -Constants.Shooter.backboardSpeedMax, + Constants.Shooter.backboardSpeedMax + ); + backboardMotor.setControl(backboardVoltageRequest.withOutput(backboardOutputVolts)); } public void setBackboardPosition(double targetPosition) { @@ -199,29 +203,47 @@ public class ShooterSubsystem extends SubsystemBase { public void outputBackboard() { // 背板仍使用旧外部编码器和 ProfiledPID；Superstructure 只选择目标位置。 - double pidOutput = backboardPID.calculate(getBackboardPosition()); - pidOutput = MathUtil.clamp(pidOutput, -Constants.Shooter.backboardSpeedMax, Constants.Shooter.backboardSpeedMax); + double rawPidOutput = backboardPID.calculate(getBackboardPosition()); + double pidOutput = MathUtil.clamp( + rawPidOutput, + -Constants.Shooter.backboardSpeedMax, + Constants.Shooter.backboardSpeedMax + ); + backboardRawPidOutputVolts = rawPidOutput; + backboardClampedPidOutputVolts = pidOutput; + backboardOutputSaturated = Math.abs(rawPidOutput - pidOutput) > 1e-9; - if (Math.abs(pidOutput) < 0.8) { - pidOutput = 0.0; + if (pidOutput != 0.0 && Math.abs(pidOutput) < Constants.Shooter.backboardSpeedMin) { + pidOutput = Math.copySign(Constants.Shooter.backboardSpeedMin, pidOutput); + backboardMinimumOutputActive = true; + } else { + backboardMinimumOutputActive = false; } shooterNetworkTable.getEntry("backboardPIDOutput").setDouble(pidOutput); - setBackboardSpeedByRPS(pidOutput); + setBackboardVoltage(pidOutput); } public void holdBackboardAt(double targetPosition) { // 上层只给目标位置；到位后停止输出，未到位时继续闭环。 setBackboardPosition(targetPosition); if (isBackboardAtTarget()) { - backboardMotor.set(0.0); + backboardRawPidOutputVolts = 0.0; + backboardClampedPidOutputVolts = 0.0; + backboardOutputSaturated = false; + backboardMinimumOutputActive = false; + setBackboardVoltage(0.0); } else { outputBackboard(); } } public void stopBackboard() { - backboardMotor.set(0.0); + backboardRawPidOutputVolts = 0.0; + backboardClampedPidOutputVolts = 0.0; + backboardOutputSaturated = false; + backboardMinimumOutputActive = false; + setBackboardVoltage(0.0); } public double getBackboardPosition() { @@ -259,20 +281,62 @@ public class ShooterSubsystem extends SubsystemBase { stopBackboard(); } + public void debugBackboardTargetPeriodic() { + boolean positionEnable = backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false); + double requestedTarget = backboardTuningTable.getEntry("backboardTargetPosition").getDouble(0.0); + double clampedTarget = MathUtil.clamp( + requestedTarget, + Constants.Shooter.backboardDownLimit, + Constants.Shooter.backboardUpLimit + ); + + backboardTuningTable.getEntry("backboardPositionEnableActive").setBoolean(positionEnable); + backboardTuningTable.getEntry("backboardCommandedTargetPosition").setDouble(requestedTarget); + backboardTuningTable.getEntry("backboardClampedTargetPosition").setDouble(clampedTarget); + + if (positionEnable) { + holdBackboardAt(clampedTarget); + } + } + @Override public void periodic() { backboardPID.calculate(getBackboardPosition()); - shooterNetworkTable.getEntry("flywheelSpeed").setDouble(getAverageFlywheelVelocity()); - shooterNetworkTable.getEntry("leftFlywheelSpeed").setDouble(getLeftFlywheelVelocity()); - shooterNetworkTable.getEntry("rightFlywheelSpeed").setDouble(getRightFlywheelVelocity()); - shooterNetworkTable.getEntry("flywheelSpeedDifference").setDouble(getFlywheelVelocityDifference()); - shooterNetworkTable.getEntry("flywheelTargetSpeed").setDouble(targetFlywheelRps); - shooterNetworkTable.getEntry("flywheelAtSpeed").setBoolean(atSpeed()); - shooterNetworkTable.getEntry("shooterConveyorTargetRps").setDouble(shooterConveyorTargetRps); - shooterNetworkTable.getEntry("conveyorSpeed").setDouble(leftConveyor.getVelocity().getValueAsDouble()); - shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition()); - shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position); + double backboardPosition = getBackboardPosition(); + double backboardGoalPosition = backboardPID.getGoal().position; + double backboardSetpointPosition = backboardPID.getSetpoint().position; + shooterNetworkTable.getEntry("flywheelVelocity").setDouble(getAverageFlywheelVelocity()); + shooterNetworkTable.getEntry("leftFlywheelVelocity").setDouble(getLeftFlywheelVelocity()); + shooterNetworkTable.getEntry("rightFlywheelVelocity").setDouble(getRightFlywheelVelocity()); + shooterNetworkTable.getEntry("flywheelVelocityDifference").setDouble(getFlywheelVelocityDifference()); + shooterNetworkTable.getEntry("flywheelTargetVoltage").setDouble(getTargetFlywheelVoltage()); + shooterNetworkTable.getEntry("flywheelOutputVolts").setDouble(flywheelOutputVolts); + shooterNetworkTable.getEntry("flywheelAtVoltage").setBoolean(atVoltage()); + shooterNetworkTable.getEntry("shooterConveyorOutputVolts").setDouble(shooterConveyorOutputVolts); + shooterNetworkTable.getEntry("conveyorVelocity").setDouble(leftConveyor.getVelocity().getValueAsDouble()); + shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(backboardPosition); + shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardSetpointPosition); + shooterNetworkTable.getEntry("backboardOutputVolts").setDouble(backboardOutputVolts); shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget()); + shooterNetworkTable.getEntry("backboardGoalPosition").setDouble(backboardGoalPosition); + shooterNetworkTable.getEntry("backboardSetpointPosition").setDouble(backboardSetpointPosition); + shooterNetworkTable.getEntry("backboardSetpointVelocity").setDouble(backboardPID.getSetpoint().velocity); + shooterNetworkTable.getEntry("backboardGoalError").setDouble(backboardGoalPosition - backboardPosition); + shooterNetworkTable.getEntry("backboardSetpointError").setDouble(backboardSetpointPosition - backboardPosition); + shooterNetworkTable.getEntry("backboardRawPIDOutput").setDouble(backboardRawPidOutputVolts); + shooterNetworkTable.getEntry("backboardClampedPIDOutput").setDouble(backboardClampedPidOutputVolts); + shooterNetworkTable.getEntry("backboardOutputSaturated").setBoolean(backboardOutputSaturated); + shooterNetworkTable.getEntry("backboardMinimumOutputActive").setBoolean(backboardMinimumOutputActive); + shooterNetworkTable.getEntry("backboardMaxOutputVolts").setDouble(Constants.Shooter.backboardSpeedMax); + shooterNetworkTable.getEntry("backboardMinOutputVolts").setDouble(Constants.Shooter.backboardSpeedMin); + shooterNetworkTable.getEntry("backboardPositionTolerance").setDouble(10.0); + shooterNetworkTable.getEntry("backboardShootPosition").setDouble(Constants.Superstructure.backboardShootPosition); + shooterNetworkTable.getEntry("backboardPassPosition").setDouble(Constants.Superstructure.passBallBackboardPosition); + shooterNetworkTable.getEntry("backboardMotorPosition").setDouble(backboardMotor.getPosition().getValueAsDouble()); + shooterNetworkTable.getEntry("backboardMotorVelocity").setDouble(backboardMotor.getVelocity().getValueAsDouble()); + shooterNetworkTable.getEntry("backboardMotorVoltage").setDouble(backboardMotor.getMotorVoltage().getValueAsDouble()); + shooterNetworkTable.getEntry("backboardMotorStatorCurrent").setDouble(backboardMotor.getStatorCurrent().getValueAsDouble()); shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent()); } } + diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 8660463..966141e 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -52,7 +52,7 @@ public class Superstructure extends SubsystemBase { private WantedState wantedState = WantedState.IDLE; private SystemState systemState = SystemState.STOWED; private double stateStartTimestamp = Timer.getFPGATimestamp(); - private double flywheelSpeedOffsetRps = 0.0; + private double flywheelVoltageOffset = 0.0; private double backboardPositionOffset = 0.0; private boolean shotCompletedThisRequest = false; @@ -164,7 +164,7 @@ public class Superstructure extends SubsystemBase { public boolean canShoot() { boolean stationaryGate = !Constants.Superstructure.stationaryGateEnabled || isDriveStationary(); - return shooter.atSpeed() + return shooter.atVoltage() && drive.isAimed() && stationaryGate; } @@ -179,8 +179,8 @@ public class Superstructure extends SubsystemBase { /* 现场微调 offset */ /* ====================== */ - public void adjustFlywheelSpeedOffset(double offsetRps) { - flywheelSpeedOffsetRps += offsetRps; + public void adjustFlywheelVoltageOffset(double offsetVolts) { + flywheelVoltageOffset += offsetVolts; } public void adjustBackboardPositionOffset(double offset) { @@ -188,7 +188,7 @@ public class Superstructure extends SubsystemBase { } public void resetOffsets() { - flywheelSpeedOffsetRps = 0.0; + flywheelVoltageOffset = 0.0; backboardPositionOffset = 0.0; } @@ -227,6 +227,7 @@ public class Superstructure extends SubsystemBase { // 安全默认状态：球路不动，intaker 不主动动作，背板保持受控。 conveyor.stop(); intakeRoller.stop(); + intakeRotater.stop(); shooter.stopFlywheel(); shooter.stopShooterConveyor(); shooter.holdBackboardAt(0.0); @@ -253,18 +254,18 @@ public class Superstructure extends SubsystemBase { } private void handleShootHub() { - double targetRps = limelight.getShooterSetpointByDistance() + flywheelSpeedOffsetRps; + double targetVolts = limelight.getShooterVoltageByDistance() + flywheelVoltageOffset; double backboardPosition = Constants.Superstructure.backboardShootPosition + backboardPositionOffset; - handlePreparedFeed(targetRps, backboardPosition); + handlePreparedFeed(targetVolts, backboardPosition); } private void handlePassBall() { - double targetRps = Constants.Superstructure.passBallFlywheelRps + flywheelSpeedOffsetRps; + double targetVolts = Constants.Superstructure.passBallFlywheelVoltage + flywheelVoltageOffset; double backboardPosition = Constants.Superstructure.passBallBackboardPosition + backboardPositionOffset; - handlePreparedFeed(targetRps, backboardPosition); + handlePreparedFeed(targetVolts, backboardPosition); } - private void handlePreparedFeed(double targetRps, double backboardPosition) { + private void handlePreparedFeed(double targetVolts, double backboardPosition) { if (systemState != SystemState.PREP_SHOOT && systemState != SystemState.SHOOTING) { setSystemState(SystemState.PREP_SHOOT); @@ -274,7 +275,7 @@ public class Superstructure extends SubsystemBase { shooter.stopShooterConveyor(); // 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调， // 但不会绕过 Superstructure 状态机。 - shooter.setFlywheelVelocity(targetRps); + shooter.setFlywheelVoltage(targetVolts); shooter.holdBackboardAt(backboardPosition); if (systemState == SystemState.SHOOTING) { @@ -282,7 +283,7 @@ public class Superstructure extends SubsystemBase { // 避免球接触飞轮导致的掉速中断喂球。 intakeRotater.shootAssist(); conveyor.feedToShooter(); - shooter.runShooterConveyorByRPS(Constants.Superstructure.shooterFeedSpeedRps); + shooter.runShooterConveyorVoltage(Constants.Superstructure.shooterFeedVoltage); if (timeInState() > Constants.Superstructure.shootTimeoutSeconds) { shotCompletedThisRequest = true; @@ -306,7 +307,7 @@ public class Superstructure extends SubsystemBase { setSystemState(SystemState.EJECTING); intakeRoller.outtake(); conveyor.reverse(); - shooter.runShooterConveyorByRPS(Constants.Superstructure.shooterReverseSpeedRps); + shooter.runShooterConveyorVoltage(Constants.Superstructure.shooterReverseVoltage); shooter.stopFlywheel(); shooter.holdBackboardAt(0.0); } @@ -345,11 +346,11 @@ public class Superstructure extends SubsystemBase { table.getEntry("systemState").setString(systemState.name()); table.getEntry("timeInState").setDouble(timeInState()); table.getEntry("canShoot").setBoolean(canShoot()); - table.getEntry("shooterAtSpeed").setBoolean(shooter.atSpeed()); + 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("flywheelSpeedOffsetRps").setDouble(flywheelSpeedOffsetRps); + table.getEntry("flywheelVoltageOffset").setDouble(flywheelVoltageOffset); 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 47132a7..447cd0c 100644 --- a/src/main/java/frc/robot/tuning/ConfigTalonFXMotorTuner.java +++ b/src/main/java/frc/robot/tuning/ConfigTalonFXMotorTuner.java @@ -2,8 +2,9 @@ package frc.robot.tuning; import java.util.function.Function; import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.controls.VelocityDutyCycle; +import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.networktables.*; public class ConfigTalonFXMotorTuner { @@ -22,10 +23,10 @@ public class ConfigTalonFXMotorTuner { private final DoubleSubscriber KsSub; private final BooleanSubscriber enableRuningSub; - private final DoubleSubscriber runningSpeedRPS; + private final DoubleSubscriber runningVolts; private double lastKp = 0, lastKi = 0, lastKd = 0, lastKv = 0, lastKa = 0, lastKs = 0; - private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0); + private final VoltageOut voltageRequest = new VoltageOut(0); public ConfigTalonFXMotorTuner(TalonFX motor_, String motorName_, Slot0Configs motorSlot0Configs_) { this.motor = motor_; this.motorName = motorName_; @@ -42,7 +43,7 @@ public class ConfigTalonFXMotorTuner { table.getDoubleTopic(motorName+"kS").publish().set(motorSlot0Configs.kS); table.getBooleanTopic(motorName+"EnableRunning").publish().set(false);; - table.getDoubleTopic(motorName+"RunningSpeedRPS").publish().set(0);; + table.getDoubleTopic(motorName+"RunningVolts").publish().set(0);; this.enableSub = table.getBooleanTopic(motorName+"Enable").subscribe(false); this.KpSub = table.getDoubleTopic(motorName+"kP").subscribe(motorSlot0Configs.kP); @@ -52,7 +53,7 @@ public class ConfigTalonFXMotorTuner { this.KaSub = table.getDoubleTopic(motorName+"kA").subscribe(motorSlot0Configs.kA); this.KsSub = table.getDoubleTopic(motorName+"kS").subscribe(motorSlot0Configs.kS); this.enableRuningSub = table.getBooleanTopic(motorName+"EnableRunning").subscribe(false); - this.runningSpeedRPS = table.getDoubleTopic(motorName+"RunningSpeedRPS").subscribe(0); + this.runningVolts = table.getDoubleTopic(motorName+"RunningVolts").subscribe(0); } public void periodic(Runnable func) { @@ -80,11 +81,11 @@ public class ConfigTalonFXMotorTuner { } if (enableRuningSub.get()) {; - double speed = runningSpeedRPS.get(); - motor.setControl(velocityRequest.withVelocity(speed)); - // System.out.println("running motor with speed: "+speed+" on motor: "+motorName); + double volts = MathUtil.clamp(runningVolts.get(), -12.0, 12.0); + motor.setControl(voltageRequest.withOutput(volts)); + // System.out.println("running motor with volts: "+volts+" on motor: "+motorName); }else{ - motor.setControl(velocityRequest.withVelocity(0)); + motor.setControl(voltageRequest.withOutput(0)); } } diff --git a/src/main/java/frc/robot/tuning/ConfigTalonFXSMotorTuner.java b/src/main/java/frc/robot/tuning/ConfigTalonFXSMotorTuner.java index d409418..9e25bf6 100644 --- a/src/main/java/frc/robot/tuning/ConfigTalonFXSMotorTuner.java +++ b/src/main/java/frc/robot/tuning/ConfigTalonFXSMotorTuner.java @@ -1,7 +1,6 @@ package frc.robot.tuning; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.controls.VelocityDutyCycle; -import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFXS; import edu.wpi.first.networktables.*;