diff --git a/elastic-layout-debug.json b/elastic-layout-debug.json index 8a09b7b..e6a5cdd 100644 --- a/elastic-layout-debug.json +++ b/elastic-layout-debug.json @@ -1010,19 +1010,6 @@ "data_type": "boolean" } }, - { - "title": "Shooter Conv Enable", - "x": 0.0, - "y": 384.0, - "width": 128.0, - "height": 128.0, - "type": "Toggle Switch", - "properties": { - "topic": "/Tuning/conveyor/conveyorEnable", - "period": 0.06, - "data_type": "boolean" - } - }, { "title": "Backboard Enable", "x": 0.0, @@ -1036,33 +1023,6 @@ "data_type": "boolean" } }, - { - "title": "Backboard Run", - "x": 128.0, - "y": 576.0, - "width": 128.0, - "height": 128.0, - "type": "Toggle Switch", - "properties": { - "topic": "/Tuning/backboard/backboardEnableRunning", - "period": 0.06, - "data_type": "boolean" - } - }, - { - "title": "Backboard Run RPS", - "x": 256.0, - "y": 576.0, - "width": 256.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/Tuning/backboard/backboardRunningSpeedRPS", - "period": 0.06, - "data_type": "double", - "show_submit_button": true - } - }, { "title": "Backboard kP", "x": 512.0, @@ -1147,86 +1107,6 @@ "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, @@ -1275,46 +1155,6 @@ "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, @@ -1366,321 +1206,6 @@ ] } }, - { - "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": { @@ -2051,33 +1576,6 @@ "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, @@ -2180,7 +1678,7 @@ } }, { - "name": "Flywheel Velocity Tuning", + "name": "Flywheel Debug", "grid_layout": { "layouts": [], "containers": [ @@ -2696,354 +2194,8 @@ } } ] - } - }, - { - "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 - } - } - ] - } + }, + "title": "Flywheel Debug" } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f8026fc..4fc8832 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,22 +27,9 @@ public final class Constants { public static final double flywheelReadyDelaySeconds = 0.5; // Distance (meters) -> flywheel velocity (rotations per second). - 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 double shooterConveyorFeedVelocity = 40; + public static final double shooterConveyorReverseVelocity = -10; public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs() .withKP(0.21) @@ -54,7 +41,7 @@ public final class Constants { public static final Slot0Configs conveyorSlot0Configs = new Slot0Configs() .withKP(0.19).withKI(0).withKD(0.001) - .withKV(shooterConveyorNominalKv).withKA(0.0).withKS(0.07); + .withKV(0.125).withKA(0.0).withKS(0.07); public static final Slot0Configs backboardSlot0Configs = new Slot0Configs() .withKP(0.01).withKI(0).withKD(0) @@ -168,8 +155,7 @@ public final class Constants { public static class Superstructure { // State-machine timing and gate tolerances; tune on the real robot. - 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; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3077066..3dcf292 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,9 +36,8 @@ public class RobotContainer { /* 手柄 */ /* ====================== */ - // 使用两个 Xbox 手柄：0 号给底盘驾驶，1 号给机构操作员。 - private final CommandXboxController controllerlower = new CommandXboxController(0); - private final CommandXboxController controllerupper = new CommandXboxController(1); + // 使用一个 Xbox 手柄：0 号口同时负责底盘驾驶和机构操作。 + private final CommandXboxController controller = new CommandXboxController(0); /* ====================== */ /* 子系统 */ @@ -70,7 +69,7 @@ public class RobotContainer { /* 控制封装/调参 */ /* ====================== */ - public final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, superstructure); + public final DriveControls driveControls = new DriveControls(drivetrain, controller, superstructure); // 操作员意图层替代旧的射击/进球分离控制类，让复杂动作判断集中留在 Superstructure。 private final OperatorControls operatorControls = new OperatorControls(superstructure, shooterSubsystem); @@ -78,16 +77,8 @@ public class RobotContainer { private final GameData2026 gameData2026 = new GameData2026(); private ConfigTalonFXMotorTuner configFlywheelTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.leftFlywheel, "flywheel", Constants.Shooter.flyWheelSlot0Configs, false); - private ConfigTalonFXMotorTuner configConveyorTuner = - new ConfigTalonFXMotorTuner(shooterSubsystem.leftConveyor, "conveyor", Constants.Shooter.conveyorSlot0Configs, false); 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); @@ -128,44 +119,12 @@ public class RobotContainer { // 禁用状态下进入空闲命令，防止模块继续响应上一周期的运动请求。 RobotModeTriggers.disabled().whileTrue(driveControls.idleCommand()); - // 左保险：按住刹车。 - controllerlower.leftBumper() - .onTrue(Commands.runOnce(driveControls::emergencyStop)) - .whileTrue(driveControls.brakeWhileHeld()); - - // 右保险：按住开启加速模式，松开后恢复普通速度。 - controllerlower.rightBumper() - .onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true))) - .onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false))); - // Back 键：重置场地坐标系角度。 - controllerlower.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); - - // Start 键：强行使用 shooter 侧 Limelight 的 MegaTag2 全场定位。 - controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightmt2)); - - controllerlower.a().whileTrue(drivetrain.run(() -> { - drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Shooter); - System.out.println("Try using shooter for metatag2"); - })); - controllerlower.b().whileTrue(drivetrain.run(() -> { - drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Intaker); - System.out.println("Try using intaker for metatag2"); - })); + controller.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); // 预留的左摇杆自动瞄准切换，当前保持注释，避免改变驾驶员已习惯的按键。 - // controllerlower.leftStick().onTrue(operatorControls.requestAimHubCommand()); - // controllerlower.leftStick().onFalse(operatorControls.requestIdleCommand()); - - // 上操作员右保险：按住进入自动瞄准模式，松开回到全手动模式。 - Trigger autoAim = controllerupper.rightBumper(); // 可按需要再并入驾驶员左摇杆。 - autoAim.onTrue(operatorControls.requestAimHubCommand()); - autoAim.onFalse(operatorControls.requestIdleCommand()); - - // 上操作员左保险：按住进入传球模式，松开回到全手动模式。 - Trigger autoAim2 = controllerupper.leftBumper(); // 可按需要再并入驾驶员左摇杆。 - autoAim2.onTrue(operatorControls.requestPassBallCommand()); - autoAim2.onFalse(operatorControls.requestIdleCommand()); + // controller.leftStick().onTrue(operatorControls.requestAimHubCommand()); + // controller.leftStick().onFalse(operatorControls.requestIdleCommand()); } @@ -175,18 +134,17 @@ public class RobotContainer { // 这些命令 require Superstructure，与自动程序共用需求； // 用 teleop 门控避免自动赛阶段误触直接取消整个 PathPlanner 自动。 Trigger teleopOnly = RobotModeTriggers.teleop(); - controllerupper.rightTrigger().and(teleopOnly) + controller.rightTrigger().and(teleopOnly) .onTrue(operatorControls.requestShootHubCommand()) .onFalse(operatorControls.requestIdleCommand()); - controllerupper.b().and(teleopOnly) + controller.rightBumper().and(teleopOnly) .onTrue(operatorControls.requestEjectCommand()) .onFalse(operatorControls.requestIdleCommand()); - 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())); - controllerupper.start().onTrue(operatorControls.resetBackboardCommand()); + controller.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(-1.0))); + controller.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(1.0))); + controller.povUp().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(100))); + controller.povDown().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(-100))); + controller.start().onTrue(operatorControls.resetBackboardCommand()); } private void bingdingLED() { @@ -198,16 +156,13 @@ public class RobotContainer { } public void tunerPeriodic() { + boolean allowMotorOutputTuning = DriverStation.isTest(); + 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.debugFlywheelVelocityPeriodic(allowMotorOutputTuning); + shooterSubsystem.debugBackboardTargetPeriodic(allowMotorOutputTuning); shooterSubsystem.setRightFlywheelFollowLeft(); shooterSubsystem.setRightConveyorFollowLeft(); } @@ -219,27 +174,24 @@ public class RobotContainer { private void configueIntaker() { // 进球相关按钮也只请求 Superstructure 状态；intaker 开赛后默认保持下放。 Trigger teleopOnly = RobotModeTriggers.teleop(); - controllerupper.leftTrigger().and(teleopOnly) + controller.leftTrigger().and(teleopOnly) + .onTrue(operatorControls.requestPassBallCommand()) + .onFalse(operatorControls.requestIdleCommand()); + + controller.leftBumper().and(teleopOnly) .onTrue(operatorControls.requestIntakeCommand()) .onFalse(operatorControls.requestIdleCommand()); - Trigger manualRaiseIntaker = controllerupper.x().and(teleopOnly); + Trigger manualRaiseIntaker = controller.y().and(teleopOnly); manualRaiseIntaker .onTrue(operatorControls.requestManualCommand()) .whileTrue(operatorControls.manualRaiseIntakeRotaterCommand(intakeRotaterSubsystem)) .onFalse(operatorControls.requestIdleCommand()); - Trigger manualLowerIntaker = controllerupper.a().and(teleopOnly); + Trigger manualLowerIntaker = controller.a().and(teleopOnly); manualLowerIntaker .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/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index 1a12af4..af1ea8a 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -37,8 +37,8 @@ public class DriveControls { private final double maxAngularRate = RotationsPerSecond.of(0.75).in(edu.wpi.first.units.Units.RadiansPerSecond); // 默认速度比例，普通驾驶使用。 - private double normalDriveScale = 0.3; - private double normalTurnScale = 0.7; + private double normalDriveScale = 1.0; + private double normalTurnScale = 1.0; // 加速模式比例，按住 RB 时使用。 private double boostDriveScale = 1.0; @@ -48,7 +48,7 @@ public class DriveControls { private boolean boostEnabled = false; - private final SlewRateLimiter vxLimiter = new SlewRateLimiter(5.0); // m/s^2 等效加速度限制 + private final SlewRateLimiter vxLimiter = new SlewRateLimiter(3.5); // m/s^2 等效加速度限制 private final SlewRateLimiter vyLimiter = new SlewRateLimiter(5.0); private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 等效角加速度限制 diff --git a/src/main/java/frc/robot/controls/OperatorControls.java b/src/main/java/frc/robot/controls/OperatorControls.java index f81ec03..9660ebf 100644 --- a/src/main/java/frc/robot/controls/OperatorControls.java +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -3,6 +3,7 @@ package frc.robot.controls; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.Constants; +import frc.robot.subsystems.IntakeRollerSubsystem; import frc.robot.subsystems.IntakeRotaterSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.Superstructure; @@ -74,6 +75,24 @@ public class OperatorControls { return Commands.runEnd(intakeRotater::manualRaise, intakeRotater::stop, intakeRotater); } + public Command manualRaiseIntakeWithRollerCommand( + IntakeRotaterSubsystem intakeRotater, + IntakeRollerSubsystem intakeRoller + ) { + return Commands.runEnd( + () -> { + intakeRotater.manualRaise(); + intakeRoller.intake(); + }, + () -> { + intakeRotater.stop(); + intakeRoller.stop(); + }, + intakeRotater, + intakeRoller + ); + } + public Command manualLowerIntakeRotaterCommand(IntakeRotaterSubsystem intakeRotater) { return Commands.runEnd(intakeRotater::manualLower, intakeRotater::stop, intakeRotater); } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 9ed4726..79e4ae8 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -77,7 +77,7 @@ public class TunerConstants { new Pigeon2Configuration() .withGyroTrim( new GyroTrimConfigs() - .withGyroScalarZ(-5.0) + .withGyroScalarZ(-6.7) ) .withPigeon2Features( new Pigeon2FeaturesConfigs() diff --git a/src/main/java/frc/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/robot/subsystems/LimelightSubsystem.java index 2679c07..d9e0d9e 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LimelightSubsystem.java @@ -46,8 +46,4 @@ public class LimelightSubsystem extends SubsystemBase { public Pose2d getEstimatedPose() { return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName).pose; } - - 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 424f9be..6187e8b 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -9,12 +9,14 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.TalonFXS; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ProfiledPIDController; 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.DriverStation; import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -49,7 +51,6 @@ public class ShooterSubsystem extends SubsystemBase { private final NetworkTable shooterNetworkTable; private final NetworkTable flywheelTuningTable; - private final NetworkTable conveyorTuningTable; private final NetworkTable backboardTuningTable; private double flywheelOutputVolts = 0.0; @@ -90,6 +91,7 @@ public class ShooterSubsystem extends SubsystemBase { backboardMotor = new TalonFXS(Constants.Shooter.backboardMotorID); backboardMotor.getConfigurator().apply(Constants.Shooter.backboardSlot0Configs); + setBackboardBrakeMode(); backboardEncoder = new Encoder(0, 1, false, Encoder.EncodingType.k4X); backboardEncoder.setSamplesToAverage(5); @@ -109,9 +111,6 @@ public class ShooterSubsystem extends SubsystemBase { 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); @@ -155,18 +154,14 @@ public class ShooterSubsystem extends SubsystemBase { } public boolean atVelocity() { - if (Math.abs(flywheelTargetVelocity) < Constants.Shooter.flywheelReadyVelocityThreshold) { - flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp(); + double currentvelocity = getAverageFlywheelVelocity(); + if(currentvelocity>flywheelTargetVelocity*0.95){ + return true; + } else { return false; - } - if (Math.abs(getFlywheelVelocityError()) > flywheelVelocityTolerance) { - flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp(); - return false; - } - return Timer.getFPGATimestamp() - flywheelVelocityReadyStartTimestamp - >= Constants.Shooter.flywheelReadyDelaySeconds; + } } - + public double getLeftFlywheelVelocity() { return leftFlywheel.getVelocity().getValueAsDouble(); } @@ -196,11 +191,7 @@ public class ShooterSubsystem extends SubsystemBase { /* ====================== */ public void runShooterConveyorVelocity(double targetVelocityRps) { - shooterConveyorTargetVelocity = MathUtil.clamp( - targetVelocityRps, - -Constants.Shooter.shooterConveyorMaxVelocity, - Constants.Shooter.shooterConveyorMaxVelocity - ); + shooterConveyorTargetVelocity = targetVelocityRps; leftConveyor.setControl(conveyorVelocityRequest.withVelocity(shooterConveyorTargetVelocity)); setRightConveyorFollowLeft(); } @@ -318,6 +309,12 @@ public class ShooterSubsystem extends SubsystemBase { setBackboardVoltage(0.0); } + public void setBackboardBrakeMode() { + var backboardMotorOutputConfigs = new MotorOutputConfigs(); + backboardMotorOutputConfigs.NeutralMode = NeutralModeValue.Brake; + backboardMotor.getConfigurator().apply(backboardMotorOutputConfigs); + } + public double getBackboardPosition() { return backboardEncoder.getDistance(); } @@ -339,17 +336,17 @@ public class ShooterSubsystem extends SubsystemBase { } public boolean isBackboardPositionTuningActive() { - return backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false); + return DriverStation.isTest() + && backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false); } public boolean isFlywheelTuningControlActive() { - boolean velocityTunerEnabled = flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false); - return velocityTunerEnabled; + return DriverStation.isTest() + && flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false); } public boolean isShooterConveyorTuningControlActive() { - boolean velocityTunerEnabled = conveyorTuningTable.getEntry("conveyorVelocityEnable").getBoolean(false); - return velocityTunerEnabled; + return false; } public void resetbackboardencoder() { @@ -379,8 +376,9 @@ public class ShooterSubsystem extends SubsystemBase { stopBackboard(); } - public void debugBackboardTargetPeriodic() { - boolean positionEnable = backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false); + public void debugBackboardTargetPeriodic(boolean allowMotorOutput) { + boolean positionEnable = + allowMotorOutput && backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false); double requestedTarget = backboardTuningTable.getEntry("backboardTargetPosition").getDouble(0.0); double clampedTarget = MathUtil.clamp( requestedTarget, @@ -397,8 +395,9 @@ public class ShooterSubsystem extends SubsystemBase { } } - public void debugFlywheelVelocityPeriodic() { - flywheelVelocityTuningActive = flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false); + public void debugFlywheelVelocityPeriodic(boolean allowMotorOutput) { + flywheelVelocityTuningActive = + allowMotorOutput && flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false); double requestedTargetVelocity = flywheelTuningTable.getEntry("flywheelTargetVelocity").getDouble(Constants.Shooter.defaultFlywheelVelocity); flywheelVelocityTolerance = Math.max( @@ -422,28 +421,6 @@ public class ShooterSubsystem extends SubsystemBase { } } - 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(); @@ -499,4 +476,3 @@ public class ShooterSubsystem extends SubsystemBase { 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 9d424d3..7f5ec82 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -1,5 +1,6 @@ package frc.robot.subsystems; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; @@ -284,8 +285,13 @@ public class Superstructure extends SubsystemBase { } private void handleShootHub() { - double targetVelocity = limelight.getShooterVelocityByDistance() + flywheelVelocityOffset; - double backboardPosition = Constants.Superstructure.backboardShootPosition + backboardPositionOffset; + double x = NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0); + x = MathUtil.clamp(x, 1.2, 4.5); + double rawtargetVelocity = -0.9702766369910669*x*x*x*x*x*x+16.17984114722666*x*x*x*x*x-107.89020547785285*x*x*x*x+365.6895126610877*x*x*x-659.1553030522557*x*x+598.8266122952737*x-188.96203796840197; + double targetVelocity = rawtargetVelocity + flywheelVelocityOffset; + double rawtargetbackboardPosition = +72.63723930800967*x*x*x*x*x*x-1226.7072434356646*x*x*x*x*x+8380.732539436454*x*x*x*x-29585.06108203164*x*x*x+56753.91414329895*x*x-55469.56099616276*x+21418.08191862802; + rawtargetbackboardPosition = MathUtil.clamp(rawtargetbackboardPosition, 0, 2200); + double backboardPosition = rawtargetbackboardPosition + backboardPositionOffset; handlePreparedFeed(targetVelocity, backboardPosition); } @@ -301,8 +307,6 @@ public class Superstructure extends SubsystemBase { setSystemState(SystemState.PREP_SHOOT); } intakeRoller.stop(); - conveyor.stop(); - stopShooterConveyorUnlessTuning(); // 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调， // 但不会绕过 Superstructure 状态机。 shooter.setFlywheelVelocity(targetVelocity); @@ -312,6 +316,7 @@ public class Superstructure extends SubsystemBase { // 只有 canShoot() 通过后才会进入本状态；喂球期间不再复查门控， // 避免球接触飞轮导致的掉速中断喂球。 intakeRotater.shootAssist(); + conveyor.feedToShooter(); runShooterConveyorVelocityUnlessTuning(Constants.Superstructure.shooterFeedVelocity); return; @@ -382,6 +387,9 @@ public class Superstructure extends SubsystemBase { if (systemState != nextState) { systemState = nextState; stateStartTimestamp = Timer.getFPGATimestamp(); + if (nextState == SystemState.SHOOTING) { + shooter.setBackboardBrakeMode(); + } } } diff --git a/src/main/java/frc/robot/tuning/ConfigTalonFXSMotorTuner.java b/src/main/java/frc/robot/tuning/ConfigTalonFXSMotorTuner.java index 9e25bf6..a1afe8b 100644 --- a/src/main/java/frc/robot/tuning/ConfigTalonFXSMotorTuner.java +++ b/src/main/java/frc/robot/tuning/ConfigTalonFXSMotorTuner.java @@ -1,6 +1,5 @@ package frc.robot.tuning; import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFXS; import edu.wpi.first.networktables.*; @@ -20,11 +19,8 @@ public class ConfigTalonFXSMotorTuner { private final DoubleSubscriber KaSub; private final DoubleSubscriber KsSub; - private final BooleanSubscriber enableRuningSub; - private final DoubleSubscriber runningSpeedRPS; private double lastKp = 0, lastKi = 0, lastKd = 0, lastKv = 0, lastKa = 0, lastKs = 0; - - private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0); + public ConfigTalonFXSMotorTuner(TalonFXS motor_, String motorName_, Slot0Configs motorSlot0Configs_) { this.motor = motor_; this.motorName = motorName_; @@ -39,9 +35,6 @@ public class ConfigTalonFXSMotorTuner { table.getDoubleTopic(motorName+"kA").publish().set(motorSlot0Configs.kA); table.getDoubleTopic(motorName+"kS").publish().set(motorSlot0Configs.kS); - table.getBooleanTopic(motorName+"EnableRunning").publish().set(false); - table.getDoubleTopic(motorName+"RunningSpeedRPS").publish().set(0); - this.enableSub = table.getBooleanTopic(motorName+"Enable").subscribe(false); this.KpSub = table.getDoubleTopic(motorName+"kP").subscribe(motorSlot0Configs.kP); this.KiSub = table.getDoubleTopic(motorName+"kI").subscribe(motorSlot0Configs.kI); @@ -49,8 +42,6 @@ public class ConfigTalonFXSMotorTuner { this.KvSub = table.getDoubleTopic(motorName+"kV").subscribe(motorSlot0Configs.kV); 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); } public void periodic() { @@ -73,13 +64,5 @@ public class ConfigTalonFXSMotorTuner { lastKp = kp; lastKi = ki; lastKd = kd; lastKv = kv; lastKa = ka; lastKs = ks; } - - if (enableRuningSub.get()) {; - double speed = runningSpeedRPS.get(); - motor.setControl(velocityRequest.withVelocity(speed)); - // System.out.println("running motor with speed: "+speed+" on motor: "+motorName); - }else{ - motor.setControl(velocityRequest.withVelocity(0)); - } } }