admin6494/FRC6494_2026Regional-offseason
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

7.10晚

Browse Source
This commit is contained in:
OnDYCLaptop
2026-07-10 22:10:06 +08:00
parent 6abdf34cf0
commit d2f5adc813
10 changed files with 94 additions and 1022 deletions
Download Patch File Download Diff File Expand all files Collapse all files
elastic-layout-debug.json
+2 -850
View File
@@ -1010,19 +1010,6 @@
"data_type": "boolean" "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", "title": "Backboard Enable",
"x": 0.0, "x": 0.0,
@@ -1036,33 +1023,6 @@
"data_type": "boolean" "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", "title": "Backboard kP",
"x": 512.0, "x": 512.0,
@@ -1147,86 +1107,6 @@
"show_submit_button": true "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", "title": "Intake Roller Left Speed",
"x": 512.0, "x": 512.0,
@@ -1275,46 +1155,6 @@
"line_width": 2.0 "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", "title": "Intake Rotater Left Speed",
"x": 512.0, "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", "name": "Backboard Debug",
"grid_layout": { "grid_layout": {
@@ -2051,33 +1576,6 @@
"data_type": "boolean" "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", "title": "Talon kP",
"x": 512.0, "x": 512.0,
@@ -2180,7 +1678,7 @@
} }
}, },
{ {
"name": "Flywheel Velocity Tuning", "name": "Flywheel Debug",
"grid_layout": { "grid_layout": {
"layouts": [], "layouts": [],
"containers": [ "containers": [
@@ -2696,354 +2194,8 @@
} }
} }
] ]
}
}, },
{ "title": "Flywheel Debug"
"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
}
}
]
}
} }
] ]
} }
src/main/java/frc/robot/Constants.java
+4 -18
View File
@@ -27,22 +27,9 @@ public final class Constants {
public static final double flywheelReadyDelaySeconds = 0.5; public static final double flywheelReadyDelaySeconds = 0.5;
// Distance (meters) -> flywheel velocity (rotations per second). // 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 shooterConveyorFeedVelocity = 40;
public static final double shooterConveyorReverseVoltage = -2.0; public static final double shooterConveyorReverseVelocity = -10;
public static final double shooterConveyorNominalKv = 0.125;
public static final double shooterConveyorMaxVelocity = 12.0 / shooterConveyorNominalKv;
public static final double shooterConveyorFeedVelocity =
shooterConveyorFeedVoltage / shooterConveyorNominalKv;
public static final double shooterConveyorReverseVelocity =
shooterConveyorReverseVoltage / shooterConveyorNominalKv;
public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs() public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
.withKP(0.21) .withKP(0.21)
@@ -54,7 +41,7 @@ public final class Constants {
public static final Slot0Configs conveyorSlot0Configs = new Slot0Configs() public static final Slot0Configs conveyorSlot0Configs = new Slot0Configs()
.withKP(0.19).withKI(0).withKD(0.001) .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() public static final Slot0Configs backboardSlot0Configs = new Slot0Configs()
.withKP(0.01).withKI(0).withKD(0) .withKP(0.01).withKI(0).withKD(0)
@@ -168,8 +155,7 @@ public final class Constants {
public static class Superstructure { public static class Superstructure {
// State-machine timing and gate tolerances; tune on the real robot. // 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 shooterFeedVelocity = Shooter.shooterConveyorFeedVelocity;
public static final double shooterReverseVelocity = Shooter.shooterConveyorReverseVelocity; public static final double shooterReverseVelocity = Shooter.shooterConveyorReverseVelocity;
public static final double shootTimeoutSeconds = 1.0; public static final double shootTimeoutSeconds = 1.0;
src/main/java/frc/robot/RobotContainer.java
+24 -72
View File
@@ -36,9 +36,8 @@ public class RobotContainer {
/* 手柄 */ /* 手柄 */
/* ====================== */ /* ====================== */
// 使用两个 Xbox 手柄：0 号给底盘驾驶，1 号给机构操作员。 // 使用一个 Xbox 手柄：0 号口同时负责底盘驾驶和机构操作。
private final CommandXboxController controllerlower = new CommandXboxController(0); private final CommandXboxController controller = new CommandXboxController(0);
private final CommandXboxController controllerupper = new CommandXboxController(1);
/* ====================== */ /* ====================== */
/* 子系统 */ /* 子系统 */
@@ -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。 // 操作员意图层替代旧的射击/进球分离控制类，让复杂动作判断集中留在 Superstructure。
private final OperatorControls operatorControls = new OperatorControls(superstructure, shooterSubsystem); private final OperatorControls operatorControls = new OperatorControls(superstructure, shooterSubsystem);
@@ -78,16 +77,8 @@ public class RobotContainer {
private final GameData2026 gameData2026 = new GameData2026(); private final GameData2026 gameData2026 = new GameData2026();
private ConfigTalonFXMotorTuner configFlywheelTuner = private ConfigTalonFXMotorTuner configFlywheelTuner =
new ConfigTalonFXMotorTuner(shooterSubsystem.leftFlywheel, "flywheel", Constants.Shooter.flyWheelSlot0Configs, false); new ConfigTalonFXMotorTuner(shooterSubsystem.leftFlywheel, "flywheel", Constants.Shooter.flyWheelSlot0Configs, false);
private ConfigTalonFXMotorTuner configConveyorTuner =
new ConfigTalonFXMotorTuner(shooterSubsystem.leftConveyor, "conveyor", Constants.Shooter.conveyorSlot0Configs, false);
private ConfigTalonFXSMotorTuner configBackboardTuner = private ConfigTalonFXSMotorTuner configBackboardTuner =
new ConfigTalonFXSMotorTuner(shooterSubsystem.backboardMotor, "backboard", Constants.Shooter.backboardSlot0Configs); 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<Command> autoChooser; private final SendableChooser<Command> autoChooser;
private final Trigger enableTrigger = new Trigger(DriverStation::isEnabled); private final Trigger enableTrigger = new Trigger(DriverStation::isEnabled);
@@ -128,44 +119,12 @@ public class RobotContainer {
// 禁用状态下进入空闲命令，防止模块继续响应上一周期的运动请求。 // 禁用状态下进入空闲命令，防止模块继续响应上一周期的运动请求。
RobotModeTriggers.disabled().whileTrue(driveControls.idleCommand()); 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 键：重置场地坐标系角度。 // Back 键：重置场地坐标系角度。
controllerlower.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); controller.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");
}));
// 预留的左摇杆自动瞄准切换，当前保持注释，避免改变驾驶员已习惯的按键。 // 预留的左摇杆自动瞄准切换，当前保持注释，避免改变驾驶员已习惯的按键。
// controllerlower.leftStick().onTrue(operatorControls.requestAimHubCommand()); // controller.leftStick().onTrue(operatorControls.requestAimHubCommand());
// controllerlower.leftStick().onFalse(operatorControls.requestIdleCommand()); // controller.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());
} }
@@ -175,18 +134,17 @@ public class RobotContainer {
// 这些命令 require Superstructure，与自动程序共用需求； // 这些命令 require Superstructure，与自动程序共用需求；
// 用 teleop 门控避免自动赛阶段误触直接取消整个 PathPlanner 自动。 // 用 teleop 门控避免自动赛阶段误触直接取消整个 PathPlanner 自动。
Trigger teleopOnly = RobotModeTriggers.teleop(); Trigger teleopOnly = RobotModeTriggers.teleop();
controllerupper.rightTrigger().and(teleopOnly) controller.rightTrigger().and(teleopOnly)
.onTrue(operatorControls.requestShootHubCommand()) .onTrue(operatorControls.requestShootHubCommand())
.onFalse(operatorControls.requestIdleCommand()); .onFalse(operatorControls.requestIdleCommand());
controllerupper.b().and(teleopOnly) controller.rightBumper().and(teleopOnly)
.onTrue(operatorControls.requestEjectCommand()) .onTrue(operatorControls.requestEjectCommand())
.onFalse(operatorControls.requestIdleCommand()); .onFalse(operatorControls.requestIdleCommand());
controllerupper.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(-1.0))); controller.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(-1.0)));
controllerupper.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(1.0))); controller.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVelocityOffset(1.0)));
controllerupper.povUp().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(100))); controller.povUp().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(100)));
controllerupper.povDown().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(-100))); controller.povDown().onTrue(Commands.runOnce(() -> operatorControls.adjustBackboardRateOffset(-100)));
controllerupper.back().onTrue(Commands.runOnce(() -> operatorControls.resetOffsets())); controller.start().onTrue(operatorControls.resetBackboardCommand());
controllerupper.start().onTrue(operatorControls.resetBackboardCommand());
} }
private void bingdingLED() { private void bingdingLED() {
@@ -198,16 +156,13 @@ public class RobotContainer {
} }
public void tunerPeriodic() { public void tunerPeriodic() {
boolean allowMotorOutputTuning = DriverStation.isTest();
driveGainsTuner.periodic(); driveGainsTuner.periodic();
configFlywheelTuner.periodic(() -> shooterSubsystem.applyLeftConfigurationToRight()); configFlywheelTuner.periodic(() -> shooterSubsystem.applyLeftConfigurationToRight());
shooterSubsystem.debugFlywheelVelocityPeriodic();
configConveyorTuner.periodic(null);
shooterSubsystem.debugShooterConveyorVelocityPeriodic();
configBackboardTuner.periodic(); configBackboardTuner.periodic();
shooterSubsystem.debugBackboardTargetPeriodic(); shooterSubsystem.debugFlywheelVelocityPeriodic(allowMotorOutputTuning);
configMainConveyorTuner.periodic(null); shooterSubsystem.debugBackboardTargetPeriodic(allowMotorOutputTuning);
configIntakerRollerTuner.periodic(null);
configIntakerRotaterTuner.periodic(null);
shooterSubsystem.setRightFlywheelFollowLeft(); shooterSubsystem.setRightFlywheelFollowLeft();
shooterSubsystem.setRightConveyorFollowLeft(); shooterSubsystem.setRightConveyorFollowLeft();
} }
@@ -219,27 +174,24 @@ public class RobotContainer {
private void configueIntaker() { private void configueIntaker() {
// 进球相关按钮也只请求 Superstructure 状态；intaker 开赛后默认保持下放。 // 进球相关按钮也只请求 Superstructure 状态；intaker 开赛后默认保持下放。
Trigger teleopOnly = RobotModeTriggers.teleop(); 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()) .onTrue(operatorControls.requestIntakeCommand())
.onFalse(operatorControls.requestIdleCommand()); .onFalse(operatorControls.requestIdleCommand());
Trigger manualRaiseIntaker = controllerupper.x().and(teleopOnly); Trigger manualRaiseIntaker = controller.y().and(teleopOnly);
manualRaiseIntaker manualRaiseIntaker
.onTrue(operatorControls.requestManualCommand()) .onTrue(operatorControls.requestManualCommand())
.whileTrue(operatorControls.manualRaiseIntakeRotaterCommand(intakeRotaterSubsystem)) .whileTrue(operatorControls.manualRaiseIntakeRotaterCommand(intakeRotaterSubsystem))
.onFalse(operatorControls.requestIdleCommand()); .onFalse(operatorControls.requestIdleCommand());
Trigger manualLowerIntaker = controllerupper.a().and(teleopOnly); Trigger manualLowerIntaker = controller.a().and(teleopOnly);
manualLowerIntaker manualLowerIntaker
.onTrue(operatorControls.requestManualCommand()) .onTrue(operatorControls.requestManualCommand())
.whileTrue(operatorControls.manualLowerIntakeRotaterCommand(intakeRotaterSubsystem)) .whileTrue(operatorControls.manualLowerIntakeRotaterCommand(intakeRotaterSubsystem))
.onFalse(operatorControls.requestIdleCommand()); .onFalse(operatorControls.requestIdleCommand());
controllerupper.y().and(teleopOnly)
.onTrue(Commands.sequence(
operatorControls.requestManualCommand(),
operatorControls.lowerIntakeRotaterForMatchCommand(intakeRotaterSubsystem),
operatorControls.requestIdleCommand()
));
} }
} }
src/main/java/frc/robot/controls/DriveControls.java
+3 -3
View File
@@ -37,8 +37,8 @@ public class DriveControls {
private final double maxAngularRate = RotationsPerSecond.of(0.75).in(edu.wpi.first.units.Units.RadiansPerSecond); private final double maxAngularRate = RotationsPerSecond.of(0.75).in(edu.wpi.first.units.Units.RadiansPerSecond);
// 默认速度比例，普通驾驶使用。 // 默认速度比例，普通驾驶使用。
private double normalDriveScale = 0.3; private double normalDriveScale = 1.0;
private double normalTurnScale = 0.7; private double normalTurnScale = 1.0;
// 加速模式比例，按住 RB 时使用。 // 加速模式比例，按住 RB 时使用。
private double boostDriveScale = 1.0; private double boostDriveScale = 1.0;
@@ -48,7 +48,7 @@ public class DriveControls {
private boolean boostEnabled = false; 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 vyLimiter = new SlewRateLimiter(5.0);
private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 等效角加速度限制 private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 等效角加速度限制
src/main/java/frc/robot/controls/OperatorControls.java
+19
View File
@@ -3,6 +3,7 @@ package frc.robot.controls;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Commands;
import frc.robot.Constants; import frc.robot.Constants;
import frc.robot.subsystems.IntakeRollerSubsystem;
import frc.robot.subsystems.IntakeRotaterSubsystem; import frc.robot.subsystems.IntakeRotaterSubsystem;
import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.ShooterSubsystem;
import frc.robot.subsystems.Superstructure; import frc.robot.subsystems.Superstructure;
@@ -74,6 +75,24 @@ public class OperatorControls {
return Commands.runEnd(intakeRotater::manualRaise, intakeRotater::stop, intakeRotater); 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) { public Command manualLowerIntakeRotaterCommand(IntakeRotaterSubsystem intakeRotater) {
return Commands.runEnd(intakeRotater::manualLower, intakeRotater::stop, intakeRotater); return Commands.runEnd(intakeRotater::manualLower, intakeRotater::stop, intakeRotater);
} }
src/main/java/frc/robot/generated/TunerConstants.java
+1 -1
View File
@@ -77,7 +77,7 @@ public class TunerConstants {
new Pigeon2Configuration() new Pigeon2Configuration()
.withGyroTrim( .withGyroTrim(
new GyroTrimConfigs() new GyroTrimConfigs()
.withGyroScalarZ(-5.0) .withGyroScalarZ(-6.7)
) )
.withPigeon2Features( .withPigeon2Features(
new Pigeon2FeaturesConfigs() new Pigeon2FeaturesConfigs()
src/main/java/frc/robot/subsystems/LimelightSubsystem.java
-4
View File
@@ -46,8 +46,4 @@ public class LimelightSubsystem extends SubsystemBase {
public Pose2d getEstimatedPose() { public Pose2d getEstimatedPose() {
return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName).pose; return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName).pose;
} }
public double getShooterVelocityByDistance() {
return Constants.Shooter.flywheelVelocityByDistance.get(getDistanceToTarget());
}
} }
src/main/java/frc/robot/subsystems/ShooterSubsystem.java
+25 -49
View File
@@ -9,12 +9,14 @@ import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.hardware.TalonFXS; import com.ctre.phoenix6.hardware.TalonFXS;
import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.ctre.phoenix6.signals.MotorAlignmentValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints; import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Command;
@@ -49,7 +51,6 @@ public class ShooterSubsystem extends SubsystemBase {
private final NetworkTable shooterNetworkTable; private final NetworkTable shooterNetworkTable;
private final NetworkTable flywheelTuningTable; private final NetworkTable flywheelTuningTable;
private final NetworkTable conveyorTuningTable;
private final NetworkTable backboardTuningTable; private final NetworkTable backboardTuningTable;
private double flywheelOutputVolts = 0.0; private double flywheelOutputVolts = 0.0;
@@ -90,6 +91,7 @@ public class ShooterSubsystem extends SubsystemBase {
backboardMotor = new TalonFXS(Constants.Shooter.backboardMotorID); backboardMotor = new TalonFXS(Constants.Shooter.backboardMotorID);
backboardMotor.getConfigurator().apply(Constants.Shooter.backboardSlot0Configs); backboardMotor.getConfigurator().apply(Constants.Shooter.backboardSlot0Configs);
setBackboardBrakeMode();
backboardEncoder = new Encoder(0, 1, false, Encoder.EncodingType.k4X); backboardEncoder = new Encoder(0, 1, false, Encoder.EncodingType.k4X);
backboardEncoder.setSamplesToAverage(5); backboardEncoder.setSamplesToAverage(5);
@@ -109,9 +111,6 @@ public class ShooterSubsystem extends SubsystemBase {
flywheelTuningTable.getEntry("flywheelVelocityEnable").setDefaultBoolean(false); flywheelTuningTable.getEntry("flywheelVelocityEnable").setDefaultBoolean(false);
flywheelTuningTable.getEntry("flywheelTargetVelocity").setDefaultDouble(Constants.Shooter.defaultFlywheelVelocity); flywheelTuningTable.getEntry("flywheelTargetVelocity").setDefaultDouble(Constants.Shooter.defaultFlywheelVelocity);
flywheelTuningTable.getEntry("flywheelVelocityTolerance").setDefaultDouble(Constants.Shooter.flywheelReadyVelocityTolerance); 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 = NetworkTableInstance.getDefault().getTable("Tuning/backboard");
backboardTuningTable.getEntry("backboardPositionEnable").setDefaultBoolean(false); backboardTuningTable.getEntry("backboardPositionEnable").setDefaultBoolean(false);
backboardTuningTable.getEntry("backboardTargetPosition").setDefaultDouble(0.0); backboardTuningTable.getEntry("backboardTargetPosition").setDefaultDouble(0.0);
@@ -155,16 +154,12 @@ public class ShooterSubsystem extends SubsystemBase {
} }
public boolean atVelocity() { public boolean atVelocity() {
if (Math.abs(flywheelTargetVelocity) < Constants.Shooter.flywheelReadyVelocityThreshold) { double currentvelocity = getAverageFlywheelVelocity();
flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp(); if(currentvelocity>flywheelTargetVelocity*0.95){
return true;
} else {
return false; return false;
} }
if (Math.abs(getFlywheelVelocityError()) > flywheelVelocityTolerance) {
flywheelVelocityReadyStartTimestamp = Timer.getFPGATimestamp();
return false;
}
return Timer.getFPGATimestamp() - flywheelVelocityReadyStartTimestamp
>= Constants.Shooter.flywheelReadyDelaySeconds;
} }
public double getLeftFlywheelVelocity() { public double getLeftFlywheelVelocity() {
@@ -196,11 +191,7 @@ public class ShooterSubsystem extends SubsystemBase {
/* ====================== */ /* ====================== */
public void runShooterConveyorVelocity(double targetVelocityRps) { public void runShooterConveyorVelocity(double targetVelocityRps) {
shooterConveyorTargetVelocity = MathUtil.clamp( shooterConveyorTargetVelocity = targetVelocityRps;
targetVelocityRps,
-Constants.Shooter.shooterConveyorMaxVelocity,
Constants.Shooter.shooterConveyorMaxVelocity
);
leftConveyor.setControl(conveyorVelocityRequest.withVelocity(shooterConveyorTargetVelocity)); leftConveyor.setControl(conveyorVelocityRequest.withVelocity(shooterConveyorTargetVelocity));
setRightConveyorFollowLeft(); setRightConveyorFollowLeft();
} }
@@ -318,6 +309,12 @@ public class ShooterSubsystem extends SubsystemBase {
setBackboardVoltage(0.0); setBackboardVoltage(0.0);
} }
public void setBackboardBrakeMode() {
var backboardMotorOutputConfigs = new MotorOutputConfigs();
backboardMotorOutputConfigs.NeutralMode = NeutralModeValue.Brake;
backboardMotor.getConfigurator().apply(backboardMotorOutputConfigs);
}
public double getBackboardPosition() { public double getBackboardPosition() {
return backboardEncoder.getDistance(); return backboardEncoder.getDistance();
} }
@@ -339,17 +336,17 @@ public class ShooterSubsystem extends SubsystemBase {
} }
public boolean isBackboardPositionTuningActive() { public boolean isBackboardPositionTuningActive() {
return backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false); return DriverStation.isTest()
&& backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false);
} }
public boolean isFlywheelTuningControlActive() { public boolean isFlywheelTuningControlActive() {
boolean velocityTunerEnabled = flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false); return DriverStation.isTest()
return velocityTunerEnabled; && flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false);
} }
public boolean isShooterConveyorTuningControlActive() { public boolean isShooterConveyorTuningControlActive() {
boolean velocityTunerEnabled = conveyorTuningTable.getEntry("conveyorVelocityEnable").getBoolean(false); return false;
return velocityTunerEnabled;
} }
public void resetbackboardencoder() { public void resetbackboardencoder() {
@@ -379,8 +376,9 @@ public class ShooterSubsystem extends SubsystemBase {
stopBackboard(); stopBackboard();
} }
public void debugBackboardTargetPeriodic() { public void debugBackboardTargetPeriodic(boolean allowMotorOutput) {
boolean positionEnable = backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false); boolean positionEnable =
allowMotorOutput && backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false);
double requestedTarget = backboardTuningTable.getEntry("backboardTargetPosition").getDouble(0.0); double requestedTarget = backboardTuningTable.getEntry("backboardTargetPosition").getDouble(0.0);
double clampedTarget = MathUtil.clamp( double clampedTarget = MathUtil.clamp(
requestedTarget, requestedTarget,
@@ -397,8 +395,9 @@ public class ShooterSubsystem extends SubsystemBase {
} }
} }
public void debugFlywheelVelocityPeriodic() { public void debugFlywheelVelocityPeriodic(boolean allowMotorOutput) {
flywheelVelocityTuningActive = flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false); flywheelVelocityTuningActive =
allowMotorOutput && flywheelTuningTable.getEntry("flywheelVelocityEnable").getBoolean(false);
double requestedTargetVelocity = double requestedTargetVelocity =
flywheelTuningTable.getEntry("flywheelTargetVelocity").getDouble(Constants.Shooter.defaultFlywheelVelocity); flywheelTuningTable.getEntry("flywheelTargetVelocity").getDouble(Constants.Shooter.defaultFlywheelVelocity);
flywheelVelocityTolerance = Math.max( 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 @Override
public void periodic() { public void periodic() {
double backboardPosition = getBackboardPosition(); double backboardPosition = getBackboardPosition();
@@ -499,4 +476,3 @@ public class ShooterSubsystem extends SubsystemBase {
shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent()); shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent());
} }
} }
src/main/java/frc/robot/subsystems/Superstructure.java
+12 -4
View File
@@ -1,5 +1,6 @@
package frc.robot.subsystems; package frc.robot.subsystems;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.NetworkTableInstance;
@@ -284,8 +285,13 @@ public class Superstructure extends SubsystemBase {
} }
private void handleShootHub() { private void handleShootHub() {
double targetVelocity = limelight.getShooterVelocityByDistance() + flywheelVelocityOffset; double x = NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
double backboardPosition = Constants.Superstructure.backboardShootPosition + backboardPositionOffset; 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); handlePreparedFeed(targetVelocity, backboardPosition);
} }
@@ -301,8 +307,6 @@ public class Superstructure extends SubsystemBase {
setSystemState(SystemState.PREP_SHOOT); setSystemState(SystemState.PREP_SHOOT);
} }
intakeRoller.stop(); intakeRoller.stop();
conveyor.stop();
stopShooterConveyorUnlessTuning();
// 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调， // 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调，
// 但不会绕过 Superstructure 状态机。 // 但不会绕过 Superstructure 状态机。
shooter.setFlywheelVelocity(targetVelocity); shooter.setFlywheelVelocity(targetVelocity);
@@ -312,6 +316,7 @@ public class Superstructure extends SubsystemBase {
// 只有 canShoot() 通过后才会进入本状态；喂球期间不再复查门控， // 只有 canShoot() 通过后才会进入本状态；喂球期间不再复查门控，
// 避免球接触飞轮导致的掉速中断喂球。 // 避免球接触飞轮导致的掉速中断喂球。
intakeRotater.shootAssist(); intakeRotater.shootAssist();
conveyor.feedToShooter(); conveyor.feedToShooter();
runShooterConveyorVelocityUnlessTuning(Constants.Superstructure.shooterFeedVelocity); runShooterConveyorVelocityUnlessTuning(Constants.Superstructure.shooterFeedVelocity);
return; return;
@@ -382,6 +387,9 @@ public class Superstructure extends SubsystemBase {
if (systemState != nextState) { if (systemState != nextState) {
systemState = nextState; systemState = nextState;
stateStartTimestamp = Timer.getFPGATimestamp(); stateStartTimestamp = Timer.getFPGATimestamp();
if (nextState == SystemState.SHOOTING) {
shooter.setBackboardBrakeMode();
}
} }
} }
src/main/java/frc/robot/tuning/ConfigTalonFXSMotorTuner.java
-17
View File
@@ -1,6 +1,5 @@
package frc.robot.tuning; package frc.robot.tuning;
import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.Slot0Configs;
import com.ctre.phoenix6.controls.VelocityDutyCycle;
import com.ctre.phoenix6.hardware.TalonFXS; import com.ctre.phoenix6.hardware.TalonFXS;
import edu.wpi.first.networktables.*; import edu.wpi.first.networktables.*;
@@ -20,11 +19,8 @@ public class ConfigTalonFXSMotorTuner {
private final DoubleSubscriber KaSub; private final DoubleSubscriber KaSub;
private final DoubleSubscriber KsSub; 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 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_) { public ConfigTalonFXSMotorTuner(TalonFXS motor_, String motorName_, Slot0Configs motorSlot0Configs_) {
this.motor = motor_; this.motor = motor_;
this.motorName = motorName_; this.motorName = motorName_;
@@ -39,9 +35,6 @@ public class ConfigTalonFXSMotorTuner {
table.getDoubleTopic(motorName+"kA").publish().set(motorSlot0Configs.kA); table.getDoubleTopic(motorName+"kA").publish().set(motorSlot0Configs.kA);
table.getDoubleTopic(motorName+"kS").publish().set(motorSlot0Configs.kS); 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.enableSub = table.getBooleanTopic(motorName+"Enable").subscribe(false);
this.KpSub = table.getDoubleTopic(motorName+"kP").subscribe(motorSlot0Configs.kP); this.KpSub = table.getDoubleTopic(motorName+"kP").subscribe(motorSlot0Configs.kP);
this.KiSub = table.getDoubleTopic(motorName+"kI").subscribe(motorSlot0Configs.kI); 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.KvSub = table.getDoubleTopic(motorName+"kV").subscribe(motorSlot0Configs.kV);
this.KaSub = table.getDoubleTopic(motorName+"kA").subscribe(motorSlot0Configs.kA); this.KaSub = table.getDoubleTopic(motorName+"kA").subscribe(motorSlot0Configs.kA);
this.KsSub = table.getDoubleTopic(motorName+"kS").subscribe(motorSlot0Configs.kS); 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() { public void periodic() {
@@ -73,13 +64,5 @@ public class ConfigTalonFXSMotorTuner {
lastKp = kp; lastKi = ki; lastKd = kd; lastKv = kv; lastKa = ka; lastKs = ks; 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));
}
} }
} }