Compare commits
14
Commits
1
..
1c39a3bc42
| Author | SHA1 | Date | |
|---|---|---|---|
 OnDYCLaptop
|
1c39a3bc42 | ||
|
OnDYCLaptop
|
2b14e3f99e | ||
|
OnDYCLaptop
|
0053c29893 | ||
|
OnDYCLaptop
|
179913194c | ||
|
OnDYCLaptop
|
5947fa27d3 | ||
|
OnDYCLaptop
|
a1fc79659e | ||
|
OnDYCLaptop
|
0c97452a13 | ||
|
OnDYCLaptop
|
a78a71e4ed | ||
|
dingdoorfiah-ui
|
fad52c40c0 | ||
|
Claude
|
9da20a108c | ||
|
Claude
|
6264ef0762 | ||
|
OnDYCLaptop
|
3c5d17b527 | ||
|
OnDYCLaptop
|
132b231c36 | ||
|
OnDYCLaptop
|
99a92af429 |
File diff suppressed because it is too large
Load Diff
+115
-28
@@ -91,6 +91,93 @@
|
||||
"period": 0.06,
|
||||
"sort_options": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "HubActive",
|
||||
"x": 0.0,
|
||||
"y": 0.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Boolean Box",
|
||||
"properties": {
|
||||
"topic": "/GameData2026/HubActive",
|
||||
"period": 0.06,
|
||||
"data_type": "boolean",
|
||||
"true_color": 4283215696,
|
||||
"false_color": 4294198070,
|
||||
"true_icon": "None",
|
||||
"false_icon": "None"
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "HubStatus",
|
||||
"x": 256.0,
|
||||
"y": 0.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/GameData2026/HubStatus",
|
||||
"period": 0.06,
|
||||
"data_type": "string",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "SecondsUntilHubOpens",
|
||||
"x": 0.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/GameData2026/SecondsUntilHubOpens",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "Shift",
|
||||
"x": 256.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/GameData2026/Shift",
|
||||
"period": 0.06,
|
||||
"data_type": "string",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "FirstInactiveAlliance",
|
||||
"x": 0.0,
|
||||
"y": 256.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/GameData2026/FirstInactiveAlliance",
|
||||
"period": 0.06,
|
||||
"data_type": "string",
|
||||
"show_submit_button": false
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "MatchTime",
|
||||
"x": 256.0,
|
||||
"y": 256.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/GameData2026/MatchTime",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": false
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -681,14 +768,14 @@
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "flywheelRunningSpeedRPS",
|
||||
"title": "flywheelRunningVolts",
|
||||
"x": 640.0,
|
||||
"y": 128.0,
|
||||
"width": 256.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Tuning/flywheel/flywheelRunningSpeedRPS",
|
||||
"topic": "/Tuning/flywheel/flywheelRunningVolts",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
@@ -758,27 +845,27 @@
|
||||
"layouts": [],
|
||||
"containers": [
|
||||
{
|
||||
"title": "limelight-intaker",
|
||||
"title": "limelight-front",
|
||||
"x": 0.0,
|
||||
"y": 0.0,
|
||||
"width": 256.0,
|
||||
"height": 256.0,
|
||||
"type": "Camera Stream",
|
||||
"properties": {
|
||||
"topic": "/CameraPublisher/limelight-intaker",
|
||||
"topic": "/CameraPublisher/limelight-front",
|
||||
"period": 0.06,
|
||||
"rotation_turns": 0
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "limelight-shooter",
|
||||
"title": "limelight-left",
|
||||
"x": 768.0,
|
||||
"y": 512.0,
|
||||
"width": 640.0,
|
||||
"height": 384.0,
|
||||
"type": "Camera Stream",
|
||||
"properties": {
|
||||
"topic": "/CameraPublisher/limelight-shooter",
|
||||
"topic": "/CameraPublisher/limelight-left",
|
||||
"period": 0.06,
|
||||
"rotation_turns": 0
|
||||
}
|
||||
@@ -822,7 +909,7 @@
|
||||
"height": 128.0,
|
||||
"type": "Boolean Box",
|
||||
"properties": {
|
||||
"topic": "/Positioning/limelight-intaker/available",
|
||||
"topic": "/Positioning/limelight-front/available",
|
||||
"period": 0.06,
|
||||
"data_type": "boolean",
|
||||
"true_color": 4283215696,
|
||||
@@ -839,7 +926,7 @@
|
||||
"height": 128.0,
|
||||
"type": "Boolean Box",
|
||||
"properties": {
|
||||
"topic": "/Positioning/limelight-shooter/available",
|
||||
"topic": "/Positioning/limelight-left/available",
|
||||
"period": 0.06,
|
||||
"data_type": "boolean",
|
||||
"true_color": 4283215696,
|
||||
@@ -947,7 +1034,7 @@
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Positioning/limelight-intaker/Distance",
|
||||
"topic": "/Positioning/limelight-front/Distance",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
@@ -977,7 +1064,7 @@
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Positioning/limelight-shooter/Distance",
|
||||
"topic": "/Positioning/limelight-left/Distance",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
@@ -1051,7 +1138,7 @@
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Positioning/limelight-intaker/X",
|
||||
"topic": "/Positioning/limelight-front/X",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
@@ -1065,7 +1152,7 @@
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Positioning/limelight-intaker/Y",
|
||||
"topic": "/Positioning/limelight-front/Y",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
@@ -1079,7 +1166,7 @@
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Positioning/limelight-shooter/X",
|
||||
"topic": "/Positioning/limelight-left/X",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
@@ -1093,7 +1180,7 @@
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Positioning/limelight-shooter/Y",
|
||||
"topic": "/Positioning/limelight-left/Y",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
@@ -1108,14 +1195,14 @@
|
||||
"layouts": [],
|
||||
"containers": [
|
||||
{
|
||||
"title": "limelight-shooter",
|
||||
"title": "limelight-left",
|
||||
"x": 1152.0,
|
||||
"y": 0.0,
|
||||
"width": 384.0,
|
||||
"height": 256.0,
|
||||
"type": "Camera Stream",
|
||||
"properties": {
|
||||
"topic": "/CameraPublisher/limelight-shooter",
|
||||
"topic": "/CameraPublisher/limelight-left",
|
||||
"period": 0.06,
|
||||
"rotation_turns": 0
|
||||
}
|
||||
@@ -1137,14 +1224,14 @@
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "conveyorSpeed",
|
||||
"title": "conveyorVelocity",
|
||||
"x": 384.0,
|
||||
"y": 256.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Shooter/conveyorSpeed",
|
||||
"topic": "/Shooter/conveyorVelocity",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
@@ -1184,14 +1271,14 @@
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "flywheelSpeed",
|
||||
"title": "flywheelVelocity",
|
||||
"x": 640.0,
|
||||
"y": 0.0,
|
||||
"width": 256.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/flywheelSpeed",
|
||||
"topic": "/Shooter/flywheelVelocity",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 5.0,
|
||||
@@ -1235,14 +1322,14 @@
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "conveyorSpeed",
|
||||
"title": "conveyorVelocity",
|
||||
"x": 384.0,
|
||||
"y": 0.0,
|
||||
"width": 256.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/Shooter/conveyorSpeed",
|
||||
"topic": "/Shooter/conveyorVelocity",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 5.0,
|
||||
@@ -1251,14 +1338,14 @@
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "flywheelSpeed",
|
||||
"title": "flywheelVelocity",
|
||||
"x": 640.0,
|
||||
"y": 256.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/Shooter/flywheelSpeed",
|
||||
"topic": "/Shooter/flywheelVelocity",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
@@ -1293,14 +1380,14 @@
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "flywheelTargetSpeed",
|
||||
"title": "flywheelTargetVoltage",
|
||||
"x": 768.0,
|
||||
"y": 256.0,
|
||||
"width": 128.0,
|
||||
"height": 128.0,
|
||||
"type": "Text Display",
|
||||
"properties": {
|
||||
"topic": "/ShooterControl/flywheelTargetSpeed",
|
||||
"topic": "/Shooter/flywheelTargetVoltage",
|
||||
"period": 0.06,
|
||||
"data_type": "double",
|
||||
"show_submit_button": true
|
||||
@@ -1337,14 +1424,14 @@
|
||||
}
|
||||
},
|
||||
{
|
||||
"title": "flywheelTargetSpeed",
|
||||
"title": "flywheelTargetVoltage",
|
||||
"x": 640.0,
|
||||
"y": 384.0,
|
||||
"width": 256.0,
|
||||
"height": 256.0,
|
||||
"type": "Graph",
|
||||
"properties": {
|
||||
"topic": "/ShooterControl/flywheelTargetSpeed",
|
||||
"topic": "/Shooter/flywheelTargetVoltage",
|
||||
"period": 0.033,
|
||||
"data_type": "double",
|
||||
"time_displayed": 5.0,
|
||||
|
||||
@@ -13,13 +13,13 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
}
|
||||
]
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
}
|
||||
]
|
||||
|
||||
@@ -30,7 +30,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -100,7 +100,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -125,7 +125,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
|
||||
@@ -76,7 +76,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
}
|
||||
]
|
||||
|
||||
@@ -76,7 +76,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
}
|
||||
]
|
||||
|
||||
@@ -19,7 +19,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
}
|
||||
]
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
|
||||
@@ -26,7 +26,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
|
||||
@@ -30,7 +30,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -100,7 +100,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -125,7 +125,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
|
||||
@@ -26,7 +26,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -99,7 +99,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -124,7 +124,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
|
||||
@@ -32,7 +32,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -93,7 +93,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
|
||||
@@ -32,7 +32,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -93,7 +93,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
|
||||
@@ -32,7 +32,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "shoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
@@ -93,7 +93,7 @@
|
||||
{
|
||||
"type": "named",
|
||||
"data": {
|
||||
"name": "AutoShoot"
|
||||
"name": "shoot_hub"
|
||||
}
|
||||
},
|
||||
{
|
||||
|
||||
@@ -1,36 +1,40 @@
|
||||
package frc.robot;
|
||||
|
||||
import javax.net.ssl.TrustManager;
|
||||
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
|
||||
|
||||
public final class Constants {
|
||||
public static final class DemoMode {
|
||||
public static final boolean ENABLED = true;
|
||||
|
||||
public static final double normalDriveScale = 0.12;
|
||||
public static final double normalTurnScale = 0.20;
|
||||
public static final double boostDriveScale = 0.25;
|
||||
public static final double boostTurnScale = 0.30;
|
||||
|
||||
public static final double maxLinearSpeedMetersPerSecond = 2.4;
|
||||
public static final double maxAngularRateRadiansPerSecond = 1.6;
|
||||
}
|
||||
private Constants() {}
|
||||
|
||||
public static final boolean LEDUsing = false;
|
||||
public enum RobotStatus{
|
||||
Stopped,
|
||||
AllTelop,
|
||||
PassingBall,
|
||||
AutoAimming,
|
||||
Climbing,
|
||||
CrossingBump,
|
||||
CrossingTrench
|
||||
|
||||
public static class Shooter {
|
||||
// 机构 CAN ID 目前是占位值；季后赛最终接线确认后，只需要改这里。
|
||||
public static final int leftFlywheelID = 38;
|
||||
public static final int rightFlywheelID = 39;
|
||||
public static final int leftConveyorID = 40;
|
||||
public static final int rightConveyorID = 37;
|
||||
public static final int backboardMotorID = 44;
|
||||
|
||||
public static final double defaultFlywheelVoltage = 3.34;
|
||||
public static final double flywheelReadyVoltageThreshold = 1.0;
|
||||
public static final double flywheelReadyDelaySeconds = 0.5;
|
||||
|
||||
// 距离(米) -> 飞轮电压(V) 射表。当前为占位点；
|
||||
// 真车试射后按实测数据加点即可，查表自动线性插值并在两端取边界值。
|
||||
public static final InterpolatingDoubleTreeMap flywheelVoltageByDistance = new InterpolatingDoubleTreeMap();
|
||||
static {
|
||||
flywheelVoltageByDistance.put(1.2, 5.82);
|
||||
flywheelVoltageByDistance.put(2.5, 6.34);
|
||||
flywheelVoltageByDistance.put(3.5, 6.73);
|
||||
flywheelVoltageByDistance.put(5.0, 7.33);
|
||||
}
|
||||
public class Shooter {
|
||||
public static final double conveyorSpeed = 80;
|
||||
public static final double conveyorSpeedForAuto = 10;
|
||||
|
||||
public static final double shooterConveyorFeedVoltage = 3.0;
|
||||
public static final double shooterConveyorReverseVoltage = -2.0;
|
||||
|
||||
public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.0).withKI(0).withKD(0)
|
||||
.withKV(0.099).withKA(0.0).withKS(0.0);
|
||||
@@ -43,18 +47,19 @@ public final class Constants {
|
||||
.withKP(0.01).withKI(0).withKD(0)
|
||||
.withKV(0.03).withKA(0.0).withKS(0.010);
|
||||
|
||||
public class backboardPositionPID {
|
||||
public static final double kP = 0.3;
|
||||
public static class backboardPositionPID {
|
||||
public static final double kP = 0.007;
|
||||
public static final double kI = 0.0;
|
||||
public static final double kD = 0.0;
|
||||
}
|
||||
|
||||
public static final double backboardSpeedMax = 5.0;
|
||||
public static final double backboardSpeedMin = 0.4;
|
||||
public static final double backboardUpLimit = 2200.0;
|
||||
public static final double backboardDownLimit = 0.0;
|
||||
}
|
||||
public static final double backboardSpeedMax = 60; //也就是output
|
||||
// 背板的上下限位角度
|
||||
public static final double backboardUpLimit = 3000.0; // 背板上限角度（根据实际需求设置）
|
||||
public static final double backboardDownLimit = 0.0; // 背板下限角度（根据实际需求设置）
|
||||
}
|
||||
public class AutoPositioning {
|
||||
|
||||
public static class AutoPositioning {
|
||||
public static final double autoRotationkP = 0.08;
|
||||
public static final double autoRotationForBumpTargetDegrees = 45;
|
||||
public static final double autoRotationForTrenchTargetDegrees = 0;
|
||||
@@ -62,21 +67,20 @@ public final class Constants {
|
||||
public static final double teleopOffsetkP = 15;
|
||||
|
||||
public static final double autoPositioningkP = 1.0;
|
||||
public static final double autoPositioningAngleError = 1.0; // 角度误差容忍度，单位为度
|
||||
public static final double autoPositioningAngleError = 1.0;
|
||||
|
||||
public static final double[] bumpY = {2.51,5.556};
|
||||
public static final double[] trenchY = {0.639,7.43};
|
||||
public static final double[] bumpY = {2.51, 5.556};
|
||||
public static final double[] trenchY = {0.639, 7.43};
|
||||
|
||||
public static final double TrenchtargetAngle = 0;
|
||||
public static final double TurningkP = 1.5;
|
||||
}
|
||||
|
||||
public class StartingPoints{
|
||||
public class Red{
|
||||
public static final Pose2d Point1 = new Pose2d(12.93, 7.47, edu.wpi.first.math.geometry.Rotation2d.fromDegrees(0));
|
||||
public static final Pose2d Point2 = new Pose2d(14.314, 4.15, edu.wpi.first.math.geometry.Rotation2d.fromDegrees(-180));
|
||||
public static class StartingPoints {
|
||||
public static class Red {
|
||||
public static final Pose2d Point1 = new Pose2d(12.93, 7.47, Rotation2d.fromDegrees(0));
|
||||
public static final Pose2d Point2 = new Pose2d(14.314, 4.15, Rotation2d.fromDegrees(-180));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public enum DriveMode {
|
||||
@@ -85,41 +89,19 @@ public final class Constants {
|
||||
SYSID,
|
||||
OTHER
|
||||
}
|
||||
public class Pigeon {
|
||||
|
||||
public static class Pigeon {
|
||||
public static final int CANID = 0;
|
||||
}
|
||||
|
||||
public class Limelight {
|
||||
public static final String LIMELIGHT_NAME_Shooter = "limelight-shooter";
|
||||
public static final String LIMELIGHT_NAME_Intaker = "limelight-intaker";
|
||||
public static final Boolean UsingMetaTag2 = false; // Use Limelight's own pose/yaw estimate instead of gyro-assisted MegaTag2.
|
||||
public static final int climbPipelineIndex = 8;
|
||||
public static class Limelight {
|
||||
public static final String LIMELIGHT_NAME_Shooter = "limelight-left";
|
||||
public static final String LIMELIGHT_NAME_Intaker = "limelight-front";
|
||||
public static final Boolean UsingMetaTag2 = true;
|
||||
public static final int locatePipelineIndex = 0;
|
||||
|
||||
// 误差死区 / 对齐判定
|
||||
public static final double AutoClimbToleranceTX = 1.5; // deg
|
||||
public static final double AutoClimbToleranceTZ = 0.10; // m
|
||||
public static final double AutoClimbToleranceRY = 2.0; // deg
|
||||
|
||||
// P 控制系数
|
||||
public static final double AutoClimbKpTX = 0.020; // deg -> rad/s
|
||||
public static final double AutoClimbKpTZ = 0.90; // m -> m/s
|
||||
public static final double AutoClimbKpRY = 0.015; // deg -> rad/s
|
||||
|
||||
// 最小输出（防止快到目标时推不动）
|
||||
public static final double AutoClimbMinVX = 0.18; // m/s
|
||||
public static final double AutoClimbMinOmega = 0.10; // rad/s
|
||||
|
||||
// 最大输出（防止冲太猛）
|
||||
public static final double AutoClimbMaxVX = 1.20; // m/s
|
||||
public static final double AutoClimbMaxOmega = 0.60; // rad/s
|
||||
|
||||
// 远距离 / 大角度时额外限速，可后续微调
|
||||
public static final double AutoClimbFastTurnThresholdTX = 15.0; // deg
|
||||
public static final double AutoClimbFastTurnOmega = 0.45; // rad/s
|
||||
}
|
||||
|
||||
public class Field {
|
||||
public static class Field {
|
||||
public static final double RedHubPositionX = 11.914;
|
||||
public static final double RedHubPositionY = 4.034;
|
||||
public static final double BlueHubPositionX = 4.625;
|
||||
@@ -130,55 +112,58 @@ public final class Constants {
|
||||
public static final double BluePassingBallPosX = 3.184;
|
||||
}
|
||||
|
||||
public static class Intaker {
|
||||
// 进球机构 CAN ID 同样是占位值，真实 ID 确认后集中在这里修改。
|
||||
public static final int leftIntakeRollerID = 20;
|
||||
public static final int rightIntakeRollerID = 47;
|
||||
public static final int leftIntakeRotaterID = 48;
|
||||
public static final int rightIntakeRotaterID = 43;
|
||||
|
||||
public class Intaker {
|
||||
public static final double intakeRollerIntakeVoltage = -6.0;
|
||||
public static final double intakeRollerOuttakeVoltage = 3.0;
|
||||
|
||||
public static final double intakeRotaterUpLimit = 0.0; // rotator上限角度
|
||||
public static final double intakeRotaterDownLimit = -7.6; // rotator下限角度
|
||||
public static final double intakeRotaterVelocity = 5; // rotater的速度（如果你用速度控制的话）
|
||||
public static final double IntakeRotaterGravityFF = 0.0; // 重力前馈增益（需要实测调整）
|
||||
public static final double intakeRotaterRaiseVoltage = 0.8;
|
||||
public static final double intakeRotaterLowerVoltage = 0.05;
|
||||
|
||||
public static final Slot0Configs intakeGetterSlot0Configs = new Slot0Configs()
|
||||
public static final double intakeRotaterShootAssistPeriodSeconds = 0.4;
|
||||
public static final double intakeRotaterShootAssistUpDutyCycle = 0.5;
|
||||
|
||||
public static final Slot0Configs intakeRollerSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.00).withKI(0.00).withKD(0.00)
|
||||
.withKV(0.00).withKA(0.00).withKS(0.00);
|
||||
// getter 如果你只用固定速度开关，也可以先不启闭环；若用闭环速度，这里 kP/kV/KS 等需要实测调参(这句fromAI)
|
||||
public static final Slot0Configs intakeGetterSlot0Configs = intakeRollerSlot0Configs;
|
||||
|
||||
public static final Slot0Configs intakeRotaterSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.00).withKI(0.00).withKD(0.00)
|
||||
.withKV(0.00).withKA(0.00).withKS(0.00);
|
||||
//如果 rotater 受重力影响明显，可能需要用 withKG/withGravityType/withGravityArmPositionOffset（看机构形式）(这句fromAI)
|
||||
|
||||
public static final double IntakeGetterSpeedforAuto = 0.5;
|
||||
// 单位RPS
|
||||
public static final double positionToleranceRotations = 0.00;
|
||||
//rotater容差
|
||||
|
||||
// 这里给出可用的占位值，真车调好位置闭环后再收紧。
|
||||
}
|
||||
public static class Climber {
|
||||
|
||||
public static final int motorID = 49;
|
||||
public static class Conveyor {
|
||||
public static final int mainConveyorID = 42;
|
||||
public static final double feedVoltage = 2.5;
|
||||
public static final double reverseVoltage = -2.0;
|
||||
|
||||
public static final boolean inverted = false;
|
||||
public static final Slot0Configs mainConveyorSlot0Configs = new Slot0Configs()
|
||||
.withKP(0.00).withKI(0.00).withKD(0.00)
|
||||
.withKV(0.00).withKA(0.00).withKS(0.00);
|
||||
}
|
||||
|
||||
public static double climbUpTicks = 94; // TODO: 实测后调整
|
||||
public static double climbDownTicks = 0; // TODO: 实测后调整
|
||||
public static double climbDownAutoTicks = 0; // TODO: 实测后调整，自动爬时可能不需要完全降到底
|
||||
public static final double climbVelocity = -200;
|
||||
public static final double holdToleranceRot = 0.5;
|
||||
public static final double deltaDownTicks = 1;
|
||||
public static class Superstructure {
|
||||
// 状态机时间和门控容差。真车基础动作验证后，需要现场调这些值。
|
||||
public static final double shooterFeedVoltage = Shooter.shooterConveyorFeedVoltage;
|
||||
public static final double shooterReverseVoltage = Shooter.shooterConveyorReverseVoltage;
|
||||
public static final double shootTimeoutSeconds = 1.0;
|
||||
public static final double aimToleranceDegrees = 1.5;
|
||||
public static final double stationarySpeedToleranceMetersPerSecond = 0.5;
|
||||
public static final double passBallFlywheelVoltage = Shooter.defaultFlywheelVoltage;
|
||||
public static final double passBallBackboardPosition = 175.0;
|
||||
|
||||
public static final Slot0Configs slot0Configs = new Slot0Configs();
|
||||
|
||||
static {
|
||||
// TODO: PID/F 值你们实测后填
|
||||
slot0Configs.kP = 0.2;
|
||||
slot0Configs.kI = 0.0;
|
||||
slot0Configs.kD = 0.0;
|
||||
|
||||
// TODO: 如果要前馈（速度闭环一般建议有 kV）
|
||||
slot0Configs.kS = 0.0;
|
||||
slot0Configs.kV = 0.1;
|
||||
slot0Configs.kA = 0.0;
|
||||
// 射击时背板的目标位置（外接编码器计数）。
|
||||
public static final double backboardShootPosition = 175.0;
|
||||
// 射击门控是否要求底盘接近静止。
|
||||
public static final boolean stationaryGateEnabled = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
// 版权所有 (c) FIRST 和其他 WPILib 贡献者。
|
||||
// 本项目为开源软件；你可以依据项目根目录中的 WPILib BSD license 文件
|
||||
// 修改和/或分享本代码。
|
||||
|
||||
package frc.robot;
|
||||
|
||||
|
||||
@@ -1,14 +1,11 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
// 版权所有 (c) FIRST 和其他 WPILib 贡献者。
|
||||
// 本项目为开源软件；你可以依据项目根目录中的 WPILib BSD license 文件
|
||||
// 修改和/或分享本代码。
|
||||
|
||||
package frc.robot;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.TimedRobot;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.CommandScheduler;
|
||||
import frc.robot.subsystems.ShooterSubsystem;
|
||||
|
||||
public class Robot extends TimedRobot {
|
||||
private Command m_autonomousCommand;
|
||||
@@ -23,15 +20,20 @@ public class Robot extends TimedRobot {
|
||||
public void robotPeriodic() {
|
||||
CommandScheduler.getInstance().run();
|
||||
m_robotContainer.tunerPeriodic();
|
||||
m_robotContainer.dashboardPeriodic();
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void disabledInit() {
|
||||
System.out.println("Robot disabledInit called");
|
||||
// 进入 disabled 时强制所有重构后的机构回到安全状态，
|
||||
// 即使上一周期还有手动或自动请求也不能继续动作。
|
||||
m_robotContainer.superstructure.requestIdle();
|
||||
m_robotContainer.shooterSubsystem.stopAll();
|
||||
// m_robotContainer.climberSubsystem.stopAll();
|
||||
m_robotContainer.IntakerSubsystem.stopAll();
|
||||
m_robotContainer.intakeRollerSubsystem.stop();
|
||||
m_robotContainer.intakeRotaterSubsystem.stop();
|
||||
m_robotContainer.conveyorSubsystem.stop();
|
||||
}
|
||||
|
||||
@Override
|
||||
|
||||
@@ -9,94 +9,101 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
|
||||
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
||||
import frc.robot.Constants.RobotStatus;
|
||||
import frc.robot.controls.Autocommand;
|
||||
import frc.robot.controls.DriveControls;
|
||||
import frc.robot.controls.IntakerControls;
|
||||
import frc.robot.controls.ShooterControls;
|
||||
import frc.robot.controls.OperatorControls;
|
||||
import frc.robot.generated.TunerConstants;
|
||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
||||
import frc.robot.subsystems.IntakerSubsystem;
|
||||
import frc.robot.subsystems.ConveyorSubsystem;
|
||||
import frc.robot.subsystems.IntakeRollerSubsystem;
|
||||
import frc.robot.subsystems.IntakeRotaterSubsystem;
|
||||
import frc.robot.subsystems.LEDSubsystem;
|
||||
import frc.robot.subsystems.LimelightSubsystem;
|
||||
import frc.robot.subsystems.ShooterSubsystem;
|
||||
import frc.robot.subsystems.Superstructure;
|
||||
import frc.robot.tuning.ConfigTalonFXMotorTuner;
|
||||
import frc.robot.tuning.ConfigTalonFXSMotorTuner;
|
||||
import frc.robot.tuning.DriveGainsTuner;
|
||||
import frc.robot.utils.GameData2026;
|
||||
import frc.robot.utils.LedBindings;
|
||||
import frc.robot.controls.ClimberControls;
|
||||
import frc.robot.controls.Autocommand;
|
||||
import frc.robot.subsystems.ClimberSubsystem;
|
||||
|
||||
public class RobotContainer {
|
||||
|
||||
/* ====================== */
|
||||
/* 手柄 */
|
||||
/* ====================== */
|
||||
|
||||
// Demo mode uses one Xbox controller on port 0.
|
||||
// 使用两个 Xbox 手柄：0 号给底盘驾驶，1 号给机构操作员。
|
||||
private final CommandXboxController controllerlower = new CommandXboxController(0);
|
||||
private final CommandXboxController controllerupper = controllerlower;
|
||||
private final CommandXboxController controllerupper = new CommandXboxController(1);
|
||||
|
||||
/* ====================== */
|
||||
/* 子系统 */
|
||||
/* ====================== */
|
||||
|
||||
// Phoenix TunerX 生成的底盘
|
||||
// 这里继续使用 Phoenix TunerX 生成的底盘，避免改动已经验证过的 swerve 底层。
|
||||
public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
|
||||
//public final ClimberSubsystem climberSubsystem = new ClimberSubsystem();
|
||||
public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
|
||||
public final IntakerSubsystem IntakerSubsystem = new IntakerSubsystem();
|
||||
public final IntakeRollerSubsystem intakeRollerSubsystem = new IntakeRollerSubsystem();
|
||||
public final IntakeRotaterSubsystem intakeRotaterSubsystem = new IntakeRotaterSubsystem();
|
||||
public final ConveyorSubsystem conveyorSubsystem = new ConveyorSubsystem();
|
||||
public final LimelightSubsystem limelightSubsystem = new LimelightSubsystem();
|
||||
|
||||
// LED 子系统
|
||||
private final LEDSubsystem leds = new LEDSubsystem(0, 72 ,0); // 总长度=72+72
|
||||
// Superstructure 是所有得分机构的唯一调度器。
|
||||
// 控制层和自动命令只能向它请求状态，不能直接命令电机。
|
||||
public final Superstructure superstructure = new Superstructure(
|
||||
shooterSubsystem,
|
||||
intakeRollerSubsystem,
|
||||
intakeRotaterSubsystem,
|
||||
conveyorSubsystem,
|
||||
drivetrain,
|
||||
limelightSubsystem
|
||||
);
|
||||
|
||||
// LED 子系统仍然只负责灯带显示，不参与机构状态判断。
|
||||
private final LEDSubsystem leds = new LEDSubsystem(0, 72, 0); // 总长度 = 72 + 72
|
||||
|
||||
private final RobotStatusManager robotStatusManager = new RobotStatusManager();
|
||||
/* ====================== */
|
||||
/* 控制封装/日志 */
|
||||
/* 控制封装/调参 */
|
||||
/* ====================== */
|
||||
|
||||
public final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, robotStatusManager );
|
||||
private final ShooterControls shooterControls = new ShooterControls(shooterSubsystem, controllerupper,robotStatusManager);
|
||||
//private final ClimberControls climberControls = new ClimberControls(climberSubsystem, controllerupper);
|
||||
private final IntakerControls intakerControls = new IntakerControls(IntakerSubsystem, shooterSubsystem, controllerupper);
|
||||
public final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, superstructure);
|
||||
|
||||
// 操作员意图层替代旧的射击/进球分离控制类，让复杂动作判断集中留在 Superstructure。
|
||||
private final OperatorControls operatorControls = new OperatorControls(superstructure, shooterSubsystem);
|
||||
private final DriveGainsTuner driveGainsTuner = new DriveGainsTuner(drivetrain);
|
||||
private ConfigTalonFXMotorTuner configFlywheelTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.flywheelMotorLeft, "flywheel", Constants.Shooter.flyWheelSlot0Configs);
|
||||
private ConfigTalonFXMotorTuner configConveyorTuner = new ConfigTalonFXMotorTuner(shooterSubsystem.conveyorMotor, "conveyor", Constants.Shooter.conveyorSlot0Configs);
|
||||
private ConfigTalonFXSMotorTuner configBackboardTuner = new ConfigTalonFXSMotorTuner(shooterSubsystem.backboardMotor, "backboard", Constants.Shooter.backboardSlot0Configs);
|
||||
private final GameData2026 gameData2026 = new GameData2026();
|
||||
private ConfigTalonFXMotorTuner configFlywheelTuner =
|
||||
new ConfigTalonFXMotorTuner(shooterSubsystem.leftFlywheel, "flywheel", Constants.Shooter.flyWheelSlot0Configs);
|
||||
private ConfigTalonFXMotorTuner configConveyorTuner =
|
||||
new ConfigTalonFXMotorTuner(shooterSubsystem.leftConveyor, "conveyor", Constants.Shooter.conveyorSlot0Configs);
|
||||
private ConfigTalonFXSMotorTuner configBackboardTuner =
|
||||
new ConfigTalonFXSMotorTuner(shooterSubsystem.backboardMotor, "backboard", Constants.Shooter.backboardSlot0Configs);
|
||||
private ConfigTalonFXMotorTuner configMainConveyorTuner =
|
||||
new ConfigTalonFXMotorTuner(conveyorSubsystem.mainConveyor, "mainconveyor", Constants.Conveyor.mainConveyorSlot0Configs);
|
||||
private ConfigTalonFXMotorTuner configIntakerRollerTuner =
|
||||
new ConfigTalonFXMotorTuner(intakeRollerSubsystem.leftIntakeRoller, "intakerroller", Constants.Intaker.intakeRollerSlot0Configs);
|
||||
private ConfigTalonFXMotorTuner configIntakerRotaterTuner =
|
||||
new ConfigTalonFXMotorTuner(intakeRotaterSubsystem.leftIntakeRotater, "intakerrotater", Constants.Intaker.intakeRotaterSlot0Configs);
|
||||
|
||||
private final SendableChooser<Command> autoChooser;
|
||||
|
||||
private final Trigger enableTrigger = new Trigger(DriverStation::isEnabled);
|
||||
|
||||
|
||||
|
||||
public RobotContainer() {
|
||||
|
||||
Pose2d startingPose = Constants.StartingPoints.Red.Point2;
|
||||
drivetrain.resetPose(startingPose);
|
||||
configueSwerve();
|
||||
configueShooter();
|
||||
// configueClimber();
|
||||
bingdingLED();
|
||||
configueIntaker();
|
||||
// Autocommand.preNameCommands(
|
||||
// climberSubsystem,
|
||||
// IntakerSubsystem,
|
||||
// drivetrain,
|
||||
// shooterSubsystem,
|
||||
// driveControls,
|
||||
// shooterControls
|
||||
// );
|
||||
|
||||
Autocommand.preNameCommands(
|
||||
IntakerSubsystem,
|
||||
drivetrain,
|
||||
shooterSubsystem,
|
||||
superstructure,
|
||||
intakeRotaterSubsystem,
|
||||
driveControls,
|
||||
shooterControls
|
||||
operatorControls
|
||||
);
|
||||
SignalLogger.enableAutoLogging(false);
|
||||
autoChooser = AutoBuilder.buildAutoChooser();
|
||||
@@ -112,86 +119,74 @@ public class RobotContainer {
|
||||
Pose2d startingPose = Constants.StartingPoints.Red.Point2;
|
||||
drivetrain.resetPose(startingPose);
|
||||
|
||||
// 默认驾驶命令
|
||||
/*
|
||||
* LeftX 底盘左右
|
||||
* LeftY 底盘前后 Bump/Trench偏移未完成
|
||||
* RightX Aiming偏移 AllTelop模式下 底盘旋转
|
||||
*
|
||||
*/
|
||||
// 默认驾驶命令说明：
|
||||
// 左摇杆 X 控制底盘左右平移。
|
||||
// 左摇杆 Y 控制底盘前后平移，越障/壕沟偏移逻辑目前仍沿用旧状态。
|
||||
// 右摇杆 X 在自动瞄准模式下作为偏移量，在全手动模式下控制底盘旋转。
|
||||
drivetrain.setDefaultCommand(driveControls.defaultDriveCommand());
|
||||
|
||||
// Disabled 时进入 idle（防止模块乱动）
|
||||
// 禁用状态下进入空闲命令，防止模块继续响应上一周期的运动请求。
|
||||
RobotModeTriggers.disabled().whileTrue(driveControls.idleCommand());
|
||||
|
||||
if (!Constants.DemoMode.ENABLED) {
|
||||
|
||||
|
||||
//左保险：按住刹车
|
||||
// 左保险：按住刹车。
|
||||
controllerlower.leftBumper()
|
||||
.onTrue(Commands.runOnce(driveControls::emergencyStop))
|
||||
.whileTrue(driveControls.brakeWhileHeld());
|
||||
|
||||
// 右保险：按住加速（Boost）
|
||||
// 右保险：按住开启加速模式，松开后恢复普通速度。
|
||||
controllerlower.rightBumper()
|
||||
.onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true)))
|
||||
.onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false)));
|
||||
|
||||
// Back：重置场地坐标系角度
|
||||
// Back 键：重置场地坐标系角度。
|
||||
controllerlower.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
|
||||
|
||||
// Start：强行使用metaTag2全场定位
|
||||
// Start 键：强行使用 shooter 侧 Limelight 的 MegaTag2 全场定位。
|
||||
controllerlower.start().whileTrue(drivetrain.run(drivetrain::forceUsingLimelightmt2));
|
||||
|
||||
controllerlower.a().whileTrue(drivetrain.run(()->{drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Shooter);
|
||||
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);
|
||||
controllerlower.b().whileTrue(drivetrain.run(() -> {
|
||||
drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Intaker);
|
||||
System.out.println("Try using intaker for metatag2");
|
||||
}));
|
||||
//LM 按下时autoaimming，松开时alltelop
|
||||
// controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
|
||||
// controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
|
||||
|
||||
//双操均可触发autoaimming模式，松开时alltelop
|
||||
Trigger autoAim = controllerupper.rightBumper();//.or(controllerlower.rightStick());
|
||||
autoAim.onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming));
|
||||
autoAim.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
|
||||
// 预留的左摇杆自动瞄准切换，当前保持注释，避免改变驾驶员已习惯的按键。
|
||||
// controllerlower.leftStick().onTrue(operatorControls.requestAimHubCommand());
|
||||
// controllerlower.leftStick().onFalse(operatorControls.requestIdleCommand());
|
||||
|
||||
//双操均可触发passingball模式，松开时alltelop
|
||||
Trigger autoAim2 = controllerupper.leftBumper();//.or(controllerlower.leftStick());
|
||||
autoAim2.onTrue(robotStatusManager.setStatusCommand(RobotStatus.PassingBall));
|
||||
autoAim2.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
|
||||
// 上操作员右保险：按住进入自动瞄准模式，松开回到全手动模式。
|
||||
Trigger autoAim = controllerupper.rightBumper(); // 可按需要再并入驾驶员左摇杆。
|
||||
autoAim.onTrue(operatorControls.requestAimHubCommand());
|
||||
autoAim.onFalse(operatorControls.requestIdleCommand());
|
||||
|
||||
controllerlower.leftTrigger()
|
||||
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench))
|
||||
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
|
||||
// 上操作员左保险：按住进入传球模式，松开回到全手动模式。
|
||||
Trigger autoAim2 = controllerupper.leftBumper(); // 可按需要再并入驾驶员左摇杆。
|
||||
autoAim2.onTrue(operatorControls.requestPassBallCommand());
|
||||
autoAim2.onFalse(operatorControls.requestIdleCommand());
|
||||
|
||||
controllerlower.rightTrigger()
|
||||
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump))
|
||||
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
|
||||
|
||||
controllerlower.a()
|
||||
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.Climbing))
|
||||
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
|
||||
}
|
||||
}
|
||||
|
||||
private void configueShooter(){
|
||||
shooterSubsystem.setDefaultCommand(shooterControls.defaultShooterCommand());
|
||||
controllerupper.povLeft().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(-1)));
|
||||
controllerupper.povRight().onTrue(Commands.runOnce(() -> shooterControls.adjustFlywheelSpeedOffset(1)));
|
||||
controllerupper.povUp().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(100)));
|
||||
controllerupper.povDown().onTrue(Commands.runOnce(() -> shooterControls.adjustBackboardRateOffset(-100)));
|
||||
controllerupper.back().onTrue(Commands.runOnce(() -> shooterControls.resetOffsets()));
|
||||
if (Constants.DemoMode.ENABLED) {
|
||||
controllerupper.rightTrigger()
|
||||
.onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming))
|
||||
.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop));
|
||||
} else {
|
||||
controllerupper.start().onTrue(shooterControls.resetBackboardCommand());
|
||||
}
|
||||
|
||||
private void configueShooter() {
|
||||
// 射击相关按钮只请求 Superstructure 状态；是否喂球由 Superstructure
|
||||
// 根据飞轮速度和底盘瞄准统一判断。
|
||||
// 这些命令 require Superstructure，与自动程序共用需求；
|
||||
// 用 teleop 门控避免自动赛阶段误触直接取消整个 PathPlanner 自动。
|
||||
Trigger teleopOnly = RobotModeTriggers.teleop();
|
||||
controllerupper.rightTrigger().and(teleopOnly)
|
||||
.onTrue(operatorControls.requestShootHubCommand())
|
||||
.onFalse(operatorControls.requestIdleCommand());
|
||||
controllerupper.b().and(teleopOnly)
|
||||
.onTrue(operatorControls.requestEjectCommand())
|
||||
.onFalse(operatorControls.requestIdleCommand());
|
||||
controllerupper.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVoltageOffset(-0.1)));
|
||||
controllerupper.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelVoltageOffset(0.1)));
|
||||
controllerupper.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());
|
||||
}
|
||||
|
||||
private void bingdingLED() {
|
||||
@@ -199,9 +194,6 @@ public class RobotContainer {
|
||||
}
|
||||
|
||||
public Command getAutonomousCommand() {
|
||||
if (Constants.DemoMode.ENABLED) {
|
||||
return Commands.none();
|
||||
}
|
||||
return autoChooser.getSelected();
|
||||
}
|
||||
|
||||
@@ -210,20 +202,41 @@ public class RobotContainer {
|
||||
configFlywheelTuner.periodic(() -> shooterSubsystem.applyLeftConfigurationToRight());
|
||||
configConveyorTuner.periodic(null);
|
||||
configBackboardTuner.periodic();
|
||||
shooterSubsystem.setRightFollowLeft();
|
||||
shooterSubsystem.debugBackboardTargetPeriodic();
|
||||
configMainConveyorTuner.periodic(null);
|
||||
configIntakerRollerTuner.periodic(null);
|
||||
configIntakerRotaterTuner.periodic(null);
|
||||
shooterSubsystem.setRightConveyorFollowLeft();
|
||||
}
|
||||
|
||||
// private void configueClimber() {
|
||||
// climberSubsystem.setDefaultCommand(climberControls.defaultClimberCommand());
|
||||
// }
|
||||
public void dashboardPeriodic() {
|
||||
gameData2026.periodic();
|
||||
}
|
||||
|
||||
private void configueIntaker() {
|
||||
IntakerSubsystem.setDefaultCommand(intakerControls.defaultIntakerCommand());
|
||||
//controllerupper.a().onTrue(intakerControls.resetIntakerRotaterEncoderCommand(IntakerSubsystem));
|
||||
// 进球相关按钮也只请求 Superstructure 状态；intaker 开赛后默认保持下放。
|
||||
Trigger teleopOnly = RobotModeTriggers.teleop();
|
||||
controllerupper.leftTrigger().and(teleopOnly)
|
||||
.onTrue(operatorControls.requestIntakeCommand())
|
||||
.onFalse(operatorControls.requestIdleCommand());
|
||||
|
||||
Trigger manualRaiseIntaker = controllerupper.x().and(teleopOnly);
|
||||
manualRaiseIntaker
|
||||
.onTrue(operatorControls.requestManualCommand())
|
||||
.whileTrue(operatorControls.manualRaiseIntakeRotaterCommand(intakeRotaterSubsystem))
|
||||
.onFalse(operatorControls.requestIdleCommand());
|
||||
|
||||
Trigger manualLowerIntaker = controllerupper.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()
|
||||
));
|
||||
}
|
||||
|
||||
// public ClimberSubsystem getClimberSubsystem() {
|
||||
// return climberSubsystem;
|
||||
// }
|
||||
|
||||
}
|
||||
|
||||
@@ -1,25 +0,0 @@
|
||||
package frc.robot;
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import frc.robot.Constants.RobotStatus;
|
||||
|
||||
public class RobotStatusManager {
|
||||
private RobotStatus robotStatus;
|
||||
|
||||
public RobotStatusManager(){
|
||||
this.robotStatus = RobotStatus.AllTelop;
|
||||
}
|
||||
|
||||
public RobotStatus getStatus(){
|
||||
return this.robotStatus;
|
||||
}
|
||||
|
||||
public void setStatus(RobotStatus rs){
|
||||
this.robotStatus = rs;
|
||||
}
|
||||
|
||||
public Command setStatusCommand(RobotStatus rs){
|
||||
return Commands.runOnce(()->setStatus(rs));
|
||||
}
|
||||
}
|
||||
@@ -2,188 +2,76 @@ package frc.robot.controls;
|
||||
|
||||
import com.pathplanner.lib.auto.NamedCommands;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.Subsystem;
|
||||
import frc.robot.subsystems.ClimberSubsystem;
|
||||
import frc.robot.RobotStatusManager;
|
||||
import frc.robot.subsystems.IntakerSubsystem;
|
||||
import frc.robot.subsystems.ShooterSubsystem;
|
||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
||||
import frc.robot.Constants;
|
||||
import frc.robot.Constants.RobotStatus;
|
||||
|
||||
public class Autocommand extends SequentialCommandGroup{
|
||||
import frc.robot.subsystems.IntakeRotaterSubsystem;
|
||||
import frc.robot.subsystems.Superstructure;
|
||||
|
||||
/**
|
||||
* 注册 PathPlanner 的 NamedCommands。
|
||||
*
|
||||
* 自动路径只请求机器人级动作，不直接设置 shooter、intake 或 conveyor 电机；
|
||||
* 这样自动和手动都会走同一套 Superstructure 状态机。
|
||||
*/
|
||||
public class Autocommand extends SequentialCommandGroup {
|
||||
public static void preNameCommands(
|
||||
// ClimberSubsystem climber,
|
||||
IntakerSubsystem intaker,
|
||||
CommandSwerveDrivetrain drivetrain,
|
||||
ShooterSubsystem shooter,
|
||||
Superstructure superstructure,
|
||||
IntakeRotaterSubsystem intakeRotater,
|
||||
DriveControls driveControls,
|
||||
ShooterControls shooterControls
|
||||
OperatorControls operatorControls
|
||||
) {
|
||||
// NamedCommands.registerCommand("climb_up", climb_up(climber));
|
||||
// NamedCommands.registerCommand("climb_down", climb_down(climber));
|
||||
NamedCommands.registerCommand("start_intake", start_intake(intaker));
|
||||
NamedCommands.registerCommand("start_intake_and_conveyor", start_intake_and_conveyor(intaker,shooter));
|
||||
NamedCommands.registerCommand("stop_intake_and_conveyor", stop_intake_and_conveyor(intaker,shooter));
|
||||
NamedCommands.registerCommand("start_intake_timelimit", start_intake_timelimit(intaker));
|
||||
NamedCommands.registerCommand("stop_intake", stop_intake(intaker));
|
||||
NamedCommands.registerCommand("intaker_down", intaker_down(intaker));
|
||||
NamedCommands.registerCommand("intaker_up", intaker_up(intaker));
|
||||
//NamedCommands.registerCommand("auto_align_to_climb", autoAlignToClimb(drivetrain));
|
||||
NamedCommands.registerCommand("shoot", shoot(shooter));
|
||||
NamedCommands.registerCommand("start_intake", startIntake(superstructure));
|
||||
NamedCommands.registerCommand("start_intake_and_conveyor", startIntake(superstructure));
|
||||
NamedCommands.registerCommand("stop_intake_and_conveyor", idle(superstructure));
|
||||
NamedCommands.registerCommand("start_intake_timelimit", timedIntake(superstructure));
|
||||
NamedCommands.registerCommand("stop_intake", idle(superstructure));
|
||||
NamedCommands.registerCommand("intaker_down", lowerIntaker(intakeRotater));
|
||||
NamedCommands.registerCommand("intaker_up", raiseIntaker(intakeRotater));
|
||||
NamedCommands.registerCommand("shoot_hub", shootHub(superstructure));
|
||||
NamedCommands.registerCommand("Shake", shake(driveControls));
|
||||
NamedCommands.registerCommand("AutoAim", autoAim(driveControls));
|
||||
NamedCommands.registerCommand("AutoShoot", autoShoot(shooterControls));
|
||||
// NamedCommands.registerCommand("climber_init", climber_Zeroencoder(climber));
|
||||
NamedCommands.registerCommand("reset_backboard0", resetBackboard0Command(shooterControls));
|
||||
|
||||
NamedCommands.registerCommand("reset_backboard0", operatorControls.resetBackboard0Command());
|
||||
}
|
||||
|
||||
public static Command climb_up(ClimberSubsystem climber) {
|
||||
return new RunCommand(() -> {
|
||||
climber.climbUp(); // 控制爬升器上升
|
||||
}, climber).withTimeout(2).andThen(new RunCommand(() -> {
|
||||
climber.climbUpNonStopforAuto();
|
||||
}, climber).withTimeout(0.5)).andThen(new InstantCommand(() -> {
|
||||
climber.captureHoldPosition(); // 捕获当前爬升器位置以保持
|
||||
climber.holdPosition(); // 停止并锁定当前爬升器位置
|
||||
}, climber));
|
||||
}
|
||||
public static Command climb_down(ClimberSubsystem climber) {
|
||||
return new RunCommand(() -> {
|
||||
climber.climbDown(); // 控制爬升器下降
|
||||
}, climber).withTimeout(2.0)
|
||||
.andThen(new InstantCommand(() -> {
|
||||
climber.captureHoldPosition(); // 捕获当前爬升器位置以保持
|
||||
climber.holdPosition(); // 停止并锁定当前爬升器位置
|
||||
}, climber));
|
||||
public static Command startIntake(Superstructure superstructure) {
|
||||
return superstructure.requestIntakeCommand();
|
||||
}
|
||||
|
||||
public static Command start_intake(IntakerSubsystem intaker) {
|
||||
return new InstantCommand(() -> {
|
||||
intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 启动吸球
|
||||
}, intaker);
|
||||
public static Command timedIntake(Superstructure superstructure) {
|
||||
return Commands.sequence(
|
||||
superstructure.requestIntakeCommand(),
|
||||
Commands.waitSeconds(1.5),
|
||||
superstructure.requestIdleCommand()
|
||||
);
|
||||
}
|
||||
public static Command idle(Superstructure superstructure) {
|
||||
return superstructure.requestIdleCommand();
|
||||
}
|
||||
public static Command start_intake_and_conveyor(IntakerSubsystem intaker, ShooterSubsystem shooter) {
|
||||
return new InstantCommand(() -> {
|
||||
shooter.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeedForAuto);
|
||||
intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 启动吸球
|
||||
|
||||
}, intaker, shooter);
|
||||
public static Command lowerIntaker(IntakeRotaterSubsystem intakeRotater) {
|
||||
return intakeRotater.lowerForMatchCommand();
|
||||
}
|
||||
public static Command stop_intake_and_conveyor(IntakerSubsystem intaker, ShooterSubsystem shooter) {
|
||||
return new InstantCommand(() -> {
|
||||
intaker.setIntakerGetterSpeed(0); // 启动吸球
|
||||
shooter.setConveyorSpeedByRPS(0);
|
||||
}, intaker, shooter);
|
||||
}
|
||||
public static Command start_intake_timelimit(IntakerSubsystem intaker) {
|
||||
return new RunCommand(() -> {
|
||||
intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 吸球机构下降
|
||||
}, intaker).withTimeout(1.5).andThen(new InstantCommand(() -> {
|
||||
intaker.setIntakerGetterSpeed(0); // 停止旋转
|
||||
}, intaker));
|
||||
}
|
||||
public static Command stop_intake(IntakerSubsystem intaker) {
|
||||
return new RunCommand(() -> {
|
||||
intaker.setIntakerGetterSpeed(0); // 停止吸球
|
||||
}, intaker);
|
||||
}
|
||||
public static Command intaker_down(IntakerSubsystem intaker) {
|
||||
return new RunCommand(() -> {
|
||||
intaker.IntakerDownNonStop(); // 吸球机构下降
|
||||
}, intaker).withTimeout(0.6)
|
||||
|
||||
.andThen(new RunCommand(() -> {
|
||||
intaker.IntakerUpNonStop(); // 吸球机构上升
|
||||
}, intaker).withTimeout(0.1))
|
||||
public static Command raiseIntaker(IntakeRotaterSubsystem intakeRotater) {
|
||||
return intakeRotater.raiseForMatchCommand();
|
||||
}
|
||||
|
||||
.andThen(new InstantCommand(() -> {
|
||||
intaker.stopIntakerotater(); // 停止
|
||||
}, intaker));
|
||||
// 自动射击和手动射击使用同一套 gated shooting 流程。
|
||||
public static Command shootHub(Superstructure superstructure) {
|
||||
return Commands.sequence(
|
||||
superstructure.requestShootHubCommand(),
|
||||
Commands.waitSeconds(Constants.Superstructure.shootTimeoutSeconds),
|
||||
superstructure.requestIdleCommand()
|
||||
);
|
||||
}
|
||||
public static Command intaker_up(IntakerSubsystem intaker) {
|
||||
return new RunCommand(() -> {
|
||||
intaker.IntakerUpNonStop(); // 吸球机构上升
|
||||
}, intaker).withTimeout(0.4).andThen(new InstantCommand(() -> {
|
||||
intaker.stopIntakerotater(); // 停止旋转
|
||||
}, intaker));
|
||||
}
|
||||
// public static Command autoAlignToClimb(CommandSwerveDrivetrain drivetrain) {
|
||||
// return Commands.sequence(
|
||||
// new InstantCommand(() -> {
|
||||
// LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
|
||||
// }),
|
||||
// Commands.runEnd(
|
||||
// () -> {
|
||||
// drivetrain.driveToAprilTag();
|
||||
// },
|
||||
// () -> {
|
||||
// LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
|
||||
// },
|
||||
// drivetrain
|
||||
// ).until(() -> drivetrain.isAlignedToAprilTag()));
|
||||
// }
|
||||
|
||||
public static Command shoot(ShooterSubsystem shooter) {
|
||||
return new RunCommand(() -> {
|
||||
shooter.setFlywheelSpeedByRPS(64);
|
||||
shooter.setBackboardPosition(175); // 设置自动射击转速
|
||||
if(shooter.isBackboardAtTarget()){
|
||||
shooter.backboardMotor.set(0);
|
||||
}else{
|
||||
shooter.outputBackboard();
|
||||
}
|
||||
}, shooter).withTimeout(0.25).andThen(new RunCommand(() -> {
|
||||
shooter.setFlywheelSpeedByRPS(64);
|
||||
shooter.setBackboardPosition(175);
|
||||
if(shooter.isBackboardAtTarget()){
|
||||
shooter.backboardMotor.set(0);
|
||||
}else{
|
||||
shooter.outputBackboard();
|
||||
}
|
||||
shooter.setConveyorSpeedByRPS(25);
|
||||
}, shooter).withTimeout(1.5).andThen(new InstantCommand(() -> {
|
||||
shooter.setFlywheelSpeedByRPS(0); // 停止射击
|
||||
//shooter.setBackboardPosition(0); // 恢复背板位置
|
||||
shooter.setConveyorSpeedByRPS(0); // 停止输送
|
||||
}, shooter)));
|
||||
}
|
||||
public static Command autoAim(DriveControls driveControls) {
|
||||
return driveControls.autoAimCommand();
|
||||
}
|
||||
|
||||
public static Command autoShoot(ShooterControls shooterControl) {
|
||||
return shooterControl.autoShootToHubCommand();
|
||||
}
|
||||
public static Command resetBackboard0Command(ShooterControls shooterControl) {
|
||||
return shooterControl.resetBackboard0Command();
|
||||
}
|
||||
|
||||
public static Command shake(DriveControls driveControls) {
|
||||
return driveControls.shakeCommand();
|
||||
}
|
||||
|
||||
public static Command climber_Zeroencoder(ClimberSubsystem climber) {
|
||||
return climber.initCommand();
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
@@ -1,57 +0,0 @@
|
||||
package frc.robot.controls;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import frc.robot.Constants;
|
||||
import frc.robot.subsystems.ClimberSubsystem;
|
||||
|
||||
|
||||
public class ClimberControls {
|
||||
|
||||
private final ClimberSubsystem climber;
|
||||
private final CommandXboxController controller;
|
||||
|
||||
private final NetworkTable table;
|
||||
|
||||
public ClimberControls(ClimberSubsystem climber, CommandXboxController controller) {
|
||||
this.climber = climber;
|
||||
this.controller = controller;
|
||||
this.table = NetworkTableInstance.getDefault().getTable("ClimberControls");
|
||||
}
|
||||
|
||||
public Command defaultClimberCommand() {
|
||||
return Commands.run(()->{
|
||||
//climber.climbUp();
|
||||
if(controller.getRightY() < -0.5){
|
||||
climber.climbUpNonStop();
|
||||
} else if(controller.getRightY() > 0.5){
|
||||
climber.climbDownNonStop();
|
||||
} else if (controller.y().getAsBoolean()) {
|
||||
climber.climbUp();
|
||||
} else if (controller.x().getAsBoolean()) {
|
||||
climber.climbDown();
|
||||
} else {
|
||||
// 松开：记录当前位置 + 锁死
|
||||
climber.captureHoldPosition();
|
||||
climber.holdPosition();
|
||||
}
|
||||
|
||||
|
||||
// 调试用
|
||||
table.getEntry("xHeld_up").setBoolean(controller.x().getAsBoolean());
|
||||
table.getEntry("yHeld_down").setBoolean(controller.y().getAsBoolean());
|
||||
}, climber);
|
||||
|
||||
}
|
||||
|
||||
public Command resetClimberEncoderCommand(ClimberSubsystem climber) {
|
||||
return Commands.runOnce(climber::zeroEncoder, climber);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -15,11 +15,11 @@ import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import frc.robot.Constants;
|
||||
import frc.robot.RobotStatusManager;
|
||||
import frc.robot.Constants.DriveMode;
|
||||
import frc.robot.Constants.RobotStatus;
|
||||
import frc.robot.generated.TunerConstants;
|
||||
import frc.robot.subsystems.CommandSwerveDrivetrain;
|
||||
import frc.robot.subsystems.Superstructure;
|
||||
import frc.robot.subsystems.Superstructure.WantedState;
|
||||
|
||||
import java.util.Optional;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
@@ -36,35 +36,35 @@ public class DriveControls {
|
||||
private final double maxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
|
||||
private final double maxAngularRate = RotationsPerSecond.of(0.75).in(edu.wpi.first.units.Units.RadiansPerSecond);
|
||||
|
||||
// 默认速度比例（平时开车）
|
||||
// 默认速度比例，普通驾驶使用。
|
||||
private double normalDriveScale = 0.3;
|
||||
private double normalTurnScale = 0.7;
|
||||
|
||||
// 加速模式比例（按住 RB）
|
||||
// 加速模式比例，按住 RB 时使用。
|
||||
private double boostDriveScale = 1.0;
|
||||
private double boostTurnScale = 1.0;
|
||||
//autoaim模式
|
||||
// 自动瞄准时降低平移速度。
|
||||
private double slowDriveScale =0.1;
|
||||
|
||||
private boolean boostEnabled = false;
|
||||
|
||||
private final SlewRateLimiter vxLimiter = new SlewRateLimiter(5.0); // m/s^2 等效（调）
|
||||
private final SlewRateLimiter vxLimiter = new SlewRateLimiter(5.0); // m/s^2 等效加速度限制
|
||||
private final SlewRateLimiter vyLimiter = new SlewRateLimiter(5.0);
|
||||
private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 等效（调）
|
||||
private final SlewRateLimiter omegaLimiter = new SlewRateLimiter(9.0); // rad/s^2 等效角加速度限制
|
||||
|
||||
// ====== 类成员变量里新增：记录上一次输出 ======
|
||||
// 记录上一周期输出，用于只限制加速、不限制减速。
|
||||
private double lastVx = 0.0;
|
||||
private double lastVy = 0.0;
|
||||
private double lastOmega = 0.0;
|
||||
|
||||
private NetworkTable autoControlNetworkTable;
|
||||
|
||||
private final RobotStatusManager robotStatusManager;
|
||||
private final Superstructure superstructure;
|
||||
public void setBoostEnabled(boolean enabled) {
|
||||
boostEnabled = enabled;
|
||||
}
|
||||
|
||||
// 新增：当前是否使用场地坐标系（默认 true）
|
||||
// 当前是否使用场地坐标系，默认启用。
|
||||
private boolean fieldCentricEnabled = true;
|
||||
|
||||
private final SwerveRequest.FieldCentric fieldCentric =
|
||||
@@ -86,10 +86,10 @@ public class DriveControls {
|
||||
.withRotationalDeadband(maxAngularRate * 0.05)
|
||||
.withDriveRequestType(DriveRequestType.Velocity);
|
||||
|
||||
public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver, RobotStatusManager rsm) {
|
||||
public DriveControls(CommandSwerveDrivetrain drivetrain, CommandXboxController driver, Superstructure superstructure) {
|
||||
this.drivetrain = drivetrain;
|
||||
this.driver = driver;
|
||||
this.robotStatusManager = rsm;
|
||||
this.superstructure = superstructure;
|
||||
autoControlNetworkTable = NetworkTableInstance.getDefault().getTable("AutoControl");
|
||||
}
|
||||
|
||||
@@ -101,25 +101,16 @@ public class DriveControls {
|
||||
return fieldCentricEnabled;
|
||||
}
|
||||
|
||||
/** 底盘的默认驾驶命令（teleop 期间持续执行） */
|
||||
/** 底盘默认驾驶命令，在 teleop 期间持续执行。 */
|
||||
public Command defaultDriveCommand() {
|
||||
return drivetrain.applyRequest(() -> {
|
||||
SmartDashboard.putString("robotStatus",robotStatusManager.getStatus().name());
|
||||
WantedState wantedState = superstructure.getWantedState();
|
||||
SmartDashboard.putString("wantedState", wantedState.name());
|
||||
double driveScale = boostEnabled ? boostDriveScale : normalDriveScale;
|
||||
double turnScale = boostEnabled ? boostTurnScale : normalTurnScale;
|
||||
if (Constants.DemoMode.ENABLED) {
|
||||
driveScale = boostEnabled ? Constants.DemoMode.boostDriveScale : Constants.DemoMode.normalDriveScale;
|
||||
turnScale = boostEnabled ? Constants.DemoMode.boostTurnScale : Constants.DemoMode.normalTurnScale;
|
||||
}
|
||||
double vx = -driver.getLeftY() * maxSpeed * driveScale;
|
||||
double vy = -driver.getLeftX() * maxSpeed * driveScale;
|
||||
double vomega = -driver.getRightX() * maxAngularRate * turnScale;
|
||||
if (Constants.DemoMode.ENABLED) {
|
||||
vx = clamp(vx, -Constants.DemoMode.maxLinearSpeedMetersPerSecond, Constants.DemoMode.maxLinearSpeedMetersPerSecond);
|
||||
vy = clamp(vy, -Constants.DemoMode.maxLinearSpeedMetersPerSecond, Constants.DemoMode.maxLinearSpeedMetersPerSecond);
|
||||
vomega = clamp(vomega, -Constants.DemoMode.maxAngularRateRadiansPerSecond, Constants.DemoMode.maxAngularRateRadiansPerSecond);
|
||||
}
|
||||
// ====== execute 内替换 limiter：只限加速、不限减速 ======
|
||||
double limitedVx = limitAccelOnly(vx, lastVx, vxLimiter);
|
||||
double limitedVy = limitAccelOnly(vy, lastVy, vyLimiter);
|
||||
double limitedOmega = limitAccelOnly(vomega, lastOmega, omegaLimiter);
|
||||
@@ -132,113 +123,40 @@ public class DriveControls {
|
||||
vy = limitedVy;
|
||||
vomega = limitedOmega;
|
||||
|
||||
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
|
||||
// 将距离值写入 NetworkTable
|
||||
autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
|
||||
autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
|
||||
autoControlNetworkTable.getEntry("distanceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[0]);
|
||||
autoControlNetworkTable.getEntry("angleDifferenceToPassball").setDouble(calculateDistanceAndRotationToPassBall()[1]);
|
||||
double[] hubAim = calculateDistanceAndRotationToHub();
|
||||
double[] passAim = calculateDistanceAndRotationToPassBall();
|
||||
autoControlNetworkTable.getEntry("distanceToHub").setDouble(hubAim[0]);
|
||||
autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(hubAim[1]);
|
||||
autoControlNetworkTable.getEntry("distanceToPassball").setDouble(passAim[0]);
|
||||
autoControlNetworkTable.getEntry("angleDifferenceToPassball").setDouble(passAim[1]);
|
||||
|
||||
RobotStatus currentStatus = robotStatusManager.getStatus();
|
||||
if (Constants.DemoMode.ENABLED
|
||||
&& currentStatus != RobotStatus.Stopped
|
||||
&& currentStatus != RobotStatus.AllTelop
|
||||
&& currentStatus != RobotStatus.Climbing) {
|
||||
currentStatus = RobotStatus.AllTelop;
|
||||
}
|
||||
double[] activeAim = wantedState == WantedState.PASS_BALL ? passAim : hubAim;
|
||||
drivetrain.setTargetHeading(
|
||||
drivetrain.getRotation().plus(Rotation2d.fromDegrees(activeAim[1])));
|
||||
|
||||
switch (currentStatus) {
|
||||
case Stopped:
|
||||
vx = 0;
|
||||
vy = 0;
|
||||
vomega = 0;
|
||||
break;
|
||||
case PassingBall:
|
||||
//LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
|
||||
vomega = calculateRotationSpeedFromRotationAngle(calculateDistanceAndRotationToPassBall()[1]);
|
||||
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
|
||||
break;
|
||||
case AutoAimming:
|
||||
//LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
|
||||
vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
|
||||
switch (wantedState) {
|
||||
case SHOOT_HUB:
|
||||
case AIM_HUB:
|
||||
vomega = calculateRotationSpeedFromRotationAngle(hubAim[1]);
|
||||
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
|
||||
vx = -driver.getLeftY() * maxSpeed * slowDriveScale;
|
||||
vy = -driver.getLeftX() * maxSpeed * slowDriveScale;
|
||||
break;
|
||||
case AllTelop:
|
||||
case PASS_BALL:
|
||||
vomega = calculateRotationSpeedFromRotationAngle(passAim[1]);
|
||||
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(vomega);
|
||||
break;
|
||||
default:
|
||||
autoControlNetworkTable.getEntry("autoRotationRate").setDouble(Double.NaN);
|
||||
break;
|
||||
case Climbing:
|
||||
// alignToClimb();
|
||||
break;
|
||||
case CrossingBump:
|
||||
double differenceBump = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.bumpY[0], Constants.AutoPositioning.bumpY[1]);
|
||||
if (Math.abs(differenceBump) < 0.1) {
|
||||
vy = 0;
|
||||
} else {
|
||||
double raw = differenceBump * Constants.AutoPositioning.autoPositioningkP;
|
||||
vy = Math.copySign(
|
||||
Math.max(Math.abs(raw), 0.4),
|
||||
raw
|
||||
);
|
||||
vy = isRedAlliance ? -vy : vy;
|
||||
}
|
||||
autoControlNetworkTable.getEntry("BumpDifference").setDouble(differenceBump);
|
||||
|
||||
Rotation2d BumpcurrentAngle = drivetrain.getState().Pose.getRotation();
|
||||
double BumpcurrentRad = BumpcurrentAngle.getRadians();
|
||||
double targetA = Math.toRadians(45);
|
||||
double targetB = Math.toRadians(-45);
|
||||
// 计算误差
|
||||
double errorA = targetA - BumpcurrentRad;
|
||||
double errorB = targetB - BumpcurrentRad;
|
||||
// 角度归一化到 [-pi, pi]
|
||||
errorA = Math.atan2(Math.sin(errorA), Math.cos(errorA));
|
||||
errorB = Math.atan2(Math.sin(errorB), Math.cos(errorB));
|
||||
// 选误差绝对值更小的那个
|
||||
double chosenError = (Math.abs(errorA) <= Math.abs(errorB)) ? errorA : errorB;
|
||||
vomega = calculatevomegaFromRotationAngle(chosenError);
|
||||
autoControlNetworkTable.getEntry("BumpAutoRotationRate").setDouble(vomega);
|
||||
//vomega = calculateRotationSpeedForBump();
|
||||
break;
|
||||
case CrossingTrench:
|
||||
double differenceTrench = calculateDifferenceToTwoTarget(drivetrain.getState().Pose.getY(), Constants.AutoPositioning.trenchY[0], Constants.AutoPositioning.trenchY[1]);
|
||||
if (Math.abs(differenceTrench) < 0.1) {
|
||||
vy = 0;
|
||||
} else {
|
||||
double raw = differenceTrench * Constants.AutoPositioning.autoPositioningkP;
|
||||
vy = Math.copySign(
|
||||
Math.max(Math.abs(raw), 0.4),
|
||||
raw
|
||||
);
|
||||
vy = isRedAlliance ? -vy : vy;
|
||||
}
|
||||
autoControlNetworkTable.getEntry("TrenchDifference").setDouble(differenceTrench);
|
||||
Rotation2d currentAngle = drivetrain.getState().Pose.getRotation();
|
||||
double currentRad = currentAngle.getRadians();
|
||||
double targetRad = Math.toRadians(Constants.AutoPositioning.TrenchtargetAngle);
|
||||
double TrenchAngleerror = targetRad - currentRad;
|
||||
vomega = calculatevomegaFromRotationAngle(TrenchAngleerror);
|
||||
autoControlNetworkTable.getEntry("TrenchAutoRotationRate").setDouble(vomega);
|
||||
//vomega = calculateRotationSpeedForTrench();
|
||||
break;
|
||||
}
|
||||
|
||||
autoControlNetworkTable.getEntry("ActualVX").setDouble(vx);
|
||||
autoControlNetworkTable.getEntry("ActualVY").setDouble(vy);
|
||||
autoControlNetworkTable.getEntry("ActualVOmega").setDouble(vomega);
|
||||
|
||||
boolean isAutoLike =
|
||||
!Constants.DemoMode.ENABLED && (
|
||||
robotStatusManager.getStatus() == RobotStatus.CrossingBump ||
|
||||
robotStatusManager.getStatus() == RobotStatus.CrossingTrench);
|
||||
|
||||
if (fieldCentricEnabled || isAutoLike) {
|
||||
|
||||
// 自动状态用 BlueAlliance（不翻转），手柄 field-centric 仍用原 fieldCentric（OperatorPerspective）
|
||||
//var chosen = isAutoLike ? fieldCentricAuto : fieldCentric;
|
||||
var chosen = fieldCentric;
|
||||
return chosen
|
||||
if (fieldCentricEnabled) {
|
||||
return fieldCentric
|
||||
.withVelocityX(vx)
|
||||
.withVelocityY(vy)
|
||||
.withRotationalRate(vomega);
|
||||
@@ -248,34 +166,26 @@ public class DriveControls {
|
||||
.withVelocityY(vy)
|
||||
.withRotationalRate(vomega);
|
||||
}
|
||||
}, () -> fieldCentricEnabled ||
|
||||
(!Constants.DemoMode.ENABLED && (
|
||||
robotStatusManager.getStatus() == RobotStatus.CrossingBump ||
|
||||
robotStatusManager.getStatus() == RobotStatus.CrossingTrench))
|
||||
? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC);
|
||||
}, () -> fieldCentricEnabled ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC);
|
||||
}
|
||||
|
||||
/**
|
||||
*
|
||||
* 输出移动到距离两个目标之间较近的那个点的位移
|
||||
* @return 位移 有正负
|
||||
*/
|
||||
// 返回当前位置到两个目标点中更近目标的位移。
|
||||
// 返回到更近目标的带符号位移。
|
||||
public double calculateDifferenceToTwoTarget(double current, double target1, double target2){
|
||||
double x1 = target1 - current;
|
||||
double x2 = target2 - current;
|
||||
return Math.abs(x1) < Math.abs(x2) ? x1 : x2;
|
||||
}
|
||||
public double[] calculateDistanceAndRotationToHub() {
|
||||
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
|
||||
Pose2d currentPose = drivetrain.getState().Pose; // 当前机器人位姿
|
||||
double targetX = isRedAlliance ? Constants.Field.RedHubPositionX : Constants.Field.BlueHubPositionX;
|
||||
double targetY = isRedAlliance ? Constants.Field.RedHubPositionY : Constants.Field.BlueHubPositionY;
|
||||
|
||||
// 计算目标角度（相对于场地坐标系）
|
||||
// 计算目标角度，相对于场地坐标系。
|
||||
double deltaX = targetX - currentPose.getX();
|
||||
double deltaY = targetY - currentPose.getY();
|
||||
double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为度
|
||||
double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为角度
|
||||
|
||||
// 当前机器人角度
|
||||
// 当前机器人朝向。
|
||||
double currentAngle = currentPose.getRotation().getDegrees();
|
||||
double angleDifference = targetAngle - currentAngle;
|
||||
// Normalize angle difference to the range [-180, 180]
|
||||
@@ -285,26 +195,26 @@ public class DriveControls {
|
||||
angleDifference += 360;
|
||||
}
|
||||
|
||||
// 计算距离
|
||||
// 计算距离。
|
||||
double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
// 计算距离和旋转角度
|
||||
// 返回距离和旋转角度误差。
|
||||
double[] distanceAndRotation = new double[]{distance, angleDifference};
|
||||
|
||||
// 返回距离和角度差
|
||||
// 距离单位为米，角度单位为度。
|
||||
return distanceAndRotation;
|
||||
}
|
||||
public double[] calculateDistanceAndRotationToPassBall() {
|
||||
Pose2d currentPose = drivetrain.getState().Pose; // 获取机器人当前的位置和角度
|
||||
double targetX = isRedAlliance ? Constants.Field.RedPassingBallPosX : Constants.Field.BluePassingBallPosX; // 目标 X 坐标
|
||||
double targetY1 = Constants.Field.PassingBallPosY1; // 目标 Y 坐标
|
||||
double targetY2 = Constants.Field.PassingBallPosY2; // 目标 Y 坐标
|
||||
Pose2d currentPose = drivetrain.getState().Pose; // 当前机器人位姿
|
||||
double targetX = isRedAlliance ? Constants.Field.RedPassingBallPosX : Constants.Field.BluePassingBallPosX; // 传球目标 X 坐标
|
||||
double targetY1 = Constants.Field.PassingBallPosY1; // 传球目标 Y 坐标
|
||||
double targetY2 = Constants.Field.PassingBallPosY2; // 传球目标 Y 坐标
|
||||
|
||||
// 计算目标角度（相对于场地坐标系）
|
||||
// 计算目标角度，相对于场地坐标系。
|
||||
double deltaX = targetX - currentPose.getX();
|
||||
double deltaY = calculateDifferenceToTwoTarget(currentPose.getY(), targetY1, targetY2);
|
||||
double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为度
|
||||
double targetAngle = Math.toDegrees(Math.atan2(deltaY, deltaX)); // 转换为角度
|
||||
|
||||
// 当前机器人角度
|
||||
// 当前机器人朝向。
|
||||
double currentAngle = currentPose.getRotation().getDegrees();
|
||||
double angleDifference = targetAngle - currentAngle;
|
||||
// Normalize angle difference to the range [-180, 180]
|
||||
@@ -314,12 +224,12 @@ public class DriveControls {
|
||||
angleDifference += 360;
|
||||
}
|
||||
|
||||
// 计算距离
|
||||
// 计算距离。
|
||||
double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY);
|
||||
// 计算距离和旋转角度
|
||||
// 返回距离和旋转角度误差。
|
||||
double[] distanceAndRotation = new double[]{distance, angleDifference};
|
||||
|
||||
// 返回距离和角度差
|
||||
// 距离单位为米，角度单位为度。
|
||||
return distanceAndRotation;
|
||||
}
|
||||
|
||||
@@ -347,30 +257,26 @@ public class DriveControls {
|
||||
} else {
|
||||
vomega = error * Constants.AutoPositioning.TurningkP;
|
||||
}
|
||||
// 限制最大角速度
|
||||
// 限制最大角速度。
|
||||
vomega = Math.max(-maxAngularRate, Math.min(maxAngularRate, vomega));
|
||||
return vomega;
|
||||
}
|
||||
|
||||
/**
|
||||
* 只限制“加速”（幅值变大），不限制“减速”（幅值变小）
|
||||
* 这样松杆/刹车会更及时，同时 boost 仍然能抑制电流冲击
|
||||
* 只限制加速，也就是输出幅值变大时；不限制减速。
|
||||
* 这样松杆或刹车能更及时，同时 boost 仍能抑制电流冲击。
|
||||
*/
|
||||
private double limitAccelOnly(double target, double last, SlewRateLimiter limiter) {
|
||||
// 如果正在减速（幅值变小），直接放行到目标值
|
||||
// 如果正在减速，直接放行到目标值。
|
||||
if (Math.abs(target) < Math.abs(last)) {
|
||||
limiter.reset(target); // 同步 limiter 内部状态，防止下一次突变
|
||||
limiter.reset(target); // 同步 limiter 内部状态，防止下一次突跳。
|
||||
return target;
|
||||
}
|
||||
// 否则是加速，使用 limiter
|
||||
// 否则是加速，使用 limiter。
|
||||
return limiter.calculate(target);
|
||||
}
|
||||
|
||||
private double clamp(double value, double min, double max) {
|
||||
return Math.max(min, Math.min(max, value));
|
||||
}
|
||||
|
||||
/** 紧急停止：清零限速器，让速度输出立刻归零 */
|
||||
/** 紧急停止：清零限速器，让速度输出立刻归零。 */
|
||||
public void emergencyStop() {
|
||||
vxLimiter.reset(0.0);
|
||||
vyLimiter.reset(0.0);
|
||||
@@ -408,9 +314,9 @@ public class DriveControls {
|
||||
return maxAngularRate;
|
||||
}
|
||||
public void setTeamColors() {
|
||||
Optional<Alliance> ally = DriverStation.getAlliance(); // 获取队伍颜色
|
||||
Optional<Alliance> ally = DriverStation.getAlliance(); // 获取联盟颜色
|
||||
if (ally.isPresent()) {
|
||||
// 根据队伍颜色设置布尔值
|
||||
// 根据联盟颜色更新自动瞄准目标。
|
||||
if (ally.get() == Alliance.Red) {
|
||||
isRedAlliance = true; // 红队
|
||||
} else if (ally.get() == Alliance.Blue) {
|
||||
@@ -428,20 +334,7 @@ public class DriveControls {
|
||||
public Command getAllianceColorCommand() {
|
||||
return Commands.runOnce(this::setTeamColors);
|
||||
}
|
||||
// public void alignToClimb(){
|
||||
// LimelightSupplier.setPipeline(Constants.Limelight.climbPipelineIndex);
|
||||
// if(!drivetrain.isAlignedToAprilTag()){
|
||||
// drivetrain.driveToAprilTag();
|
||||
// }
|
||||
// else{
|
||||
// LimelightSupplier.setPipeline(Constants.Limelight.locatePipelineIndex);
|
||||
// drivetrain.stop();
|
||||
// }
|
||||
// }
|
||||
public Command autoAimCommand() {
|
||||
if (Constants.DemoMode.ENABLED) {
|
||||
return Commands.runOnce(drivetrain::stop, drivetrain);
|
||||
}
|
||||
return drivetrain.applyRequest(() -> {
|
||||
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
|
||||
double vomega = calculateRotationSpeedFromRotationAngle(distanceAndRotation[1]);
|
||||
@@ -457,9 +350,6 @@ public class DriveControls {
|
||||
}, drivetrain));
|
||||
}
|
||||
public Command shakeCommand(double amplitude, double switchPeriod, double totalTime) {
|
||||
if (Constants.DemoMode.ENABLED) {
|
||||
return Commands.runOnce(drivetrain::stop, drivetrain);
|
||||
}
|
||||
Timer timer = new Timer();
|
||||
|
||||
return Commands.sequence(
|
||||
@@ -481,7 +371,7 @@ public class DriveControls {
|
||||
break;
|
||||
case 1:
|
||||
vx = 0.0;
|
||||
vy = amplitude; // 右
|
||||
vy = amplitude; // 左
|
||||
break;
|
||||
case 2:
|
||||
vx = -amplitude; // 后
|
||||
@@ -489,7 +379,7 @@ public class DriveControls {
|
||||
break;
|
||||
case 3:
|
||||
vx = 0.0;
|
||||
vy = -amplitude; // 左
|
||||
vy = -amplitude; // 右
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -511,9 +401,13 @@ public class DriveControls {
|
||||
|
||||
public void pushDistanceData(){
|
||||
double[] distanceAndRotation = calculateDistanceAndRotationToHub();
|
||||
// 将距离值写入 NetworkTable
|
||||
// 将距离数据写入 NetworkTable。
|
||||
autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]);
|
||||
autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]);
|
||||
// 自动阶段默认驾驶命令不运行，这里同样保持目标朝向最新，
|
||||
// 让自动程序里的射击门控 isAimed() 有效。
|
||||
drivetrain.setTargetHeading(
|
||||
drivetrain.getRotation().plus(Rotation2d.fromDegrees(distanceAndRotation[1])));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -1,102 +0,0 @@
|
||||
package frc.robot.controls;
|
||||
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import frc.robot.subsystems.IntakerSubsystem;
|
||||
import frc.robot.subsystems.ShooterSubsystem;
|
||||
import frc.robot.Constants;
|
||||
|
||||
|
||||
public class IntakerControls {
|
||||
private final IntakerSubsystem IntakerSubsystem;
|
||||
private final CommandXboxController controller;
|
||||
private final ShooterSubsystem ShooterSubsystem;
|
||||
|
||||
//private static final double ROTATER_STEP_PER_CYCLE = 0.00;
|
||||
|
||||
public IntakerControls(IntakerSubsystem Intaker, ShooterSubsystem Shooter, CommandXboxController controller) {
|
||||
this.ShooterSubsystem = Shooter;
|
||||
this.IntakerSubsystem = Intaker;
|
||||
this.controller = controller;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//B.A：按住下降 + getter转；松开后 rotater保持当前位置，getter继续转
|
||||
// controller.a()
|
||||
// .whileTrue(
|
||||
// Commands.run(() -> {
|
||||
// // rotater下降：范围会被 subsystem 限制
|
||||
// double cur = intakerSubsystem.getIntakerotaterPosition();
|
||||
// intakerSubsystem.setIntakerRotaterPosition(cur - ROTATER_STEP_PER_CYCLE);
|
||||
|
||||
// // getter 开
|
||||
// intakerSubsystem.setIntakeGetterOn(true);
|
||||
// }, intakerSubsystem)
|
||||
// )
|
||||
// .onFalse(
|
||||
// Commands.runOnce(() -> {
|
||||
// // 松：把当前位置作为target保持
|
||||
// double cur = intakerSubsystem.getIntakerotaterPosition();
|
||||
// intakerSubsystem.setIntakerRotaterPosition(cur);
|
||||
|
||||
// // gtter继续转
|
||||
// intakerSubsystem.setIntakeGetterOn(true);
|
||||
// }, intakerSubsystem)
|
||||
// );
|
||||
// // === B.B：按住上升+getter停；松开后rotater保持当前位置
|
||||
// controller.b()
|
||||
// .whileTrue(
|
||||
// Commands.run(() -> {
|
||||
// // rotater升
|
||||
// double cur = intakerSubsystem.getIntakerotaterPosition();
|
||||
// intakerSubsystem.setIntakerRotaterPosition(cur + ROTATER_STEP_PER_CYCLE);
|
||||
|
||||
// // getter关
|
||||
// intakerSubsystem.setIntakeGetterOn(false);
|
||||
// }, intakerSubsystem)
|
||||
// )
|
||||
// .onFalse(
|
||||
// Commands.runOnce(() -> {
|
||||
// // 松：保持当前位置
|
||||
// double cur = intakerSubsystem.getIntakerotaterPosition();
|
||||
// intakerSubsystem.setIntakerRotaterPosition(cur);
|
||||
|
||||
// // getter仍然停止
|
||||
// intakerSubsystem.setIntakeGetterOn(false);
|
||||
// }, intakerSubsystem)
|
||||
// );
|
||||
|
||||
public Command defaultIntakerCommand() {
|
||||
return Commands.run(() -> {
|
||||
double getterspeed = 0;
|
||||
double statorCurrent = ShooterSubsystem.getConveyorStatorCurrent();
|
||||
|
||||
if (controller.getLeftTriggerAxis() > 0.1) {
|
||||
getterspeed = controller.getLeftTriggerAxis() * 0.5 + 0.3;
|
||||
}
|
||||
|
||||
IntakerSubsystem.setIntakerGetterSpeed(getterspeed);
|
||||
|
||||
boolean demoModeEnabled = Constants.DemoMode.ENABLED;
|
||||
if (demoModeEnabled) {
|
||||
IntakerSubsystem.stopIntakerotater();
|
||||
return;
|
||||
}
|
||||
|
||||
if(controller.getLeftY() < -0.5){
|
||||
IntakerSubsystem.IntakerUpNonStop();
|
||||
} else if(controller.getLeftY() > 0.5){
|
||||
IntakerSubsystem.IntakerDownNonStop();
|
||||
} else {
|
||||
IntakerSubsystem.stopIntakerotater();
|
||||
}
|
||||
}, IntakerSubsystem);
|
||||
}
|
||||
public Command resetIntakerRotaterEncoderCommand(IntakerSubsystem intakerSubsystem) {
|
||||
return Commands.runOnce(intakerSubsystem::zeroEncoder, intakerSubsystem);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,96 @@
|
||||
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.IntakeRotaterSubsystem;
|
||||
import frc.robot.subsystems.ShooterSubsystem;
|
||||
import frc.robot.subsystems.Superstructure;
|
||||
|
||||
/**
|
||||
* 上操作员手柄的“意图层”。
|
||||
*
|
||||
* 这里刻意不直接设置任何机构电机。按钮、自动辅助命令和调参快捷键都会
|
||||
* 转换成 Superstructure 请求，由 Superstructure 统一处理多机构动作顺序。
|
||||
*/
|
||||
public class OperatorControls {
|
||||
private final Superstructure superstructure;
|
||||
private final ShooterSubsystem shooterSubsystem;
|
||||
|
||||
public OperatorControls(Superstructure superstructure, ShooterSubsystem shooterSubsystem) {
|
||||
this.superstructure = superstructure;
|
||||
this.shooterSubsystem = shooterSubsystem;
|
||||
}
|
||||
|
||||
public Command requestIdleCommand() {
|
||||
return superstructure.requestIdleCommand();
|
||||
}
|
||||
|
||||
public Command requestIntakeCommand() {
|
||||
return superstructure.requestIntakeCommand();
|
||||
}
|
||||
|
||||
public Command requestShootHubCommand() {
|
||||
return superstructure.requestShootHubCommand();
|
||||
}
|
||||
|
||||
public Command requestAimHubCommand() {
|
||||
return superstructure.requestAimHubCommand();
|
||||
}
|
||||
|
||||
public Command requestPassBallCommand() {
|
||||
return superstructure.requestPassBallCommand();
|
||||
}
|
||||
|
||||
public Command requestEjectCommand() {
|
||||
return superstructure.requestEjectCommand();
|
||||
}
|
||||
|
||||
public Command requestManualCommand() {
|
||||
return superstructure.requestManualCommand();
|
||||
}
|
||||
|
||||
public void adjustFlywheelVoltageOffset(double offset) {
|
||||
superstructure.adjustFlywheelVoltageOffset(offset);
|
||||
}
|
||||
|
||||
public void adjustBackboardRateOffset(double offset) {
|
||||
superstructure.adjustBackboardPositionOffset(offset);
|
||||
}
|
||||
|
||||
public void resetOffsets() {
|
||||
superstructure.resetOffsets();
|
||||
}
|
||||
|
||||
public Command resetBackboardCommand() {
|
||||
return Commands.runOnce(superstructure::resetBackboardEncoder, shooterSubsystem);
|
||||
}
|
||||
|
||||
public Command manualRaiseIntakeRotaterCommand(IntakeRotaterSubsystem intakeRotater) {
|
||||
return Commands.runEnd(intakeRotater::manualRaise, intakeRotater::stop, intakeRotater);
|
||||
}
|
||||
|
||||
public Command manualLowerIntakeRotaterCommand(IntakeRotaterSubsystem intakeRotater) {
|
||||
return Commands.runEnd(intakeRotater::manualLower, intakeRotater::stop, intakeRotater);
|
||||
}
|
||||
|
||||
public Command lowerIntakeRotaterForMatchCommand(IntakeRotaterSubsystem intakeRotater) {
|
||||
return intakeRotater.lowerForMatchCommand();
|
||||
}
|
||||
|
||||
// PathPlanner 自动射击辅助：先预备到所有射击条件满足，再请求喂球，
|
||||
// 最后回到空闲状态。真正的“是否能射”判断仍然放在 Superstructure。
|
||||
public Command autoShootToHubCommand() {
|
||||
return Commands.sequence(
|
||||
superstructure.requestShootHubCommand(),
|
||||
Commands.waitSeconds(Constants.Superstructure.shootTimeoutSeconds),
|
||||
superstructure.requestIdleCommand()
|
||||
);
|
||||
}
|
||||
|
||||
// 保留旧背板回零动作，方便调参和现场恢复；
|
||||
// 具体电机操作在 ShooterSubsystem 内实现，这里只转发命令。
|
||||
public Command resetBackboard0Command() {
|
||||
return shooterSubsystem.homeBackboardCommand();
|
||||
}
|
||||
}
|
||||
@@ -1,227 +0,0 @@
|
||||
package frc.robot.controls;
|
||||
|
||||
import java.util.Set;
|
||||
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import frc.robot.Constants;
|
||||
import frc.robot.RobotStatusManager;
|
||||
import frc.robot.Constants.Intaker;
|
||||
import frc.robot.Constants.RobotStatus;
|
||||
import frc.robot.subsystems.IntakerSubsystem;
|
||||
import frc.robot.subsystems.ShooterSubsystem;
|
||||
|
||||
public class ShooterControls {
|
||||
private final ShooterSubsystem shooterSubsystem;
|
||||
private final CommandXboxController controller;
|
||||
|
||||
private double flywheelSpeedOffset = 0.0;
|
||||
private double backboardPositionOffset = 0.0;
|
||||
private NetworkTable shooterControlTable;
|
||||
private double conveyorSpeed = 0.0;
|
||||
private double flywheelSpeed = 0;
|
||||
private double backboardPosition = 0;
|
||||
private final RobotStatusManager robotStatusManager;
|
||||
public ShooterControls(ShooterSubsystem shooterSubsystem, CommandXboxController controller,RobotStatusManager rsm) {
|
||||
this.shooterSubsystem = shooterSubsystem;
|
||||
this.controller = controller;
|
||||
this.robotStatusManager = rsm;
|
||||
shooterControlTable = NetworkTableInstance.getDefault().getTable("ShooterControl");
|
||||
}
|
||||
|
||||
public Command defaultShooterCommand() {
|
||||
return Commands.run(() -> {
|
||||
switch (robotStatusManager.getStatus()) {
|
||||
case Stopped:
|
||||
shooterSubsystem.stopMotors();
|
||||
break;
|
||||
case AllTelop:
|
||||
shooterSubsystem.setBackboardPosition(0);
|
||||
if(shooterSubsystem.isBackboardAtTarget()){
|
||||
shooterSubsystem.backboardMotor.set(0);
|
||||
}else{
|
||||
shooterSubsystem.outputBackboard();
|
||||
}
|
||||
shooterSubsystem.setFlywheelSpeedByRPS(0);
|
||||
if(!Constants.DemoMode.ENABLED && controller.a().getAsBoolean()) { shooterSubsystem.setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);}
|
||||
else{shooterSubsystem.setConveyorSpeedByRPS(0);}
|
||||
break;
|
||||
case PassingBall:
|
||||
var xP = shooterSubsystem.getDistanceToPassball();
|
||||
xP = Math.max(2.5, Math.min(10.0, xP));
|
||||
flywheelSpeed = +0.004960317456971805*xP*xP*xP*xP*xP*xP*xP-0.22361111097424535*xP*xP*xP*xP*xP*xP+4.234722219890569*xP*xP*xP*xP*xP-43.63194442298942*xP*xP*xP*xP+264.05555544032126*xP*xP*xP-938.6444440829168*xP*xP+1818.2047612908214*xP-1415.9999995643104;
|
||||
flywheelSpeed += flywheelSpeedOffset;
|
||||
backboardPosition = +0.10912698402591796*xP*xP*xP*xP*xP*xP*xP-5.277777773586415*xP*xP*xP*xP*xP*xP+105.55555548303796*xP*xP*xP*xP*xP-1125.6944437656143*xP*xP*xP*xP+6865.763885174154*xP*xP*xP-23769.027765888906*xP*xP+43328.57140795847*xP-31599.99998506075;
|
||||
conveyorSpeed = Constants.Shooter.conveyorSpeed;
|
||||
if (controller.getRightTriggerAxis() < 0.1) {
|
||||
flywheelSpeed = 0;
|
||||
conveyorSpeed = 0;
|
||||
}
|
||||
if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){
|
||||
conveyorSpeed = 0;
|
||||
}
|
||||
//if(controller.a().getAsBoolean()) { conveyorSpeed = Constants.Shooter.conveyorSpeed;}
|
||||
shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
|
||||
shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
|
||||
shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
|
||||
shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed);
|
||||
shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
|
||||
shooterSubsystem.setBackboardPosition(backboardPosition);
|
||||
if(shooterSubsystem.isBackboardAtTarget()){
|
||||
shooterSubsystem.backboardMotor.set(0);
|
||||
}else{
|
||||
shooterSubsystem.outputBackboard();
|
||||
}
|
||||
break;
|
||||
case AutoAimming:
|
||||
var x = shooterSubsystem.getDistanceToHub();
|
||||
x = Math.max(1.2, Math.min(4.5, x));
|
||||
//flywheelSpeed = -1.4964026859575532*x*x*x*x*x*x*x+29.82445640241506*x*x*x*x*x*x-247.71603153159896*x*x*x*x*x+1108.8727302184625*x*x*x*x-2881.517500617481*x*x*x+4332.05363145275*x*x-3467.2508337871454*x+1192.1508062663759;
|
||||
flywheelSpeed = 60;
|
||||
flywheelSpeed += flywheelSpeedOffset;
|
||||
//backboardPosition = +34.5286998307899*x*x*x*x*x*x*x*x-788.5788623683884*x*x*x*x*x*x*x+7658.073303693148*x*x*x*x*x*x-41191.69338972519*x*x*x*x*x+133870.54588688645*x*x*x*x-268559.6987062158*x*x*x+324254.6530324262*x*x-215015.48893131423*x+59977.46545382827;
|
||||
backboardPosition = 500;
|
||||
backboardPosition += backboardPositionOffset;
|
||||
conveyorSpeed = Constants.Shooter.conveyorSpeed;
|
||||
if (controller.getRightTriggerAxis() < 0.1) {
|
||||
flywheelSpeed = 0;
|
||||
conveyorSpeed = 0;
|
||||
}
|
||||
if(shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble()<flywheelSpeed*0.9){
|
||||
conveyorSpeed = 0;
|
||||
}
|
||||
//if(controller.a().getAsBoolean()) { conveyorSpeed = Constants.Shooter.conveyorSpeed;}
|
||||
shooterControlTable.getEntry("flywheelTargetSpeed").setDouble(flywheelSpeed);
|
||||
shooterControlTable.getEntry("conveyerTargetSpeed").setDouble(conveyorSpeed);
|
||||
shooterControlTable.getEntry("backboardTargetPosition").setDouble(backboardPosition);
|
||||
shooterSubsystem.setFlywheelSpeedByRPS(flywheelSpeed);
|
||||
shooterSubsystem.setConveyorSpeedByRPS(conveyorSpeed);
|
||||
shooterSubsystem.setBackboardPosition(backboardPosition);
|
||||
if(shooterSubsystem.isBackboardAtTarget()){
|
||||
shooterSubsystem.backboardMotor.set(0);
|
||||
}else{
|
||||
shooterSubsystem.outputBackboard();
|
||||
}
|
||||
break;
|
||||
case Climbing:
|
||||
case CrossingBump:
|
||||
case CrossingTrench:
|
||||
if(shooterSubsystem.getBackboardPosition() <= 20)
|
||||
shooterSubsystem.stopMotors();
|
||||
else{
|
||||
shooterSubsystem.setBackboardPosition(0);
|
||||
shooterSubsystem.outputBackboard();
|
||||
}
|
||||
break;
|
||||
|
||||
}
|
||||
}, shooterSubsystem);
|
||||
}
|
||||
|
||||
// public Command resetBackboardCommand(){
|
||||
// return
|
||||
// Commands.runOnce(()->{
|
||||
// shooterSubsystem.backboardMotor.set(-0.1);
|
||||
// shooterSubsystem.currentBackboardInalyzeMode = Mode.Inalyzing;}, shooterSubsystem)
|
||||
// .andThen(
|
||||
// Commands.waitSeconds(0.5))
|
||||
// .andThen(Commands.runOnce(()->{
|
||||
// shooterSubsystem.backboardMotor.set(0);
|
||||
// shooterSubsystem.backboardEncoder.reset();
|
||||
// shooterSubsystem.backboardPID.reset(0);}, shooterSubsystem))
|
||||
// .andThen(
|
||||
// Commands.waitSeconds(1))
|
||||
// .andThen(Commands.runOnce(()->{
|
||||
// shooterSubsystem.currentBackboardInalyzeMode = Mode.Inalyzed;}, shooterSubsystem));
|
||||
// }
|
||||
|
||||
public void outputInfo(){
|
||||
double currentConveyorSpeed = shooterSubsystem.conveyorMotor.getVelocity().getValueAsDouble();
|
||||
double currentFlywheelSpeed = shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble();
|
||||
double currentBackboardRate = shooterSubsystem.getBackboardPosition();
|
||||
System.out.println("Conveyor: " + currentConveyorSpeed + "\t Flywheel: " + currentFlywheelSpeed + "\t Backboard:" + currentBackboardRate);
|
||||
}
|
||||
public Command resetBackboardCommand(){
|
||||
return
|
||||
Commands.runOnce(()->{
|
||||
shooterSubsystem.backboardEncoder.reset();
|
||||
shooterSubsystem.backboardPID.reset(0);}, shooterSubsystem);
|
||||
}
|
||||
|
||||
public void adjustFlywheelSpeedOffset(double offset) {
|
||||
flywheelSpeedOffset += offset;
|
||||
}
|
||||
|
||||
public void adjustBackboardRateOffset(double offset) {
|
||||
backboardPositionOffset += offset;
|
||||
}
|
||||
public void resetOffsets(){
|
||||
flywheelSpeedOffset = 0.0;
|
||||
backboardPositionOffset = 0.0;
|
||||
}
|
||||
|
||||
public Command autoShootToHubCommand() {
|
||||
return Commands.defer(() -> {
|
||||
double x = shooterSubsystem.getDistanceToHub();
|
||||
x = Math.max(1.2, Math.min(4.5, x));
|
||||
|
||||
double flywheelTargetSpeed = -1.4964026859575532*x*x*x*x*x*x*x+29.82445640241506*x*x*x*x*x*x-247.71603153159896*x*x*x*x*x+1108.8727302184625*x*x*x*x-2881.517500617481*x*x*x+4332.05363145275*x*x-3467.2508337871454*x+1192.1508062663759;
|
||||
double backboardTargetPosition = +34.5286998307899*x*x*x*x*x*x*x*x-788.5788623683884*x*x*x*x*x*x*x+7658.073303693148*x*x*x*x*x*x-41191.69338972519*x*x*x*x*x+133870.54588688645*x*x*x*x-268559.6987062158*x*x*x+324254.6530324262*x*x-215015.48893131423*x+59977.46545382827;
|
||||
double conveyorTargetSpeed = Constants.Shooter.conveyorSpeed;
|
||||
|
||||
return new RunCommand(() -> {
|
||||
shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed);
|
||||
shooterSubsystem.setConveyorSpeedByRPS(0.0);
|
||||
shooterSubsystem.setBackboardPosition(backboardTargetPosition);
|
||||
if (shooterSubsystem.isBackboardAtTarget()) {
|
||||
shooterSubsystem.backboardMotor.set(0);
|
||||
} else {
|
||||
shooterSubsystem.outputBackboard();
|
||||
}
|
||||
}, shooterSubsystem)
|
||||
.until(() ->
|
||||
shooterSubsystem.isBackboardAtTarget()
|
||||
&& shooterSubsystem.flywheelMotorLeft.getVelocity().getValueAsDouble() >= flywheelTargetSpeed * 0.9
|
||||
)
|
||||
.withTimeout(0.3)
|
||||
.andThen(new RunCommand(() -> {
|
||||
shooterSubsystem.setFlywheelSpeedByRPS(flywheelTargetSpeed);
|
||||
shooterSubsystem.setBackboardPosition(backboardTargetPosition);
|
||||
if (shooterSubsystem.isBackboardAtTarget()) {
|
||||
shooterSubsystem.backboardMotor.set(0);
|
||||
} else {
|
||||
shooterSubsystem.outputBackboard();
|
||||
}
|
||||
shooterSubsystem.setConveyorSpeedByRPS(25);
|
||||
}, shooterSubsystem).withTimeout(5.0))
|
||||
.andThen(new InstantCommand(() -> {
|
||||
shooterSubsystem.setFlywheelSpeedByRPS(0.0);
|
||||
shooterSubsystem.setConveyorSpeedByRPS(0.0);
|
||||
shooterSubsystem.backboardMotor.set(0.0);
|
||||
}, shooterSubsystem));
|
||||
}, Set.of(shooterSubsystem));
|
||||
}
|
||||
|
||||
public Command resetBackboard0Command() {
|
||||
return new RunCommand(() -> {
|
||||
shooterSubsystem.setBackboardPosition(0);
|
||||
|
||||
if (shooterSubsystem.isBackboardAtTarget()) {
|
||||
shooterSubsystem.backboardMotor.set(0);
|
||||
} else {
|
||||
shooterSubsystem.outputBackboard();
|
||||
}
|
||||
|
||||
}, shooterSubsystem)
|
||||
.until(() -> shooterSubsystem.isBackboardAtTarget())
|
||||
.andThen(new InstantCommand(() -> {
|
||||
shooterSubsystem.backboardMotor.set(0);
|
||||
}));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -42,7 +42,7 @@ public class TunerConstants {
|
||||
|
||||
// The type of motor used for the drive motor
|
||||
private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated;
|
||||
// The type of motor used for the drive motor
|
||||
// The type of motor used for the steer motor
|
||||
private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated;
|
||||
|
||||
// The remote sensor feedback type to use for the steer motors;
|
||||
@@ -55,7 +55,14 @@ public class TunerConstants {
|
||||
|
||||
// Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
|
||||
// Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
|
||||
private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
|
||||
private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
// Default supply current limit is 70 A, but it can be lowered to avoid brownouts.
|
||||
// Supply current limits can be larger than the breaker current rating.
|
||||
.withSupplyCurrentLimit(Amps.of(70))
|
||||
.withSupplyCurrentLimitEnable(true)
|
||||
);
|
||||
private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()
|
||||
.withCurrentLimits(
|
||||
new CurrentLimitsConfigs()
|
||||
@@ -77,12 +84,12 @@ public class TunerConstants {
|
||||
.withEnableCompass(false)
|
||||
);
|
||||
|
||||
|
||||
// CAN bus that the devices are located on;
|
||||
// All swerve devices must share the same CAN bus
|
||||
public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot");
|
||||
|
||||
// Theoretical free speed (m/s) at 12 V applied output;
|
||||
// Measured robot speed (m/s) at 12 V applied output;
|
||||
// This is NOT the desired max robot speed - see MaxSpeed in RobotContainer instead;
|
||||
// This needs to be tuned to your individual robot
|
||||
public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.73);
|
||||
|
||||
@@ -136,10 +143,10 @@ public class TunerConstants {
|
||||
|
||||
|
||||
// Front Left
|
||||
private static final int kFrontLeftDriveMotorId = 11;
|
||||
private static final int kFrontLeftSteerMotorId = 12;
|
||||
private static final int kFrontLeftEncoderId = 13;
|
||||
private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.412841796875);
|
||||
private static final int kFrontLeftDriveMotorId = 1;
|
||||
private static final int kFrontLeftSteerMotorId = 2;
|
||||
private static final int kFrontLeftEncoderId = 3;
|
||||
private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.18115234375);
|
||||
private static final boolean kFrontLeftSteerMotorInverted = false;
|
||||
private static final boolean kFrontLeftEncoderInverted = false;
|
||||
|
||||
@@ -147,10 +154,10 @@ public class TunerConstants {
|
||||
private static final Distance kFrontLeftYPos = Inches.of(11.375);
|
||||
|
||||
// Front Right
|
||||
private static final int kFrontRightDriveMotorId = 21;
|
||||
private static final int kFrontRightSteerMotorId = 22;
|
||||
private static final int kFrontRightEncoderId = 23;
|
||||
private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.012939453125);
|
||||
private static final int kFrontRightDriveMotorId = 11;
|
||||
private static final int kFrontRightSteerMotorId = 12;
|
||||
private static final int kFrontRightEncoderId = 13;
|
||||
private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.16162109375);
|
||||
private static final boolean kFrontRightSteerMotorInverted = false;
|
||||
private static final boolean kFrontRightEncoderInverted = false;
|
||||
|
||||
@@ -158,10 +165,10 @@ public class TunerConstants {
|
||||
private static final Distance kFrontRightYPos = Inches.of(-11.375);
|
||||
|
||||
// Back Left
|
||||
private static final int kBackLeftDriveMotorId = 1;
|
||||
private static final int kBackLeftSteerMotorId = 2;
|
||||
private static final int kBackLeftEncoderId = 3;
|
||||
private static final Angle kBackLeftEncoderOffset = Rotations.of(0.07275390625);
|
||||
private static final int kBackLeftDriveMotorId = 21;
|
||||
private static final int kBackLeftSteerMotorId = 22;
|
||||
private static final int kBackLeftEncoderId = 23;
|
||||
private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.017822265625);
|
||||
private static final boolean kBackLeftSteerMotorInverted = false;
|
||||
private static final boolean kBackLeftEncoderInverted = false;
|
||||
|
||||
@@ -172,7 +179,7 @@ public class TunerConstants {
|
||||
private static final int kBackRightDriveMotorId = 31;
|
||||
private static final int kBackRightSteerMotorId = 32;
|
||||
private static final int kBackRightEncoderId = 33;
|
||||
private static final Angle kBackRightEncoderOffset = Rotations.of(-0.4892578125);
|
||||
private static final Angle kBackRightEncoderOffset = Rotations.of(-0.490234375);
|
||||
private static final boolean kBackRightSteerMotorInverted = false;
|
||||
private static final boolean kBackRightEncoderInverted = false;
|
||||
|
||||
@@ -203,7 +210,7 @@ public class TunerConstants {
|
||||
|
||||
/**
|
||||
* Creates a CommandSwerveDrivetrain instance.
|
||||
* This should only be called once in your robot program,.
|
||||
* This should only be called once in your robot program.
|
||||
*/
|
||||
public static CommandSwerveDrivetrain createDrivetrain() {
|
||||
return new CommandSwerveDrivetrain(
|
||||
|
||||
@@ -1,127 +0,0 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||
import com.ctre.phoenix6.controls.DutyCycleOut;
|
||||
import com.ctre.phoenix6.controls.PositionVoltage;
|
||||
import com.ctre.phoenix6.controls.VelocityDutyCycle;
|
||||
import com.ctre.phoenix6.controls.VelocityVoltage;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.signals.InvertedValue;
|
||||
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.Constants;
|
||||
|
||||
|
||||
public class ClimberSubsystem extends SubsystemBase {
|
||||
|
||||
private boolean autoClimbUsedInAuto = false;
|
||||
|
||||
private final TalonFX climberMotor;
|
||||
private double targetPositionTicks;
|
||||
|
||||
private final PositionVoltage positionHoldRequest =
|
||||
new PositionVoltage(0).withSlot(0);
|
||||
|
||||
private final VelocityVoltage velocityRequest = new VelocityVoltage(0).withSlot(0);
|
||||
|
||||
private double holdPositionTicks = 0.0;
|
||||
|
||||
private final NetworkTable climberTable;
|
||||
|
||||
|
||||
public ClimberSubsystem() {
|
||||
climberMotor = new TalonFX(Constants.Climber.motorID);
|
||||
|
||||
climberMotor.getConfigurator().apply(Constants.Climber.slot0Configs);
|
||||
|
||||
var cfg = new MotorOutputConfigs();
|
||||
cfg.Inverted = Constants.Climber.inverted
|
||||
? InvertedValue.Clockwise_Positive
|
||||
: InvertedValue.CounterClockwise_Positive;
|
||||
climberMotor.getConfigurator().apply(cfg);
|
||||
|
||||
climberTable = NetworkTableInstance.getDefault().getTable("Climber");
|
||||
|
||||
holdPositionTicks = getPositionTicks();
|
||||
}
|
||||
public void initialize() {
|
||||
climberMotor.set(-0.07);
|
||||
}
|
||||
|
||||
public double getPositionTicks() {
|
||||
return climberMotor.getPosition().getValueAsDouble();
|
||||
}
|
||||
|
||||
public double getVelocityRps() {
|
||||
return climberMotor.getVelocity().getValueAsDouble();
|
||||
}
|
||||
|
||||
public void climbUp() {
|
||||
targetPositionTicks = Constants.Climber.climbUpTicks;
|
||||
climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
|
||||
}
|
||||
|
||||
public void climbDown() {
|
||||
targetPositionTicks = Constants.Climber.climbDownTicks;
|
||||
climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
|
||||
}
|
||||
public void climbDownAuto() {
|
||||
targetPositionTicks = Constants.Climber.climbDownAutoTicks;
|
||||
climberMotor.setControl(positionHoldRequest.withPosition(targetPositionTicks));
|
||||
}
|
||||
|
||||
public void climbDownNonStop(){
|
||||
climberMotor.setControl(velocityRequest.withVelocity(Constants.Climber.climbVelocity));
|
||||
}
|
||||
|
||||
public void climbUpNonStop(){
|
||||
climberMotor.setControl(velocityRequest.withVelocity(-Constants.Climber.climbVelocity));
|
||||
}
|
||||
public void climbUpNonStopforAuto(){
|
||||
climberMotor.set(0.07);
|
||||
}
|
||||
|
||||
public void captureHoldPosition() {
|
||||
holdPositionTicks = getPositionTicks();
|
||||
}
|
||||
|
||||
public void holdPosition() {
|
||||
climberMotor.setControl(
|
||||
positionHoldRequest.withPosition(holdPositionTicks)
|
||||
);
|
||||
}
|
||||
|
||||
public void stopMotor() {
|
||||
climberMotor.set(0);
|
||||
}
|
||||
public void setMotor(double speed) {
|
||||
climberMotor.set(speed);
|
||||
}
|
||||
|
||||
public Command initCommand() {
|
||||
return Commands.sequence(
|
||||
Commands.run(() -> climberMotor.set(-0.07), this).withTimeout(0.5),
|
||||
Commands.runOnce(this::zeroEncoder, this)
|
||||
);
|
||||
}
|
||||
|
||||
public void zeroEncoder() {
|
||||
climberMotor.setPosition(0);
|
||||
stopMotor();
|
||||
}
|
||||
|
||||
public void stopAll() {
|
||||
climberMotor.set(0);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
climberTable.getEntry("velocityRps").setDouble(getVelocityRps());
|
||||
climberTable.getEntry("positionTicks").setDouble(getPositionTicks());
|
||||
climberTable.getEntry("holdPositionTicks").setDouble(holdPositionTicks);
|
||||
}
|
||||
}
|
||||
@@ -1,4 +1,5 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import java.util.Optional;
|
||||
import java.util.function.Supplier;
|
||||
|
||||
@@ -15,40 +16,39 @@ import edu.wpi.first.math.Matrix;
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.numbers.N1;
|
||||
import edu.wpi.first.math.numbers.N3;
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.networktables.StructPublisher;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Subsystem;
|
||||
import frc.robot.Constants;
|
||||
import frc.robot.Constants.DriveMode;
|
||||
import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
|
||||
import frc.robot.subsystems.LimelightHelpers.RawFiducial;
|
||||
import frc.robot.Constants;
|
||||
|
||||
import edu.wpi.first.networktables.*;
|
||||
|
||||
|
||||
/**
|
||||
* Class that extends the Phoenix 6 SwerveDrivetrain class and implements
|
||||
* Subsystem so it can easily be used in command-based projects.
|
||||
* Phoenix 6 Tuner X 生成底盘的命令式封装。
|
||||
*
|
||||
* Generated by the 2026 Tuner X Swerve Project Generator
|
||||
* https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
|
||||
* 底层 swerve、模块控制和里程计保持原结构；本类只在外层补充
|
||||
* Command-Based、PathPlanner、Limelight 融合和上层状态读取接口。
|
||||
*/
|
||||
public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem {
|
||||
|
||||
/* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
|
||||
/* 蓝方视角下，正前方为 0 度。 */
|
||||
private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
|
||||
/* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
|
||||
/* 红方视角下，正前方为 180 度。 */
|
||||
private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
|
||||
/* Keep track if we've ever applied the operator perspective before or not */
|
||||
/* 记录是否已经应用过操作员视角。 */
|
||||
private boolean m_hasAppliedOperatorPerspective = false;
|
||||
private boolean isAlignedToAprilTag = false;
|
||||
|
||||
// 上层状态机射击门控使用的目标朝向；正常底盘控制仍由手动驾驶掌控。
|
||||
private Rotation2d targetHeading = Rotation2d.kZero;
|
||||
|
||||
private DriveMode m_driveMode = DriveMode.OTHER;
|
||||
|
||||
@@ -66,7 +66,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
private Field2d shooter_ll_field;
|
||||
private Field2d intaker_ll_field;
|
||||
|
||||
|
||||
public CommandSwerveDrivetrain(
|
||||
SwerveDrivetrainConstants drivetrainConstants,
|
||||
SwerveModuleConstants<?, ?, ?>... modules
|
||||
@@ -82,16 +81,13 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
LimelightHelpers.SetIMUMode(Constants.Limelight.LIMELIGHT_NAME_Intaker, 0);
|
||||
positioningNetworkTable = NetworkTableInstance.getDefault().getTable("Positioning");
|
||||
driveNetworkTable = NetworkTableInstance.getDefault().getTable("Drive");
|
||||
ll_shooter_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-shooter");
|
||||
ll_intaker_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-intaker");
|
||||
ll_shooter_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-left");
|
||||
ll_intaker_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-front");
|
||||
|
||||
configueAutoBuilder();
|
||||
|
||||
}
|
||||
|
||||
|
||||
private void configueAutoBuilder(){
|
||||
|
||||
private void configueAutoBuilder() {
|
||||
RobotConfig config;
|
||||
try {
|
||||
config = RobotConfig.fromGUISettings();
|
||||
@@ -100,20 +96,20 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
}
|
||||
|
||||
AutoBuilder.configure(
|
||||
this::getPose, // 当前机器人 Pose
|
||||
this::resetPose, // 重置里程计
|
||||
this::getRobotRelativeSpeeds, // ROBOT RELATIVE speeds
|
||||
(speeds, feedforwards) -> driveRobotRelative(speeds), // 用 speeds 驱动机器人
|
||||
this::getPose, // 当前机器人位姿。
|
||||
this::resetPose, // 重置里程计。
|
||||
this::getRobotRelativeSpeeds, // 机器人坐标系速度。
|
||||
(speeds, feedforwards) -> driveRobotRelative(speeds), // 按目标速度驱动机器人。
|
||||
new PPHolonomicDriveController(
|
||||
new PIDConstants(5.0, 0.0, 0.0), // X/Y PID
|
||||
new PIDConstants(5.0, 0.0, 0.0) // Theta PID
|
||||
new PIDConstants(5.0, 0.0, 0.0), // X/Y 位置 PID。
|
||||
new PIDConstants(5.0, 0.0, 0.0) // 朝向 PID。
|
||||
),
|
||||
config,
|
||||
() -> false,
|
||||
this
|
||||
);
|
||||
|
||||
}
|
||||
|
||||
public Pose2d getPose() {
|
||||
return getState().Pose;
|
||||
}
|
||||
@@ -122,11 +118,37 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
return getState().Speeds;
|
||||
}
|
||||
|
||||
public ChassisSpeeds getChassisSpeeds() {
|
||||
// 给上层代码使用的速度别名；原机器人坐标系方法继续留给 PathPlanner。
|
||||
return getRobotRelativeSpeeds();
|
||||
}
|
||||
|
||||
public ChassisSpeeds getFieldRelativeSpeeds() {
|
||||
return ChassisSpeeds.fromRobotRelativeSpeeds(getRobotRelativeSpeeds(), getRotation());
|
||||
}
|
||||
|
||||
public Rotation2d getRotation() {
|
||||
return getState().Pose.getRotation();
|
||||
}
|
||||
|
||||
public void setTargetHeading(Rotation2d targetHeading) {
|
||||
this.targetHeading = targetHeading;
|
||||
}
|
||||
|
||||
public double getHeadingError() {
|
||||
return targetHeading.minus(getRotation()).getDegrees();
|
||||
}
|
||||
|
||||
public boolean isAimed() {
|
||||
// 上层状态机只读取这个门控，不直接控制 swerve 模块。
|
||||
return Math.abs(getHeadingError()) <= Constants.Superstructure.aimToleranceDegrees;
|
||||
}
|
||||
|
||||
public void driveRobotRelative(ChassisSpeeds speeds) {
|
||||
ChassisSpeeds scaled = new ChassisSpeeds(
|
||||
speeds.vxMetersPerSecond, // X 不变
|
||||
speeds.vyMetersPerSecond, // Y 不变
|
||||
speeds.omegaRadiansPerSecond * 1.3 // 只放大旋转
|
||||
speeds.vxMetersPerSecond, // X 方向速度保持原值。
|
||||
speeds.vyMetersPerSecond, // Y 方向速度保持原值。
|
||||
speeds.omegaRadiansPerSecond * 1.3 // 只放大旋转输出，沿用原驾驶手感。
|
||||
);
|
||||
|
||||
setControl(
|
||||
@@ -140,14 +162,26 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
);
|
||||
}
|
||||
|
||||
private static final double kMaxVisionJumpMeters = 1.0;
|
||||
public void lockWheels() {
|
||||
setControl(new SwerveRequest.SwerveDriveBrake());
|
||||
}
|
||||
|
||||
public void aimToTarget() {
|
||||
// 给未来命令预留的高层辅助接口，仍走现有底盘请求路径，不触碰模块底层逻辑。
|
||||
double headingErrorDegrees = getHeadingError();
|
||||
double omega = Math.toRadians(headingErrorDegrees) * Constants.AutoPositioning.TurningkP;
|
||||
setControl(
|
||||
new SwerveRequest.ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0, 0, omega))
|
||||
);
|
||||
}
|
||||
|
||||
private static final double kMaxOmegaDegPerSec = 360.0;
|
||||
|
||||
/**
|
||||
* Returns a command that applies the specified control request to this swerve drivetrain.
|
||||
* 返回一个持续下发指定 swerve 请求的命令。
|
||||
*
|
||||
* @param request Function returning the request to apply
|
||||
* @return Command to run
|
||||
* @param request 生成待下发请求的函数。
|
||||
* @return 可调度执行的 Command。
|
||||
*/
|
||||
public Command applyRequest(Supplier<SwerveRequest> request) {
|
||||
return run(() -> this.setControl(request.get()));
|
||||
@@ -162,9 +196,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
}
|
||||
|
||||
public void forceUsingLimelightmt2() {
|
||||
if (Constants.DemoMode.ENABLED) {
|
||||
return;
|
||||
}
|
||||
LimelightHelpers.PoseEstimate mt2 =
|
||||
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter);
|
||||
|
||||
@@ -177,9 +208,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
}
|
||||
|
||||
public void forceUsingLimelightmt2WithllName(String llString) {
|
||||
if (Constants.DemoMode.ENABLED) {
|
||||
return;
|
||||
}
|
||||
LimelightHelpers.PoseEstimate mt2 =
|
||||
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(llString);
|
||||
|
||||
@@ -190,6 +218,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
|
||||
addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs);
|
||||
}
|
||||
|
||||
private void fuseLimelightmt2() {
|
||||
Pose2d current = getState().Pose;
|
||||
|
||||
@@ -208,32 +237,31 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
|
||||
LimelightHelpers.PoseEstimate mt2_final = null;
|
||||
|
||||
if(!shooterAvailable && !intakerAvailable) {
|
||||
if (!shooterAvailable && !intakerAvailable) {
|
||||
positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(false);
|
||||
positioningNetworkTable.getEntry("Usingll").setString("None");
|
||||
//positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(Double.NaN);
|
||||
|
||||
// 未使用视觉时不发布距离差，避免仪表盘误读旧值。
|
||||
return;
|
||||
}else if (shooterAvailable && !intakerAvailable) {
|
||||
} else if (shooterAvailable && !intakerAvailable) {
|
||||
double shooterDistance = current.getTranslation().getDistance(mt2_shooter.pose.getTranslation());
|
||||
ll_shooter_NT.getEntry("Distance").setDouble(shooterDistance);
|
||||
ll_shooter_NT.getEntry("X").setDouble(mt2_shooter.pose.getX());
|
||||
ll_shooter_NT.getEntry("Y").setDouble(mt2_shooter.pose.getY());
|
||||
positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true);
|
||||
positioningNetworkTable.getEntry("Usingll").setString("Shooter");
|
||||
//positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(Double.NaN);
|
||||
// 单个 Limelight 可用时直接采用该测量。
|
||||
mt2_final = mt2_shooter;
|
||||
}else if (!shooterAvailable && intakerAvailable) {
|
||||
} else if (!shooterAvailable && intakerAvailable) {
|
||||
double intakerDistance = current.getTranslation().getDistance(mt2_intaker.pose.getTranslation());
|
||||
ll_intaker_NT.getEntry("Distance").setDouble(intakerDistance);
|
||||
ll_intaker_NT.getEntry("X").setDouble(mt2_intaker.pose.getX());
|
||||
ll_intaker_NT.getEntry("Y").setDouble(mt2_intaker.pose.getY());
|
||||
positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true);
|
||||
positioningNetworkTable.getEntry("Usingll").setString("Intaker");
|
||||
//positioningNetworkTable.getEntry("MetaTag2DistanceDifference").setDouble(Double.NaN);
|
||||
// 单个 Limelight 可用时直接采用该测量。
|
||||
mt2_final = mt2_intaker;
|
||||
}else {
|
||||
// 两个都可用时的策略：这里简单选更近的，你也
|
||||
} else {
|
||||
// 两个 Limelight 都可用时，先选择与当前位姿更接近的测量。
|
||||
double shooterDistance = current.getTranslation().getDistance(mt2_shooter.pose.getTranslation());
|
||||
double intakerDistance = current.getTranslation().getDistance(mt2_intaker.pose.getTranslation());
|
||||
double difference = mt2_shooter.pose.getTranslation().getDistance(mt2_intaker.pose.getTranslation());
|
||||
@@ -264,11 +292,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
positioningNetworkTable.getEntry("PoseDistance").setDouble(distance);
|
||||
positioningNetworkTable.getEntry("RotationDifference").setDouble(Math.abs(deltaRotation.getDegrees()));
|
||||
|
||||
// 4) 设置测量噪声（示例：theta 基本不信任就给极大）
|
||||
// 你也可以根据 mt2.avgTagDist / mt2.tagSpan 动态调
|
||||
// 视觉测量噪声：theta 基本不信任，只融合平移。
|
||||
// 后续可以根据 mt2.avgTagDist / mt2.tagSpan 动态调整。
|
||||
var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999);
|
||||
|
||||
// 5) 融合：注意 timestampSeconds 的处理
|
||||
// 融合视觉测量，注意 timestampSeconds 的时间基转换。
|
||||
addVisionMeasurement(mt2_final.pose, mt2_final.timestampSeconds, stdDevs);
|
||||
positioningNetworkTable.getEntry("MetaTag2Available").setBoolean(true);
|
||||
}
|
||||
@@ -279,192 +307,53 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
}
|
||||
return LimelightHelpers.getBotPoseEstimate_wpiBlue(limelightName);
|
||||
}
|
||||
|
||||
private boolean ifLimelightPositioningAvailable(LimelightHelpers.PoseEstimate mt2, Pose2d currentPose) {
|
||||
// 1) 没看到 tag：拒绝
|
||||
// 没看到 tag 时拒绝该帧视觉定位。
|
||||
if (mt2 == null || mt2.tagCount == 0) {
|
||||
return false;
|
||||
}
|
||||
// 2) 转太快：拒绝（角速度你可以从 Pigeon2 或 drivetrain state 拿）
|
||||
double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble(); // 若你有这个信号
|
||||
|
||||
// 旋转太快时拒绝该帧，避免高速转动时的视觉跳变。
|
||||
double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble();
|
||||
if (Math.abs(omegaDegPerSec) > kMaxOmegaDegPerSec) {
|
||||
//System.out.println("NoUsingMetaTag2ForTooFastRotationSpeed");
|
||||
return false;
|
||||
}
|
||||
|
||||
// 3) 跳变太大：拒绝（Phoenix6 推荐的稳定策略）
|
||||
//Pose2d current = getState().Pose;
|
||||
// if (currentPose.getTranslation().getDistance(mt2.pose.getTranslation()) > kMaxVisionJumpMeters) {
|
||||
// //System.out.println("NoUsingMetaTag2ForTooChangingPose");
|
||||
// return false;
|
||||
// }
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
private double clamp(double value, double min, double max) {
|
||||
return Math.max(min, Math.min(max, value));
|
||||
}
|
||||
|
||||
// public String driveToAprilTag() {
|
||||
// System.out.println("Pipeline = " + LimelightHelpers.getCurrentPipelineIndex(Constants.Limelight.LIMELIGHT_NAME_Intaker));
|
||||
// System.out.println("TV = " + LimelightSupplier.isTargetVisible());
|
||||
|
||||
// if (!LimelightSupplier.isTargetVisible()) {
|
||||
// isAlignedToAprilTag = false;
|
||||
// //stop();
|
||||
// return "No Target";
|
||||
// }
|
||||
|
||||
// RawFiducial[] fiducials =
|
||||
// LimelightHelpers.getRawFiducials(Constants.Limelight.LIMELIGHT_NAME_Intaker);
|
||||
|
||||
// boolean foundDesiredTag = false;
|
||||
// for (RawFiducial fiducial : fiducials) {
|
||||
// if (fiducial.id == 16 || fiducial.id == 32) {
|
||||
// foundDesiredTag = true;
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
|
||||
// if (!foundDesiredTag) {
|
||||
// isAlignedToAprilTag = false;
|
||||
// stop();
|
||||
// return "No Desired Tag";
|
||||
// }
|
||||
|
||||
// // ===== 读取原始误差 =====
|
||||
// double tx = LimelightSupplier.getTX(); // deg
|
||||
// double tz = -LimelightSupplier.getTargetTZ(); // m，方向按你当前经验保留负号
|
||||
// double ry = LimelightSupplier.getTargetRotationY(); // deg
|
||||
|
||||
// // ===== 用原始误差判断是否已对齐 =====
|
||||
// isAlignedToAprilTag =
|
||||
// Math.abs(tx) < Constants.Limelight.AutoClimbToleranceTX &&
|
||||
// Math.abs(tz) < Constants.Limelight.AutoClimbToleranceTZ &&
|
||||
// Math.abs(ry) < Constants.Limelight.AutoClimbToleranceRY;
|
||||
|
||||
// if (isAlignedToAprilTag) {
|
||||
// stop();
|
||||
// return "Aligned";
|
||||
// }
|
||||
|
||||
// // ===== 计算输出 =====
|
||||
// double vx = 0.0;
|
||||
// double vy = 0.0;
|
||||
// double omega = 0.0;
|
||||
|
||||
// // 1) 前后控制：根据 tz 前进/后退
|
||||
// if (Math.abs(tz) > Constants.Limelight.AutoClimbToleranceTZ) {
|
||||
// vx = tz * Constants.Limelight.AutoClimbKpTZ;
|
||||
|
||||
// // 最小输出，避免快到目标时不动
|
||||
// if (Math.abs(vx) < Constants.Limelight.AutoClimbMinVX) {
|
||||
// vx = Math.copySign(Constants.Limelight.AutoClimbMinVX, vx);
|
||||
// }
|
||||
|
||||
// // 最大输出限幅
|
||||
// vx = clamp(vx,
|
||||
// -Constants.Limelight.AutoClimbMaxVX,
|
||||
// Constants.Limelight.AutoClimbMaxVX);
|
||||
// }
|
||||
|
||||
// // 2) 旋转控制：tx 为主，ry 为辅
|
||||
// // tx 负责“镜头中心对准 tag”
|
||||
// // ry 负责“机器人姿态微调”
|
||||
// // double omegaFromTx = 0.0;
|
||||
// // double omegaFromRy = 0.0;
|
||||
|
||||
// // if (Math.abs(tx) > Constants.Limelight.AutoClimbToleranceTX) {
|
||||
// // omegaFromTx = -tx * Constants.Limelight.AutoClimbKpTX;
|
||||
// // }
|
||||
|
||||
// // if (Math.abs(ry) > Constants.Limelight.AutoClimbToleranceRY) {
|
||||
// // omegaFromRy = ry * Constants.Limelight.AutoClimbKpRY;
|
||||
// // }
|
||||
|
||||
// // omega = omegaFromTx + omegaFromRy;
|
||||
|
||||
// // // 大偏角时直接给较明显的转速，先把朝向拉回来
|
||||
// // if (Math.abs(tx) > Constants.Limelight.AutoClimbFastTurnThresholdTX) {
|
||||
// // omega = Math.copySign(Constants.Limelight.AutoClimbFastTurnOmega, omega);
|
||||
// // }
|
||||
|
||||
// // // 最小输出，避免转不动
|
||||
// // if (Math.abs(omega) > 1e-6 && Math.abs(omega) < Constants.Limelight.AutoClimbMinOmega) {
|
||||
// // omega = Math.copySign(Constants.Limelight.AutoClimbMinOmega, omega);
|
||||
// // }
|
||||
|
||||
// // // 最大输出限幅
|
||||
// // omega = clamp(omega,
|
||||
// // -Constants.Limelight.AutoClimbMaxOmega,
|
||||
// // Constants.Limelight.AutoClimbMaxOmega);
|
||||
|
||||
// Rotation2d ClimbcurrentAngle = getState().Pose.getRotation();
|
||||
// double climbCurrentRad = ClimbcurrentAngle.getRadians();
|
||||
// double TargetAngleforClimbing = Math.toRadians(0);
|
||||
// double Climberror = TargetAngleforClimbing - climbCurrentRad;
|
||||
// if (Math.abs(Climberror) < Math.toRadians(Constants.AutoPositioning.autoPositioningAngleError)) {
|
||||
// omega = 0.0;
|
||||
// } else {
|
||||
// omega = Climberror * Constants.AutoPositioning.TurningkP;
|
||||
// }
|
||||
|
||||
|
||||
// // ===== 调试输出 =====
|
||||
// driveNetworkTable.getEntry("ClimbTX").setDouble(tx);
|
||||
// driveNetworkTable.getEntry("ClimbTZ").setDouble(tz);
|
||||
// driveNetworkTable.getEntry("ClimbRY").setDouble(ry);
|
||||
// driveNetworkTable.getEntry("ClimbVXCmd").setDouble(vx);
|
||||
// driveNetworkTable.getEntry("ClimbOmegaCmd").setDouble(omega);
|
||||
// driveNetworkTable.getEntry("ClimbAligned").setBoolean(isAlignedToAprilTag);
|
||||
|
||||
// // ===== 下发到底盘 =====
|
||||
// driveRobotRelative(new ChassisSpeeds(vx, vy, omega));
|
||||
|
||||
// return "Driving to AprilTag";
|
||||
// }
|
||||
|
||||
// public boolean isAlignedToAprilTag(){
|
||||
// return isAlignedToAprilTag;
|
||||
// }
|
||||
@Override
|
||||
public void periodic() {
|
||||
|
||||
Pose2d pose = getState().Pose;
|
||||
m_field.setRobotPose(pose);
|
||||
posePub.set(pose);
|
||||
LimelightHelpers.PoseEstimate shooterEstimate =
|
||||
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter);
|
||||
LimelightHelpers.PoseEstimate intakerEstimate =
|
||||
LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Intaker);
|
||||
if (shooterEstimate != null && shooterEstimate.tagCount > 0) {
|
||||
shooter_ll_field.setRobotPose(shooterEstimate.pose);
|
||||
}
|
||||
if (intakerEstimate != null && intakerEstimate.tagCount > 0) {
|
||||
intaker_ll_field.setRobotPose(intakerEstimate.pose);
|
||||
}
|
||||
shooter_ll_field.setRobotPose(LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter).pose);
|
||||
intaker_ll_field.setRobotPose(LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Intaker).pose);
|
||||
driveNetworkTable.getEntry("YawFromState").setDouble(getState().Pose.getRotation().getDegrees());
|
||||
driveNetworkTable.getEntry("DriveMode").setString(m_driveMode.name());
|
||||
driveNetworkTable.getEntry("IsFieldCentric").setBoolean(m_driveMode == DriveMode.FIELD_CENTRIC);
|
||||
//positioningNetworkTable.getEntry("isAlignedToAprilTag").setBoolean(isAlignedToAprilTag());
|
||||
// positioningNetworkTable.getEntry("interestPointsClimber TX").setDouble(LimelightSupplier.getTX());
|
||||
// positioningNetworkTable.getEntry("interestPointsClimber RY").setDouble(LimelightSupplier.getTargetRotationY());
|
||||
// positioningNetworkTable.getEntry("interestPointsClimber TZ").setDouble(-LimelightSupplier.getTargetTZ());
|
||||
|
||||
// 保留 Positioning 表中的基础状态输出。
|
||||
positioningNetworkTable.getEntry("interestPointsCl").setBoolean(false);
|
||||
// 1) 每帧喂给 LL：机器人当前 yaw（度），其余先全 0
|
||||
|
||||
// 每帧向 Limelight 写入当前 yaw，供 MegaTag2 使用。
|
||||
LimelightHelpers.SetRobotOrientation(
|
||||
Constants.Limelight.LIMELIGHT_NAME_Shooter,
|
||||
pose.getRotation().getDegrees(), // 关键：要符合 wpiBlue 约定
|
||||
pose.getRotation().getDegrees(), // 符合 wpiBlue 坐标约定。
|
||||
0, 0, 0, 0, 0
|
||||
);
|
||||
LimelightHelpers.SetRobotOrientation(
|
||||
Constants.Limelight.LIMELIGHT_NAME_Intaker,
|
||||
pose.getRotation().getDegrees(), // 关键：要符合 wpiBlue 约定
|
||||
pose.getRotation().getDegrees(), // 符合 wpiBlue 坐标约定。
|
||||
0, 0, 0, 0, 0
|
||||
);
|
||||
|
||||
if (!Constants.DemoMode.ENABLED) {
|
||||
fuseLimelightmt2();
|
||||
}
|
||||
|
||||
if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
|
||||
DriverStation.getAlliance().ifPresent(allianceColor -> {
|
||||
@@ -478,13 +367,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
|
||||
* while still accounting for measurement noise.
|
||||
* 添加一帧视觉位姿测量，用于修正里程计估计。
|
||||
*
|
||||
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
|
||||
* @param timestampSeconds The timestamp of the vision measurement in seconds.
|
||||
* @param visionRobotPoseMeters 视觉测得的机器人位姿。
|
||||
* @param timestampSeconds 视觉测量时间戳，单位秒。
|
||||
*/
|
||||
@Override
|
||||
public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
|
||||
@@ -492,17 +379,11 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
|
||||
* while still accounting for measurement noise.
|
||||
* <p>
|
||||
* Note that the vision measurement standard deviations passed into this method
|
||||
* will continue to apply to future measurements until a subsequent call to
|
||||
* {@link #setVisionMeasurementStdDevs(Matrix)} or this method.
|
||||
* 添加一帧带标准差的视觉位姿测量。
|
||||
*
|
||||
* @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
|
||||
* @param timestampSeconds The timestamp of the vision measurement in seconds.
|
||||
* @param visionMeasurementStdDevs Standard deviations of the vision pose measurement
|
||||
* in the form [x, y, theta]ᵀ, with units in meters and radians.
|
||||
* @param visionRobotPoseMeters 视觉测得的机器人位姿。
|
||||
* @param timestampSeconds 视觉测量时间戳，单位秒。
|
||||
* @param visionMeasurementStdDevs 视觉测量标准差，格式为 [x, y, theta]。
|
||||
*/
|
||||
@Override
|
||||
public void addVisionMeasurement(
|
||||
@@ -514,10 +395,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the pose at a given timestamp, if the buffer is not empty.
|
||||
* 从位姿缓存中读取指定时间戳的机器人位姿。
|
||||
*
|
||||
* @param timestampSeconds The timestamp of the pose in seconds.
|
||||
* @return The pose at the given timestamp (or Optional.empty() if the buffer is empty).
|
||||
* @param timestampSeconds 位姿时间戳，单位秒。
|
||||
* @return 指定时间的位姿；缓存为空时返回 Optional.empty()。
|
||||
*/
|
||||
@Override
|
||||
public Optional<Pose2d> samplePoseAt(double timestampSeconds) {
|
||||
|
||||
@@ -0,0 +1,44 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix6.controls.VoltageOut;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import frc.robot.Constants;
|
||||
|
||||
/**
|
||||
* 主 conveyor 的开环控制封装。
|
||||
*/
|
||||
public class ConveyorSubsystem extends SubsystemBase {
|
||||
public final TalonFX mainConveyor;
|
||||
private final VoltageOut voltageRequest = new VoltageOut(0);
|
||||
private final NetworkTable table = NetworkTableInstance.getDefault().getTable("Conveyor");
|
||||
private double targetOutputVolts = 0.0;
|
||||
|
||||
public ConveyorSubsystem() {
|
||||
mainConveyor = new TalonFX(Constants.Conveyor.mainConveyorID);
|
||||
mainConveyor.getConfigurator().apply(Constants.Conveyor.mainConveyorSlot0Configs);
|
||||
}
|
||||
public void feedToShooter() {
|
||||
setVoltage(Constants.Conveyor.feedVoltage);
|
||||
}
|
||||
public void reverse() {
|
||||
setVoltage(Constants.Conveyor.reverseVoltage);
|
||||
}
|
||||
public void stop() {
|
||||
setVoltage(0.0);
|
||||
}
|
||||
public void setVoltage(double outputVolts) {
|
||||
targetOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
|
||||
mainConveyor.setControl(voltageRequest.withOutput(targetOutputVolts));
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
table.getEntry("velocity").setDouble(mainConveyor.getVelocity().getValueAsDouble());
|
||||
table.getEntry("targetOutputVolts").setDouble(targetOutputVolts);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,73 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix6.controls.Follower;
|
||||
import com.ctre.phoenix6.controls.VoltageOut;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.signals.MotorAlignmentValue;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.Constants;
|
||||
|
||||
/**
|
||||
* 进球滚轮的硬件封装。
|
||||
*
|
||||
* 何时 intake、outtake、hold 或 stop 由 Superstructure 决定；
|
||||
* 本类只负责成对 TalonFX 输出和 follower 关系。
|
||||
*/
|
||||
public class IntakeRollerSubsystem extends SubsystemBase {
|
||||
public final TalonFX leftIntakeRoller;
|
||||
private final TalonFX rightIntakeRoller;
|
||||
|
||||
private final VoltageOut voltageRequest = new VoltageOut(0);
|
||||
private final Follower rightFollower;
|
||||
private final NetworkTable table = NetworkTableInstance.getDefault().getTable("IntakeRoller");
|
||||
private double targetOutputVolts = 0.0;
|
||||
|
||||
public IntakeRollerSubsystem() {
|
||||
leftIntakeRoller = new TalonFX(Constants.Intaker.leftIntakeRollerID);
|
||||
rightIntakeRoller = new TalonFX(Constants.Intaker.rightIntakeRollerID);
|
||||
leftIntakeRoller.getConfigurator().apply(Constants.Intaker.intakeRollerSlot0Configs);
|
||||
rightIntakeRoller.getConfigurator().apply(Constants.Intaker.intakeRollerSlot0Configs);
|
||||
rightFollower = new Follower(leftIntakeRoller.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
|
||||
followLeft();
|
||||
}
|
||||
|
||||
public void intake() {
|
||||
setVoltage(Constants.Intaker.intakeRollerIntakeVoltage);
|
||||
}
|
||||
|
||||
public void outtake() {
|
||||
setVoltage(Constants.Intaker.intakeRollerOuttakeVoltage);
|
||||
}
|
||||
|
||||
public void stop() {
|
||||
setVoltage(0.0);
|
||||
}
|
||||
|
||||
public void setVoltage(double outputVolts) {
|
||||
targetOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
|
||||
leftIntakeRoller.setControl(voltageRequest.withOutput(targetOutputVolts));
|
||||
followLeft();
|
||||
}
|
||||
|
||||
private void followLeft() {
|
||||
rightIntakeRoller.setControl(rightFollower);
|
||||
}
|
||||
|
||||
private double getVelocityDifference() {
|
||||
double leftVelocity = leftIntakeRoller.getVelocity().getValueAsDouble();
|
||||
double rightVelocity = rightIntakeRoller.getVelocity().getValueAsDouble();
|
||||
return Math.abs(leftVelocity) - Math.abs(rightVelocity);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
table.getEntry("leftVelocity").setDouble(leftIntakeRoller.getVelocity().getValueAsDouble());
|
||||
table.getEntry("rightVelocity").setDouble(rightIntakeRoller.getVelocity().getValueAsDouble());
|
||||
table.getEntry("velocityDifference").setDouble(getVelocityDifference());
|
||||
table.getEntry("targetOutputVolts").setDouble(targetOutputVolts);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,131 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix6.controls.Follower;
|
||||
import com.ctre.phoenix6.controls.VoltageOut;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.signals.MotorAlignmentValue;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.Constants;
|
||||
|
||||
/**
|
||||
* Intaker rotation control for the 2026 fuel path.
|
||||
*
|
||||
* The rotater uses timed voltage actions only. Position is published for
|
||||
* telemetry, but it is not used for closed-loop control.
|
||||
*/
|
||||
public class IntakeRotaterSubsystem extends SubsystemBase {
|
||||
public final TalonFX leftIntakeRotater;
|
||||
private final TalonFX rightIntakeRotater;
|
||||
|
||||
private final VoltageOut voltageRequest = new VoltageOut(0);
|
||||
private final Follower rightFollower;
|
||||
private final NetworkTable table = NetworkTableInstance.getDefault().getTable("IntakeRotater");
|
||||
|
||||
private double outputVoltage = 0.0;
|
||||
|
||||
public IntakeRotaterSubsystem() {
|
||||
leftIntakeRotater = new TalonFX(Constants.Intaker.leftIntakeRotaterID);
|
||||
rightIntakeRotater = new TalonFX(Constants.Intaker.rightIntakeRotaterID);
|
||||
leftIntakeRotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);
|
||||
rightIntakeRotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);
|
||||
rightFollower = new Follower(leftIntakeRotater.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
|
||||
followLeft();
|
||||
|
||||
leftIntakeRotater.setPosition(0.0);
|
||||
rightIntakeRotater.setPosition(0.0);
|
||||
}
|
||||
|
||||
public void lowerForMatch() {
|
||||
setVoltage(0.05);
|
||||
}
|
||||
|
||||
public Command lowerForMatchCommand() {
|
||||
return Commands.sequence(
|
||||
Commands.runOnce(() -> setVoltage(-1.5), this),
|
||||
Commands.waitSeconds(0.7),
|
||||
Commands.runOnce(() -> setVoltage(0.05), this),
|
||||
Commands.waitSeconds(0.7),
|
||||
Commands.runOnce(this::stop, this)
|
||||
);
|
||||
}
|
||||
|
||||
public void raiseForMatch() {
|
||||
setVoltage(0.8);
|
||||
}
|
||||
|
||||
public Command raiseForMatchCommand() {
|
||||
return Commands.sequence(
|
||||
Commands.runOnce(this::raiseForMatch, this),
|
||||
Commands.waitSeconds(0.5),
|
||||
Commands.runOnce(this::stop, this)
|
||||
);
|
||||
}
|
||||
|
||||
public void shootAssist() {
|
||||
double period = Constants.Intaker.intakeRotaterShootAssistPeriodSeconds;
|
||||
double phase = (Timer.getFPGATimestamp() % period) / period;
|
||||
double volts = phase < Constants.Intaker.intakeRotaterShootAssistUpDutyCycle
|
||||
? Constants.Intaker.intakeRotaterRaiseVoltage
|
||||
: Constants.Intaker.intakeRotaterLowerVoltage;
|
||||
setVoltage(volts);
|
||||
}
|
||||
|
||||
public void manualRaise() {
|
||||
setVoltage(Constants.Intaker.intakeRotaterRaiseVoltage);
|
||||
}
|
||||
|
||||
public void manualLower() {
|
||||
setVoltage(Constants.Intaker.intakeRotaterLowerVoltage);
|
||||
}
|
||||
|
||||
public void stop() {
|
||||
setVoltage(0.0);
|
||||
}
|
||||
|
||||
public boolean isRunning() {
|
||||
return Math.abs(outputVoltage) > 1e-6;
|
||||
}
|
||||
|
||||
public double getPosition() {
|
||||
return leftIntakeRotater.getPosition().getValueAsDouble();
|
||||
}
|
||||
|
||||
public double getLeftVelocity() {
|
||||
return leftIntakeRotater.getVelocity().getValueAsDouble();
|
||||
}
|
||||
|
||||
public double getRightVelocity() {
|
||||
return rightIntakeRotater.getVelocity().getValueAsDouble();
|
||||
}
|
||||
|
||||
private double getVelocityDifference() {
|
||||
return Math.abs(getLeftVelocity()) - Math.abs(getRightVelocity());
|
||||
}
|
||||
|
||||
private void setVoltage(double volts) {
|
||||
outputVoltage = MathUtil.clamp(volts, -12.0, 12.0);
|
||||
leftIntakeRotater.setControl(voltageRequest.withOutput(outputVoltage));
|
||||
followLeft();
|
||||
}
|
||||
|
||||
private void followLeft() {
|
||||
rightIntakeRotater.setControl(rightFollower);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
table.getEntry("position").setDouble(getPosition());
|
||||
table.getEntry("isRunning").setBoolean(isRunning());
|
||||
table.getEntry("outputVoltage").setDouble(outputVoltage);
|
||||
table.getEntry("leftVelocity").setDouble(getLeftVelocity());
|
||||
table.getEntry("rightVelocity").setDouble(getRightVelocity());
|
||||
table.getEntry("velocityDifference").setDouble(getVelocityDifference());
|
||||
}
|
||||
}
|
||||
@@ -1,100 +0,0 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix6.controls.PositionVoltage;
|
||||
import com.ctre.phoenix6.controls.VelocityVoltage;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
|
||||
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.Constants;
|
||||
|
||||
public class IntakerSubsystem extends SubsystemBase{
|
||||
//定义intakerotater&intakegetter
|
||||
private TalonFX intakerotater;
|
||||
private TalonFX intakegetter;
|
||||
|
||||
//PID
|
||||
private final VelocityVoltage velocityRequest = new VelocityVoltage(0);
|
||||
private final PositionVoltage positionRequest = new PositionVoltage(0);
|
||||
|
||||
private double intakeRotaterTargetPosition = 0.0;
|
||||
|
||||
public double getGravityFFVolts(){
|
||||
double intakerotaterPosition = getIntakerotaterPosition();
|
||||
double gravityFFVolts = Constants.Intaker.IntakeRotaterGravityFF * intakerotaterPosition;
|
||||
return gravityFFVolts;
|
||||
}
|
||||
|
||||
public void setIntakerRotaterVelocityRps(double IntakeRotaterTargetVelocity) {
|
||||
double gravityFeedforward = getGravityFFVolts();
|
||||
intakerotater.setControl(velocityRequest.withVelocity(IntakeRotaterTargetVelocity).withFeedForward(gravityFeedforward));// 重力前馈);
|
||||
}
|
||||
|
||||
public IntakerSubsystem() {
|
||||
//初始化rotater&getter
|
||||
intakegetter = new TalonFX(47);
|
||||
intakerotater = new TalonFX(48);
|
||||
intakegetter.getConfigurator().apply(Constants.Intaker.intakeGetterSlot0Configs);
|
||||
intakerotater.getConfigurator().apply(Constants.Intaker.intakeRotaterSlot0Configs);
|
||||
}
|
||||
|
||||
public void setIntakerRotaterPosition(double targetPosition) {
|
||||
//目标position
|
||||
if (targetPosition > Constants.Intaker.intakeRotaterUpLimit) {
|
||||
targetPosition = Constants.Intaker.intakeRotaterUpLimit; // 限制最大上限
|
||||
} else if (targetPosition < Constants.Intaker.intakeRotaterDownLimit) {
|
||||
targetPosition = Constants.Intaker.intakeRotaterDownLimit; // 限制最小下限
|
||||
}
|
||||
|
||||
intakeRotaterTargetPosition = targetPosition;
|
||||
intakerotater.setControl(positionRequest.withPosition(targetPosition)); // 设置rotater位置（TalonFX 内部闭环 PID）
|
||||
}
|
||||
|
||||
// 获取intakerotater当前角度
|
||||
public double getIntakerotaterPosition() {
|
||||
return intakerotater.getPosition().getValueAsDouble();
|
||||
}
|
||||
|
||||
public boolean isIntakerotaterAtTarget() {
|
||||
// 到位判断容差
|
||||
double error = Math.abs(intakeRotaterTargetPosition - getIntakerotaterPosition());
|
||||
return error <= Constants.Intaker.positionToleranceRotations;
|
||||
}
|
||||
|
||||
public void stopIntakerotater() {
|
||||
intakerotater.set(0);
|
||||
// 如果你希望停止后保持当前位置，建议改用中性模式/或维持 positionRequest（这句fromAI）
|
||||
}
|
||||
|
||||
//intakegetter的on/off
|
||||
// public void setIntakeGetterOn(boolean on) {
|
||||
// if (on) {
|
||||
// intakegetter.setControl(velocityRequest.withVelocity(Constants.Intaker.IntakeGetterSpeed));
|
||||
// }
|
||||
// else {
|
||||
// intakegetter.setControl(velocityRequest.withVelocity(0));
|
||||
// }
|
||||
// }
|
||||
public void setIntakerGetterSpeed(double speed) {
|
||||
intakegetter.set(speed);
|
||||
}
|
||||
public void zeroEncoder(){
|
||||
intakerotater.setPosition(0);
|
||||
stopIntakerotater();
|
||||
}
|
||||
public void IntakerDownNonStop(){
|
||||
intakerotater.set(-0.1);
|
||||
}
|
||||
|
||||
public void IntakerUpNonStop(){
|
||||
intakerotater.set(0.2);
|
||||
}
|
||||
|
||||
public void setIntakerRotater(double rotaterspeed){
|
||||
intakerotater.set(rotaterspeed);
|
||||
}
|
||||
public void stopAll() {
|
||||
stopIntakerotater();
|
||||
setIntakerGetterSpeed(0);
|
||||
}
|
||||
}
|
||||
@@ -18,6 +18,7 @@ public class LEDSubsystem extends SubsystemBase {
|
||||
private final AddressableLED led;
|
||||
private final AddressableLEDBuffer buffer;
|
||||
private final Timer timer = new Timer();
|
||||
private final boolean enabled;
|
||||
|
||||
// 左右灯带分段（左 0~71，右 72~143）
|
||||
private final int leftStart = 0;
|
||||
@@ -50,11 +51,15 @@ public class LEDSubsystem extends SubsystemBase {
|
||||
|
||||
led = new AddressableLED(pwmPort);
|
||||
buffer = new AddressableLEDBuffer(leftLen + rightLen);
|
||||
enabled = Constants.LEDUsing;
|
||||
|
||||
if (enabled) {
|
||||
led.setLength(buffer.getLength());
|
||||
led.start();
|
||||
|
||||
timer.start();
|
||||
} else {
|
||||
led.close();
|
||||
}
|
||||
}
|
||||
|
||||
/* ====================== */
|
||||
@@ -93,7 +98,9 @@ public class LEDSubsystem extends SubsystemBase {
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
if(!Constants.LEDUsing) {led.close(); return;}
|
||||
if (!enabled) {
|
||||
return;
|
||||
}
|
||||
// 左段
|
||||
switch (leftMode) {
|
||||
case OFF -> setRange(leftStart, leftLen, 0, 0, 0);
|
||||
|
||||
@@ -0,0 +1,54 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.Constants;
|
||||
|
||||
/**
|
||||
* 对现有 LimelightHelpers 的薄只读封装。
|
||||
*
|
||||
* 本类不做视觉融合，也不控制任何机构；只给 Superstructure 提供稳定的
|
||||
* 目标和距离读取接口。
|
||||
*/
|
||||
public class LimelightSubsystem extends SubsystemBase {
|
||||
private final String limelightName;
|
||||
|
||||
public LimelightSubsystem() {
|
||||
this(Constants.Limelight.LIMELIGHT_NAME_Shooter);
|
||||
}
|
||||
|
||||
public LimelightSubsystem(String limelightName) {
|
||||
this.limelightName = limelightName;
|
||||
}
|
||||
|
||||
public boolean hasTarget() {
|
||||
return LimelightHelpers.getTV(limelightName);
|
||||
}
|
||||
|
||||
public double getTargetYaw() {
|
||||
return LimelightHelpers.getTX(limelightName);
|
||||
}
|
||||
|
||||
public double getTargetPitch() {
|
||||
return LimelightHelpers.getTY(limelightName);
|
||||
}
|
||||
|
||||
public double getDistanceToTarget() {
|
||||
// DriveControls 已经把基于场地位姿计算的距离发布到 AutoControl；
|
||||
// 这里复用该值，保留旧定位和目标计算逻辑。
|
||||
return NetworkTableInstance.getDefault()
|
||||
.getTable("AutoControl")
|
||||
.getEntry("distanceToHub")
|
||||
.getDouble(0.0);
|
||||
}
|
||||
|
||||
public Pose2d getEstimatedPose() {
|
||||
return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(limelightName).pose;
|
||||
}
|
||||
|
||||
public double getShooterVoltageByDistance() {
|
||||
// 查 Constants 里的插值射表；真车试射后往表里加实测点即可。
|
||||
return Constants.Shooter.flywheelVoltageByDistance.get(getDistanceToTarget());
|
||||
}
|
||||
}
|
||||
@@ -3,192 +3,340 @@ package frc.robot.subsystems;
|
||||
import com.ctre.phoenix6.configs.MotorOutputConfigs;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.controls.Follower;
|
||||
import com.ctre.phoenix6.controls.VelocityDutyCycle;
|
||||
import com.ctre.phoenix6.controls.VelocityVoltage;
|
||||
import com.ctre.phoenix6.controls.VoltageOut;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import com.ctre.phoenix6.hardware.TalonFXS;
|
||||
import com.ctre.phoenix6.signals.InvertedValue;
|
||||
import com.ctre.phoenix6.signals.MotorAlignmentValue;
|
||||
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.Constants;
|
||||
|
||||
/**
|
||||
* Shooter 总成的硬件封装。
|
||||
*
|
||||
* 飞轮和 shooter 内部 conveyor 使用 VoltageOut 开环电压输出；
|
||||
* shooter 内部 conveyor 仍由右侧电机反向 follower 跟随左侧电机。
|
||||
* 背板控制沿用旧代码中的电机、编码器和 PID 调参流程。
|
||||
* 何时 spin up、何时喂球由 Superstructure 决定，本类不做比赛动作判断。
|
||||
*/
|
||||
public class ShooterSubsystem extends SubsystemBase {
|
||||
// 定义飞轮、传动和背板电机
|
||||
public TalonFX flywheelMotorLeft;
|
||||
private TalonFX flywheelMotorRight;
|
||||
public TalonFX conveyorMotor;
|
||||
public TalonFXS backboardMotor;
|
||||
public Encoder backboardEncoder;
|
||||
public final TalonFX leftFlywheel;
|
||||
public final TalonFX rightFlywheel;
|
||||
public final TalonFX leftConveyor;
|
||||
public final TalonFX rightConveyor;
|
||||
|
||||
// PID 控制器，控制背板角度
|
||||
public ProfiledPIDController backboardPID;
|
||||
private Follower flyWheelFollower;
|
||||
public final TalonFXS backboardMotor;
|
||||
public final Encoder backboardEncoder;
|
||||
public final ProfiledPIDController backboardPID;
|
||||
|
||||
private NetworkTable shooterNetworkTable;
|
||||
private final VoltageOut leftFlywheelVoltageRequest = new VoltageOut(0);
|
||||
private final VoltageOut rightFlywheelVoltageRequest = new VoltageOut(0);
|
||||
private final VoltageOut conveyorVoltageRequest = new VoltageOut(0);
|
||||
private final VoltageOut backboardVoltageRequest = new VoltageOut(0);
|
||||
|
||||
private final VelocityVoltage velocityRequest = new VelocityVoltage(0);
|
||||
private final Follower conveyorFollower;
|
||||
|
||||
private final NetworkTable shooterNetworkTable;
|
||||
private final NetworkTable backboardTuningTable;
|
||||
|
||||
private double flywheelOutputVolts = 0.0;
|
||||
private double flywheelVoltageStartTimestamp = 0.0;
|
||||
private double shooterConveyorOutputVolts = 0.0;
|
||||
private double backboardOutputVolts = 0.0;
|
||||
private double backboardRawPidOutputVolts = 0.0;
|
||||
private double backboardClampedPidOutputVolts = 0.0;
|
||||
private boolean backboardOutputSaturated = false;
|
||||
private boolean backboardMinimumOutputActive = false;
|
||||
|
||||
// 初始化各个部件
|
||||
public ShooterSubsystem() {
|
||||
// 初始化飞轮电机
|
||||
flywheelMotorLeft = new TalonFX(41); // 41是飞轮left的CAN ID
|
||||
flywheelMotorRight = new TalonFX(42); // 42是飞轮right的CAN ID
|
||||
flywheelMotorLeft.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
|
||||
flywheelMotorRight.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
|
||||
// 初始化传动电机
|
||||
conveyorMotor = new TalonFX(43);
|
||||
conveyorMotor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
|
||||
leftFlywheel = new TalonFX(Constants.Shooter.leftFlywheelID);
|
||||
rightFlywheel = new TalonFX(Constants.Shooter.rightFlywheelID);
|
||||
leftConveyor = new TalonFX(Constants.Shooter.leftConveyorID);
|
||||
rightConveyor = new TalonFX(Constants.Shooter.rightConveyorID);
|
||||
|
||||
var configs1 = new MotorOutputConfigs();
|
||||
configs1.Inverted = InvertedValue.Clockwise_Positive;
|
||||
flywheelMotorLeft.getConfigurator().apply(configs1);
|
||||
flyWheelFollower = new Follower(flywheelMotorLeft.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
|
||||
leftFlywheel.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
|
||||
rightFlywheel.getConfigurator().apply(Constants.Shooter.flyWheelSlot0Configs);
|
||||
leftConveyor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
|
||||
rightConveyor.getConfigurator().apply(Constants.Shooter.conveyorSlot0Configs);
|
||||
|
||||
var configs2 = new MotorOutputConfigs();
|
||||
configs2.Inverted = InvertedValue.Clockwise_Positive;
|
||||
conveyorMotor.getConfigurator().apply(configs2);
|
||||
var motorOutputConfigs = new MotorOutputConfigs();
|
||||
motorOutputConfigs.Inverted = InvertedValue.Clockwise_Positive;
|
||||
leftFlywheel.getConfigurator().apply(motorOutputConfigs);
|
||||
leftConveyor.getConfigurator().apply(motorOutputConfigs);
|
||||
|
||||
// 初始化背板电机和编码器
|
||||
backboardMotor = new TalonFXS(44);
|
||||
conveyorFollower = new Follower(leftConveyor.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50);
|
||||
setRightConveyorFollowLeft();
|
||||
|
||||
backboardMotor = new TalonFXS(Constants.Shooter.backboardMotorID);
|
||||
backboardMotor.getConfigurator().apply(Constants.Shooter.backboardSlot0Configs);
|
||||
|
||||
backboardEncoder = new Encoder(0, 1, false, Encoder.EncodingType.k4X);
|
||||
backboardEncoder.setSamplesToAverage(5);
|
||||
backboardEncoder.setMinRate(1.0);
|
||||
backboardEncoder.setDistancePerPulse(1);
|
||||
|
||||
// 初始化背板的PID控制器
|
||||
backboardPID = new ProfiledPIDController(Constants.Shooter.backboardPositionPID.kP, Constants.Shooter.backboardPositionPID.kI, Constants.Shooter.backboardPositionPID.kD,
|
||||
new Constraints(10000,100000));
|
||||
|
||||
backboardPID = new ProfiledPIDController(
|
||||
Constants.Shooter.backboardPositionPID.kP,
|
||||
Constants.Shooter.backboardPositionPID.kI,
|
||||
Constants.Shooter.backboardPositionPID.kD,
|
||||
new Constraints(10000, 100000)
|
||||
);
|
||||
backboardPID.setTolerance(10.0);
|
||||
|
||||
shooterNetworkTable = NetworkTableInstance.getDefault().getTable("Shooter");
|
||||
|
||||
backboardTuningTable = NetworkTableInstance.getDefault().getTable("Tuning/backboard");
|
||||
backboardTuningTable.getEntry("backboardPositionEnable").setDefaultBoolean(false);
|
||||
backboardTuningTable.getEntry("backboardTargetPosition").setDefaultDouble(0.0);
|
||||
}
|
||||
|
||||
public void setRightFollowLeft(){
|
||||
flywheelMotorRight.setControl(flyWheelFollower);
|
||||
/* ====================== */
|
||||
/* 飞轮 */
|
||||
/* ====================== */
|
||||
|
||||
public void setRightConveyorFollowLeft() {
|
||||
// 成对机构机械方向相反，因此右侧电机以反向 follower 跟随左侧电机。
|
||||
rightConveyor.setControl(conveyorFollower);
|
||||
}
|
||||
|
||||
public void applyLeftConfigurationToRight(){
|
||||
public void applyLeftConfigurationToRight() {
|
||||
Slot0Configs configs = new Slot0Configs();
|
||||
flywheelMotorLeft.getConfigurator().refresh(configs);
|
||||
flywheelMotorRight.getConfigurator().apply(configs);
|
||||
leftFlywheel.getConfigurator().refresh(configs);
|
||||
rightFlywheel.getConfigurator().apply(configs);
|
||||
}
|
||||
|
||||
|
||||
// 设置飞轮的速度
|
||||
public void setFlywheelSpeedByRPS(double targetSpeed) {
|
||||
flywheelMotorLeft.setControl(velocityRequest.withVelocity(targetSpeed)); //RPS 转每秒
|
||||
setRightFollowLeft();
|
||||
// System.out.println("set flywheel "+targetSpeed);
|
||||
public void setFlywheelVoltage(double outputVolts) {
|
||||
double clampedOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
|
||||
if (Math.abs(clampedOutputVolts - flywheelOutputVolts) > 1e-6) {
|
||||
flywheelVoltageStartTimestamp = Timer.getFPGATimestamp();
|
||||
}
|
||||
flywheelOutputVolts = clampedOutputVolts;
|
||||
leftFlywheel.setControl(leftFlywheelVoltageRequest.withOutput(flywheelOutputVolts));
|
||||
rightFlywheel.setControl(rightFlywheelVoltageRequest.withOutput(-flywheelOutputVolts));
|
||||
}
|
||||
|
||||
// 设置传动的速度
|
||||
public void setConveyorSpeedByRPS(double speed) {
|
||||
conveyorMotor.setControl(velocityRequest.withVelocity(speed)); // 设置传动电机速度
|
||||
}
|
||||
public void setConveyorSpeedOpen(double speed){
|
||||
conveyorMotor.set(speed);
|
||||
public void setFlywheelVoltageByDistance(double distance) {
|
||||
double targetVolts = Constants.Shooter.flywheelVoltageByDistance.get(distance);
|
||||
setFlywheelVoltage(targetVolts);
|
||||
}
|
||||
|
||||
//调试用
|
||||
public void setBackboardSpeedByRPS(double speed){
|
||||
if (speed > 0 && speed < 10) {
|
||||
speed = 10;
|
||||
} else if (speed < 0 && speed > -10) {
|
||||
speed = -10;
|
||||
}
|
||||
backboardMotor.setControl(velocityRequest.withVelocity(speed));
|
||||
public void stopFlywheel() {
|
||||
setFlywheelVoltage(0.0);
|
||||
}
|
||||
|
||||
public boolean atVoltage() {
|
||||
if (Math.abs(flywheelOutputVolts) < Constants.Shooter.flywheelReadyVoltageThreshold) {
|
||||
return false;
|
||||
}
|
||||
return Timer.getFPGATimestamp() - flywheelVoltageStartTimestamp
|
||||
>= Constants.Shooter.flywheelReadyDelaySeconds;
|
||||
}
|
||||
public double getLeftFlywheelVelocity() {
|
||||
return leftFlywheel.getVelocity().getValueAsDouble();
|
||||
}
|
||||
|
||||
public double getRightFlywheelVelocity() {
|
||||
return rightFlywheel.getVelocity().getValueAsDouble();
|
||||
}
|
||||
|
||||
public double getAverageFlywheelVelocity() {
|
||||
return (Math.abs(getLeftFlywheelVelocity()) + Math.abs(getRightFlywheelVelocity())) / 2.0;
|
||||
}
|
||||
|
||||
public double getFlywheelVelocityDifference() {
|
||||
return Math.abs(getLeftFlywheelVelocity()) - Math.abs(getRightFlywheelVelocity());
|
||||
}
|
||||
|
||||
public double getTargetFlywheelVoltage() {
|
||||
return flywheelOutputVolts;
|
||||
}
|
||||
|
||||
/* ====================== */
|
||||
/* shooter conveyor */
|
||||
/* ====================== */
|
||||
|
||||
public void runShooterConveyorVoltage(double outputVolts) {
|
||||
shooterConveyorOutputVolts = MathUtil.clamp(outputVolts, -12.0, 12.0);
|
||||
leftConveyor.setControl(conveyorVoltageRequest.withOutput(shooterConveyorOutputVolts));
|
||||
setRightConveyorFollowLeft();
|
||||
}
|
||||
|
||||
public void stopShooterConveyor() {
|
||||
runShooterConveyorVoltage(0.0);
|
||||
}
|
||||
|
||||
public double getConveyorStatorCurrent() {
|
||||
return leftConveyor.getStatorCurrent().getValueAsDouble();
|
||||
}
|
||||
|
||||
/* ====================== */
|
||||
/* 背板 */
|
||||
/* ====================== */
|
||||
|
||||
public void setBackboardVoltage(double outputVolts) {
|
||||
backboardOutputVolts = MathUtil.clamp(
|
||||
outputVolts,
|
||||
-Constants.Shooter.backboardSpeedMax,
|
||||
Constants.Shooter.backboardSpeedMax
|
||||
);
|
||||
backboardMotor.setControl(backboardVoltageRequest.withOutput(backboardOutputVolts));
|
||||
}
|
||||
|
||||
// 设置背板的目标位置，通过PID控制器来调整 范围0-3000
|
||||
public void setBackboardPosition(double targetPosition) {
|
||||
// 限制目标位置在上下限之间
|
||||
// 保留旧背板机构的软件限位。
|
||||
if (targetPosition > Constants.Shooter.backboardUpLimit) {
|
||||
targetPosition = Constants.Shooter.backboardUpLimit; // 限制最大上限
|
||||
targetPosition = Constants.Shooter.backboardUpLimit;
|
||||
} else if (targetPosition < Constants.Shooter.backboardDownLimit) {
|
||||
targetPosition = Constants.Shooter.backboardDownLimit; // 限制最小下限
|
||||
targetPosition = Constants.Shooter.backboardDownLimit;
|
||||
}
|
||||
backboardPID.setGoal(targetPosition);
|
||||
}
|
||||
public void outputBackboard(){
|
||||
|
||||
// 使用PID控制器来计算背板的运动速度
|
||||
double pidOutput = backboardPID.calculate(getBackboardPosition());
|
||||
pidOutput = Math.max(-Constants.Shooter.backboardSpeedMax, Math.min(Constants.Shooter.backboardSpeedMax, pidOutput)); // 限制输出范围
|
||||
public void outputBackboard() {
|
||||
// 背板仍使用旧外部编码器和 ProfiledPID；Superstructure 只选择目标位置。
|
||||
double rawPidOutput = backboardPID.calculate(getBackboardPosition());
|
||||
double pidOutput = MathUtil.clamp(
|
||||
rawPidOutput,
|
||||
-Constants.Shooter.backboardSpeedMax,
|
||||
Constants.Shooter.backboardSpeedMax
|
||||
);
|
||||
backboardRawPidOutputVolts = rawPidOutput;
|
||||
backboardClampedPidOutputVolts = pidOutput;
|
||||
backboardOutputSaturated = Math.abs(rawPidOutput - pidOutput) > 1e-9;
|
||||
|
||||
if (pidOutput != 0.0 && Math.abs(pidOutput) < Constants.Shooter.backboardSpeedMin) {
|
||||
pidOutput = Math.copySign(Constants.Shooter.backboardSpeedMin, pidOutput);
|
||||
backboardMinimumOutputActive = true;
|
||||
} else {
|
||||
backboardMinimumOutputActive = false;
|
||||
}
|
||||
|
||||
if (Math.abs(pidOutput) < 0.8) {
|
||||
pidOutput = 0.0;
|
||||
}//防止抖动
|
||||
// // 设置背板电机的速度，根据PID输出进行调整
|
||||
shooterNetworkTable.getEntry("backboardPIDOutput").setDouble(pidOutput);
|
||||
setBackboardSpeedByRPS(pidOutput);
|
||||
setBackboardVoltage(pidOutput);
|
||||
}
|
||||
// 停止所有电机
|
||||
public void stopMotors() {
|
||||
flywheelMotorLeft.set(0);
|
||||
flywheelMotorRight.set(0);
|
||||
conveyorMotor.set(0);
|
||||
backboardMotor.set(0);
|
||||
|
||||
public void holdBackboardAt(double targetPosition) {
|
||||
// 上层只给目标位置；到位后停止输出，未到位时继续闭环。
|
||||
setBackboardPosition(targetPosition);
|
||||
if (isBackboardAtTarget()) {
|
||||
backboardRawPidOutputVolts = 0.0;
|
||||
backboardClampedPidOutputVolts = 0.0;
|
||||
backboardOutputSaturated = false;
|
||||
backboardMinimumOutputActive = false;
|
||||
setBackboardVoltage(0.0);
|
||||
} else {
|
||||
outputBackboard();
|
||||
}
|
||||
}
|
||||
|
||||
public void stopBackboard() {
|
||||
backboardRawPidOutputVolts = 0.0;
|
||||
backboardClampedPidOutputVolts = 0.0;
|
||||
backboardOutputSaturated = false;
|
||||
backboardMinimumOutputActive = false;
|
||||
setBackboardVoltage(0.0);
|
||||
}
|
||||
|
||||
// 获取背板当前角度
|
||||
public double getBackboardPosition() {
|
||||
return backboardEncoder.getDistance(); // 返回背板电机编码器的当前角度
|
||||
return backboardEncoder.getDistance();
|
||||
}
|
||||
|
||||
// 检查背板是否到达目标角度
|
||||
public boolean isBackboardAtTarget() {
|
||||
return backboardPID.atGoal(); // 判断背板是否到达目标角度
|
||||
return backboardPID.atGoal();
|
||||
}
|
||||
public void conveyorWaitForAcceleration(){
|
||||
conveyorMotor.set(0);
|
||||
}
|
||||
public void conveyorRun(){
|
||||
setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed);
|
||||
}
|
||||
public void resetbackboardencoder(){
|
||||
|
||||
public void resetbackboardencoder() {
|
||||
backboardEncoder.reset();
|
||||
backboardPID.reset(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* 背板回零命令：闭环回到 0 位后停止输出。
|
||||
* 命令留在 subsystem 内，controls 层只负责绑定按键。
|
||||
*/
|
||||
public Command homeBackboardCommand() {
|
||||
return Commands.sequence(
|
||||
Commands.runOnce(() -> setBackboardPosition(0.0), this),
|
||||
Commands.run(() -> holdBackboardAt(0.0), this).until(this::isBackboardAtTarget),
|
||||
Commands.runOnce(this::stopBackboard, this)
|
||||
);
|
||||
}
|
||||
|
||||
/* ====================== */
|
||||
/* 整体 */
|
||||
/* ====================== */
|
||||
|
||||
public void stopAll() {
|
||||
stopFlywheel();
|
||||
stopShooterConveyor();
|
||||
stopBackboard();
|
||||
}
|
||||
|
||||
public void debugBackboardTargetPeriodic() {
|
||||
boolean positionEnable = backboardTuningTable.getEntry("backboardPositionEnable").getBoolean(false);
|
||||
double requestedTarget = backboardTuningTable.getEntry("backboardTargetPosition").getDouble(0.0);
|
||||
double clampedTarget = MathUtil.clamp(
|
||||
requestedTarget,
|
||||
Constants.Shooter.backboardDownLimit,
|
||||
Constants.Shooter.backboardUpLimit
|
||||
);
|
||||
|
||||
backboardTuningTable.getEntry("backboardPositionEnableActive").setBoolean(positionEnable);
|
||||
backboardTuningTable.getEntry("backboardCommandedTargetPosition").setDouble(requestedTarget);
|
||||
backboardTuningTable.getEntry("backboardClampedTargetPosition").setDouble(clampedTarget);
|
||||
|
||||
if (positionEnable) {
|
||||
holdBackboardAt(clampedTarget);
|
||||
}
|
||||
public double getConveyorStatorCurrent(){
|
||||
double statorCurrent = conveyorMotor.getStatorCurrent().getValueAsDouble();
|
||||
return statorCurrent;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
backboardPID.calculate(getBackboardPosition());
|
||||
shooterNetworkTable.getEntry("flywheelSpeed").setDouble(flywheelMotorLeft.getVelocity().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("flywheelPIDReference").setDouble(flywheelMotorLeft.getClosedLoopReference().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("conveyorSpeed").setDouble(conveyorMotor.getVelocity().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("conveyorPIDReference").setDouble(conveyorMotor.getClosedLoopReference().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition());
|
||||
shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position);
|
||||
double backboardPosition = getBackboardPosition();
|
||||
double backboardGoalPosition = backboardPID.getGoal().position;
|
||||
double backboardSetpointPosition = backboardPID.getSetpoint().position;
|
||||
shooterNetworkTable.getEntry("flywheelVelocity").setDouble(getAverageFlywheelVelocity());
|
||||
shooterNetworkTable.getEntry("leftFlywheelVelocity").setDouble(getLeftFlywheelVelocity());
|
||||
shooterNetworkTable.getEntry("rightFlywheelVelocity").setDouble(getRightFlywheelVelocity());
|
||||
shooterNetworkTable.getEntry("flywheelVelocityDifference").setDouble(getFlywheelVelocityDifference());
|
||||
shooterNetworkTable.getEntry("flywheelTargetVoltage").setDouble(getTargetFlywheelVoltage());
|
||||
shooterNetworkTable.getEntry("flywheelOutputVolts").setDouble(flywheelOutputVolts);
|
||||
shooterNetworkTable.getEntry("flywheelAtVoltage").setBoolean(atVoltage());
|
||||
shooterNetworkTable.getEntry("shooterConveyorOutputVolts").setDouble(shooterConveyorOutputVolts);
|
||||
shooterNetworkTable.getEntry("conveyorVelocity").setDouble(leftConveyor.getVelocity().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(backboardPosition);
|
||||
shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardSetpointPosition);
|
||||
shooterNetworkTable.getEntry("backboardOutputVolts").setDouble(backboardOutputVolts);
|
||||
shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget());
|
||||
shooterNetworkTable.getEntry("distanceGetted").setDouble(getDistanceToHub());
|
||||
shooterNetworkTable.getEntry("distancetopassball").setDouble(getDistanceToPassball());
|
||||
shooterNetworkTable.getEntry("backboardGoalPosition").setDouble(backboardGoalPosition);
|
||||
shooterNetworkTable.getEntry("backboardSetpointPosition").setDouble(backboardSetpointPosition);
|
||||
shooterNetworkTable.getEntry("backboardSetpointVelocity").setDouble(backboardPID.getSetpoint().velocity);
|
||||
shooterNetworkTable.getEntry("backboardGoalError").setDouble(backboardGoalPosition - backboardPosition);
|
||||
shooterNetworkTable.getEntry("backboardSetpointError").setDouble(backboardSetpointPosition - backboardPosition);
|
||||
shooterNetworkTable.getEntry("backboardRawPIDOutput").setDouble(backboardRawPidOutputVolts);
|
||||
shooterNetworkTable.getEntry("backboardClampedPIDOutput").setDouble(backboardClampedPidOutputVolts);
|
||||
shooterNetworkTable.getEntry("backboardOutputSaturated").setBoolean(backboardOutputSaturated);
|
||||
shooterNetworkTable.getEntry("backboardMinimumOutputActive").setBoolean(backboardMinimumOutputActive);
|
||||
shooterNetworkTable.getEntry("backboardMaxOutputVolts").setDouble(Constants.Shooter.backboardSpeedMax);
|
||||
shooterNetworkTable.getEntry("backboardMinOutputVolts").setDouble(Constants.Shooter.backboardSpeedMin);
|
||||
shooterNetworkTable.getEntry("backboardPositionTolerance").setDouble(10.0);
|
||||
shooterNetworkTable.getEntry("backboardShootPosition").setDouble(Constants.Superstructure.backboardShootPosition);
|
||||
shooterNetworkTable.getEntry("backboardPassPosition").setDouble(Constants.Superstructure.passBallBackboardPosition);
|
||||
shooterNetworkTable.getEntry("backboardMotorPosition").setDouble(backboardMotor.getPosition().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("backboardMotorVelocity").setDouble(backboardMotor.getVelocity().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("backboardMotorVoltage").setDouble(backboardMotor.getMotorVoltage().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("backboardMotorStatorCurrent").setDouble(backboardMotor.getStatorCurrent().getValueAsDouble());
|
||||
shooterNetworkTable.getEntry("statorCurrent").setDouble(getConveyorStatorCurrent());
|
||||
}
|
||||
|
||||
public double getDistanceToHub(){
|
||||
//获得车子距离Hub的距离
|
||||
return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0);
|
||||
}
|
||||
public double getDistanceToPassball(){
|
||||
return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToPassball").getDouble(0.0);
|
||||
}
|
||||
public void stopAll() {
|
||||
setFlywheelSpeedByRPS(0);
|
||||
setConveyorSpeedByRPS(0);
|
||||
backboardMotor.set(0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,356 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.Constants;
|
||||
|
||||
/**
|
||||
* 统一协调所有得分相关机构。
|
||||
*
|
||||
* subsystem 只暴露硬件级动作，OperatorControls 只表达操作员意图；
|
||||
* shooter、intake、conveyor、底盘瞄准和 Limelight 距离如何配合，
|
||||
* 统一由这里决定。底盘瞄准本身仍由 DriveControls 的 AutoAimming
|
||||
* 模式完成，这里只读取 drive.isAimed() 等状态，不直接控制底盘电机。
|
||||
*/
|
||||
public class Superstructure extends SubsystemBase {
|
||||
// 操作员或自动程序请求的目标状态。
|
||||
public enum WantedState {
|
||||
IDLE,
|
||||
INTAKE,
|
||||
SHOOT_HUB,
|
||||
AIM_HUB,
|
||||
PASS_BALL,
|
||||
EJECT,
|
||||
MANUAL
|
||||
}
|
||||
|
||||
// 机器人内部实际执行状态，用于动作排序和仪表盘诊断。
|
||||
public enum SystemState {
|
||||
DISABLED,
|
||||
STOWED,
|
||||
INTAKING,
|
||||
PREP_SHOOT,
|
||||
SHOOTING,
|
||||
EJECTING,
|
||||
MANUAL_OVERRIDE
|
||||
}
|
||||
|
||||
private final ShooterSubsystem shooter;
|
||||
private final IntakeRollerSubsystem intakeRoller;
|
||||
private final IntakeRotaterSubsystem intakeRotater;
|
||||
private final ConveyorSubsystem conveyor;
|
||||
private final CommandSwerveDrivetrain drive;
|
||||
private final LimelightSubsystem limelight;
|
||||
private final NetworkTable table = NetworkTableInstance.getDefault().getTable("Superstructure");
|
||||
|
||||
private WantedState wantedState = WantedState.IDLE;
|
||||
private SystemState systemState = SystemState.STOWED;
|
||||
private double stateStartTimestamp = Timer.getFPGATimestamp();
|
||||
private double flywheelVoltageOffset = 0.0;
|
||||
private double backboardPositionOffset = 0.0;
|
||||
private boolean shotCompletedThisRequest = false;
|
||||
|
||||
public Superstructure(
|
||||
ShooterSubsystem shooter,
|
||||
IntakeRollerSubsystem intakeRoller,
|
||||
IntakeRotaterSubsystem intakeRotater,
|
||||
ConveyorSubsystem conveyor,
|
||||
CommandSwerveDrivetrain drive,
|
||||
LimelightSubsystem limelight
|
||||
) {
|
||||
this.shooter = shooter;
|
||||
this.intakeRoller = intakeRoller;
|
||||
this.intakeRotater = intakeRotater;
|
||||
this.conveyor = conveyor;
|
||||
this.drive = drive;
|
||||
this.limelight = limelight;
|
||||
}
|
||||
|
||||
/* ====================== */
|
||||
/* 操作员意图入口 */
|
||||
/* ====================== */
|
||||
|
||||
public void requestIdle() {
|
||||
wantedState = WantedState.IDLE;
|
||||
}
|
||||
|
||||
public void requestIntake() {
|
||||
wantedState = WantedState.INTAKE;
|
||||
}
|
||||
|
||||
public void requestShootHub() {
|
||||
if (wantedState != WantedState.SHOOT_HUB) {
|
||||
shotCompletedThisRequest = false;
|
||||
}
|
||||
wantedState = WantedState.SHOOT_HUB;
|
||||
}
|
||||
|
||||
public void requestAimHub() {
|
||||
wantedState = WantedState.AIM_HUB;
|
||||
}
|
||||
|
||||
public void requestPassBall() {
|
||||
if (wantedState != WantedState.PASS_BALL) {
|
||||
shotCompletedThisRequest = false;
|
||||
}
|
||||
wantedState = WantedState.PASS_BALL;
|
||||
}
|
||||
|
||||
public void requestShoot() {
|
||||
requestShootHub();
|
||||
}
|
||||
|
||||
public void requestEject() {
|
||||
wantedState = WantedState.EJECT;
|
||||
}
|
||||
|
||||
public void requestManual() {
|
||||
wantedState = WantedState.MANUAL;
|
||||
}
|
||||
|
||||
public Command requestIdleCommand() {
|
||||
return Commands.runOnce(this::requestIdle, this);
|
||||
}
|
||||
|
||||
public Command requestIntakeCommand() {
|
||||
return Commands.runOnce(this::requestIntake, this);
|
||||
}
|
||||
|
||||
public Command requestShootHubCommand() {
|
||||
return Commands.runOnce(this::requestShootHub, this);
|
||||
}
|
||||
|
||||
public Command requestAimHubCommand() {
|
||||
return Commands.runOnce(this::requestAimHub, this);
|
||||
}
|
||||
|
||||
public Command requestPassBallCommand() {
|
||||
return Commands.runOnce(this::requestPassBall, this);
|
||||
}
|
||||
|
||||
public Command requestShootCommand() {
|
||||
return requestShootHubCommand();
|
||||
}
|
||||
|
||||
public Command requestEjectCommand() {
|
||||
return Commands.runOnce(this::requestEject, this);
|
||||
}
|
||||
|
||||
public Command requestManualCommand() {
|
||||
return Commands.runOnce(this::requestManual, this);
|
||||
}
|
||||
|
||||
public WantedState getWantedState() {
|
||||
return wantedState;
|
||||
}
|
||||
|
||||
public SystemState getSystemState() {
|
||||
return systemState;
|
||||
}
|
||||
|
||||
public boolean isShootCycleComplete() {
|
||||
return shotCompletedThisRequest;
|
||||
}
|
||||
|
||||
/* ====================== */
|
||||
/* 射击门控 */
|
||||
/* ====================== */
|
||||
|
||||
public boolean canShoot() {
|
||||
boolean stationaryGate = !Constants.Superstructure.stationaryGateEnabled || isDriveStationary();
|
||||
return shooter.atVoltage()
|
||||
&& drive.isAimed()
|
||||
&& stationaryGate;
|
||||
}
|
||||
|
||||
private boolean isDriveStationary() {
|
||||
ChassisSpeeds speeds = drive.getChassisSpeeds();
|
||||
double translationSpeed = Math.hypot(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond);
|
||||
return translationSpeed <= Constants.Superstructure.stationarySpeedToleranceMetersPerSecond;
|
||||
}
|
||||
|
||||
/* ====================== */
|
||||
/* 现场微调 offset */
|
||||
/* ====================== */
|
||||
|
||||
public void adjustFlywheelVoltageOffset(double offsetVolts) {
|
||||
flywheelVoltageOffset += offsetVolts;
|
||||
}
|
||||
|
||||
public void adjustBackboardPositionOffset(double offset) {
|
||||
backboardPositionOffset += offset;
|
||||
}
|
||||
|
||||
public void resetOffsets() {
|
||||
flywheelVoltageOffset = 0.0;
|
||||
backboardPositionOffset = 0.0;
|
||||
}
|
||||
|
||||
public void resetBackboardEncoder() {
|
||||
shooter.resetbackboardencoder();
|
||||
}
|
||||
|
||||
/* ====================== */
|
||||
/* 状态机主体 */
|
||||
/* ====================== */
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
// 禁用状态优先级最高，防止旧按钮输入或自动请求在禁用后继续驱动机构。
|
||||
if (DriverStation.isDisabled()) {
|
||||
setSystemState(SystemState.DISABLED);
|
||||
stopAllMechanisms();
|
||||
publishTelemetry();
|
||||
return;
|
||||
}
|
||||
|
||||
switch (wantedState) {
|
||||
case IDLE -> handleIdle();
|
||||
case INTAKE -> handleIntake();
|
||||
case SHOOT_HUB -> handleShootHub();
|
||||
case AIM_HUB -> handleAimHub();
|
||||
case PASS_BALL -> handlePassBall();
|
||||
case EJECT -> handleEject();
|
||||
case MANUAL -> handleManual();
|
||||
}
|
||||
|
||||
publishTelemetry();
|
||||
}
|
||||
|
||||
private void handleIdle() {
|
||||
// 安全默认状态：球路不动，intaker 不主动动作，背板保持受控。
|
||||
conveyor.stop();
|
||||
intakeRoller.stop();
|
||||
intakeRotater.stop();
|
||||
shooter.stopFlywheel();
|
||||
shooter.stopShooterConveyor();
|
||||
shooter.holdBackboardAt(0.0);
|
||||
setSystemState(SystemState.STOWED);
|
||||
}
|
||||
|
||||
private void handleIntake() {
|
||||
shooter.stopFlywheel();
|
||||
shooter.stopShooterConveyor();
|
||||
shooter.holdBackboardAt(0.0);
|
||||
// Intaker 比赛开始后保持下放，收球何时结束由操作员松开按键决定。
|
||||
intakeRoller.intake();
|
||||
conveyor.feedToShooter();
|
||||
setSystemState(SystemState.INTAKING);
|
||||
}
|
||||
|
||||
private void handleAimHub() {
|
||||
intakeRoller.stop();
|
||||
conveyor.stop();
|
||||
shooter.stopFlywheel();
|
||||
shooter.stopShooterConveyor();
|
||||
shooter.holdBackboardAt(0.0);
|
||||
setSystemState(SystemState.STOWED);
|
||||
}
|
||||
|
||||
private void handleShootHub() {
|
||||
double targetVolts = limelight.getShooterVoltageByDistance() + flywheelVoltageOffset;
|
||||
double backboardPosition = Constants.Superstructure.backboardShootPosition + backboardPositionOffset;
|
||||
handlePreparedFeed(targetVolts, backboardPosition);
|
||||
}
|
||||
|
||||
private void handlePassBall() {
|
||||
double targetVolts = Constants.Superstructure.passBallFlywheelVoltage + flywheelVoltageOffset;
|
||||
double backboardPosition = Constants.Superstructure.passBallBackboardPosition + backboardPositionOffset;
|
||||
handlePreparedFeed(targetVolts, backboardPosition);
|
||||
}
|
||||
|
||||
private void handlePreparedFeed(double targetVolts, double backboardPosition) {
|
||||
if (systemState != SystemState.PREP_SHOOT
|
||||
&& systemState != SystemState.SHOOTING) {
|
||||
setSystemState(SystemState.PREP_SHOOT);
|
||||
}
|
||||
intakeRoller.stop();
|
||||
conveyor.stop();
|
||||
shooter.stopShooterConveyor();
|
||||
// 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调，
|
||||
// 但不会绕过 Superstructure 状态机。
|
||||
shooter.setFlywheelVoltage(targetVolts);
|
||||
shooter.holdBackboardAt(backboardPosition);
|
||||
|
||||
if (systemState == SystemState.SHOOTING) {
|
||||
// 只有 canShoot() 通过后才会进入本状态；喂球期间不再复查门控，
|
||||
// 避免球接触飞轮导致的掉速中断喂球。
|
||||
intakeRotater.shootAssist();
|
||||
conveyor.feedToShooter();
|
||||
shooter.runShooterConveyorVoltage(Constants.Superstructure.shooterFeedVoltage);
|
||||
|
||||
if (timeInState() > Constants.Superstructure.shootTimeoutSeconds) {
|
||||
shotCompletedThisRequest = true;
|
||||
setSystemState(SystemState.PREP_SHOOT);
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// 未开始喂球前，球路保持静止。
|
||||
conveyor.stop();
|
||||
shooter.stopShooterConveyor();
|
||||
|
||||
// 门控条件实时刷新：条件回落时状态同步回退，仪表盘能看到卡在哪一关。
|
||||
if (canShoot()) {
|
||||
setSystemState(SystemState.SHOOTING);
|
||||
}
|
||||
}
|
||||
|
||||
private void handleEject() {
|
||||
// Eject 反转整条球路，飞轮保持停止。
|
||||
setSystemState(SystemState.EJECTING);
|
||||
intakeRoller.outtake();
|
||||
conveyor.reverse();
|
||||
shooter.runShooterConveyorVoltage(Constants.Superstructure.shooterReverseVoltage);
|
||||
shooter.stopFlywheel();
|
||||
shooter.holdBackboardAt(0.0);
|
||||
}
|
||||
|
||||
private void handleManual() {
|
||||
// 手动模式：状态机不再驱动机构，交给现场手动/调参工具；
|
||||
// 只保持飞轮与喂球停止，避免意外射出。
|
||||
setSystemState(SystemState.MANUAL_OVERRIDE);
|
||||
shooter.stopFlywheel();
|
||||
shooter.stopShooterConveyor();
|
||||
conveyor.stop();
|
||||
intakeRoller.stop();
|
||||
}
|
||||
|
||||
private void stopAllMechanisms() {
|
||||
shooter.stopAll();
|
||||
intakeRoller.stop();
|
||||
intakeRotater.stop();
|
||||
conveyor.stop();
|
||||
}
|
||||
|
||||
private void setSystemState(SystemState nextState) {
|
||||
if (systemState != nextState) {
|
||||
systemState = nextState;
|
||||
stateStartTimestamp = Timer.getFPGATimestamp();
|
||||
}
|
||||
}
|
||||
|
||||
private double timeInState() {
|
||||
return Timer.getFPGATimestamp() - stateStartTimestamp;
|
||||
}
|
||||
|
||||
private void publishTelemetry() {
|
||||
// 发布状态和各项门控信号，方便判断机器人为什么还没进入射击。
|
||||
table.getEntry("wantedState").setString(wantedState.name());
|
||||
table.getEntry("systemState").setString(systemState.name());
|
||||
table.getEntry("timeInState").setDouble(timeInState());
|
||||
table.getEntry("canShoot").setBoolean(canShoot());
|
||||
table.getEntry("shooterAtVoltage").setBoolean(shooter.atVoltage());
|
||||
table.getEntry("driveAimed").setBoolean(drive.isAimed());
|
||||
table.getEntry("driveHeadingError").setDouble(drive.getHeadingError());
|
||||
table.getEntry("driveStationary").setBoolean(isDriveStationary());
|
||||
table.getEntry("flywheelVoltageOffset").setDouble(flywheelVoltageOffset);
|
||||
table.getEntry("backboardPositionOffset").setDouble(backboardPositionOffset);
|
||||
}
|
||||
}
|
||||
@@ -2,8 +2,9 @@ package frc.robot.tuning;
|
||||
import java.util.function.Function;
|
||||
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.controls.VelocityDutyCycle;
|
||||
import com.ctre.phoenix6.controls.VoltageOut;
|
||||
import com.ctre.phoenix6.hardware.TalonFX;
|
||||
import edu.wpi.first.math.MathUtil;
|
||||
import edu.wpi.first.networktables.*;
|
||||
|
||||
public class ConfigTalonFXMotorTuner {
|
||||
@@ -22,10 +23,10 @@ public class ConfigTalonFXMotorTuner {
|
||||
private final DoubleSubscriber KsSub;
|
||||
|
||||
private final BooleanSubscriber enableRuningSub;
|
||||
private final DoubleSubscriber runningSpeedRPS;
|
||||
private final DoubleSubscriber runningVolts;
|
||||
private double lastKp = 0, lastKi = 0, lastKd = 0, lastKv = 0, lastKa = 0, lastKs = 0;
|
||||
|
||||
private final VelocityDutyCycle velocityRequest = new VelocityDutyCycle(0);
|
||||
private final VoltageOut voltageRequest = new VoltageOut(0);
|
||||
public ConfigTalonFXMotorTuner(TalonFX motor_, String motorName_, Slot0Configs motorSlot0Configs_) {
|
||||
this.motor = motor_;
|
||||
this.motorName = motorName_;
|
||||
@@ -42,7 +43,7 @@ public class ConfigTalonFXMotorTuner {
|
||||
table.getDoubleTopic(motorName+"kS").publish().set(motorSlot0Configs.kS);
|
||||
|
||||
table.getBooleanTopic(motorName+"EnableRunning").publish().set(false);;
|
||||
table.getDoubleTopic(motorName+"RunningSpeedRPS").publish().set(0);;
|
||||
table.getDoubleTopic(motorName+"RunningVolts").publish().set(0);;
|
||||
|
||||
this.enableSub = table.getBooleanTopic(motorName+"Enable").subscribe(false);
|
||||
this.KpSub = table.getDoubleTopic(motorName+"kP").subscribe(motorSlot0Configs.kP);
|
||||
@@ -52,7 +53,7 @@ public class ConfigTalonFXMotorTuner {
|
||||
this.KaSub = table.getDoubleTopic(motorName+"kA").subscribe(motorSlot0Configs.kA);
|
||||
this.KsSub = table.getDoubleTopic(motorName+"kS").subscribe(motorSlot0Configs.kS);
|
||||
this.enableRuningSub = table.getBooleanTopic(motorName+"EnableRunning").subscribe(false);
|
||||
this.runningSpeedRPS = table.getDoubleTopic(motorName+"RunningSpeedRPS").subscribe(0);
|
||||
this.runningVolts = table.getDoubleTopic(motorName+"RunningVolts").subscribe(0);
|
||||
}
|
||||
|
||||
public void periodic(Runnable func) {
|
||||
@@ -80,11 +81,11 @@ public class ConfigTalonFXMotorTuner {
|
||||
}
|
||||
|
||||
if (enableRuningSub.get()) {;
|
||||
double speed = runningSpeedRPS.get();
|
||||
motor.setControl(velocityRequest.withVelocity(speed));
|
||||
// System.out.println("running motor with speed: "+speed+" on motor: "+motorName);
|
||||
double volts = MathUtil.clamp(runningVolts.get(), -12.0, 12.0);
|
||||
motor.setControl(voltageRequest.withOutput(volts));
|
||||
// System.out.println("running motor with volts: "+volts+" on motor: "+motorName);
|
||||
}else{
|
||||
motor.setControl(velocityRequest.withVelocity(0));
|
||||
motor.setControl(voltageRequest.withOutput(0));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
package frc.robot.tuning;
|
||||
import com.ctre.phoenix6.configs.Slot0Configs;
|
||||
import com.ctre.phoenix6.controls.VelocityDutyCycle;
|
||||
import com.ctre.phoenix6.controls.VelocityVoltage;
|
||||
import com.ctre.phoenix6.hardware.TalonFXS;
|
||||
|
||||
import edu.wpi.first.networktables.*;
|
||||
|
||||
@@ -0,0 +1,117 @@
|
||||
package frc.robot.utils;
|
||||
|
||||
import java.util.Optional;
|
||||
|
||||
import edu.wpi.first.networktables.NetworkTable;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
import edu.wpi.first.wpilibj.DriverStation.Alliance;
|
||||
|
||||
public class GameData2026 {
|
||||
private static final double SHIFT_1_END_TIME = 105.0;
|
||||
private static final double SHIFT_2_END_TIME = 80.0;
|
||||
private static final double SHIFT_3_END_TIME = 55.0;
|
||||
private static final double SHIFT_4_END_TIME = 30.0;
|
||||
private static final double TRANSITION_END_TIME = 130.0;
|
||||
|
||||
private final NetworkTable table = NetworkTableInstance.getDefault().getTable("GameData2026");
|
||||
|
||||
public void periodic() {
|
||||
Optional<Alliance> alliance = DriverStation.getAlliance();
|
||||
double matchTime = DriverStation.getMatchTime();
|
||||
String gameData = DriverStation.getGameSpecificMessage();
|
||||
|
||||
HubWindow window = calculateHubWindow(alliance, matchTime, gameData);
|
||||
|
||||
table.getEntry("HubActive").setBoolean(window.hubActive);
|
||||
table.getEntry("HubStatus").setString(window.status);
|
||||
table.getEntry("SecondsUntilHubOpens").setDouble(window.secondsUntilHubOpens);
|
||||
table.getEntry("SecondsUntilHubChanges").setDouble(window.secondsUntilHubChanges);
|
||||
table.getEntry("Shift").setString(window.shift);
|
||||
table.getEntry("MatchTime").setDouble(matchTime);
|
||||
table.getEntry("GameData").setString(gameData);
|
||||
table.getEntry("Alliance").setString(alliance.map(Enum::name).orElse("Unknown"));
|
||||
table.getEntry("FirstInactiveAlliance").setString(getFirstInactiveAlliance(gameData));
|
||||
}
|
||||
|
||||
private HubWindow calculateHubWindow(Optional<Alliance> alliance, double matchTime, String gameData) {
|
||||
if (alliance.isEmpty()) {
|
||||
return new HubWindow(false, "NO_ALLIANCE", "Disabled", Double.NaN, Double.NaN);
|
||||
}
|
||||
|
||||
if (DriverStation.isAutonomousEnabled()) {
|
||||
return new HubWindow(true, "OPEN", "Auto", 0.0, Double.NaN);
|
||||
}
|
||||
|
||||
if (!DriverStation.isTeleopEnabled()) {
|
||||
return new HubWindow(false, "DISABLED", "Disabled", Double.NaN, Double.NaN);
|
||||
}
|
||||
|
||||
if (matchTime < 0.0) {
|
||||
return new HubWindow(true, "OPEN_TIME_UNKNOWN", "Unknown", 0.0, Double.NaN);
|
||||
}
|
||||
|
||||
Boolean redInactiveFirst = getRedInactiveFirst(gameData);
|
||||
if (redInactiveFirst == null) {
|
||||
return new HubWindow(true, "WAITING_DATA", "WaitingData", 0.0, Double.NaN);
|
||||
}
|
||||
|
||||
boolean shift1Active = switch (alliance.get()) {
|
||||
case Red -> !redInactiveFirst;
|
||||
case Blue -> redInactiveFirst;
|
||||
};
|
||||
|
||||
if (matchTime > TRANSITION_END_TIME) {
|
||||
return new HubWindow(true, "OPEN", "Transition", 0.0, matchTime - TRANSITION_END_TIME);
|
||||
} else if (matchTime > SHIFT_1_END_TIME) {
|
||||
return fromShift("Shift1", shift1Active, matchTime, SHIFT_1_END_TIME);
|
||||
} else if (matchTime > SHIFT_2_END_TIME) {
|
||||
return fromShift("Shift2", !shift1Active, matchTime, SHIFT_2_END_TIME);
|
||||
} else if (matchTime > SHIFT_3_END_TIME) {
|
||||
return fromShift("Shift3", shift1Active, matchTime, SHIFT_3_END_TIME);
|
||||
} else if (matchTime > SHIFT_4_END_TIME) {
|
||||
return fromShift("Shift4", !shift1Active, matchTime, SHIFT_4_END_TIME);
|
||||
} else {
|
||||
return new HubWindow(true, "OPEN", "Endgame", 0.0, Math.max(matchTime, 0.0));
|
||||
}
|
||||
}
|
||||
|
||||
private HubWindow fromShift(String shift, boolean hubActive, double matchTime, double nextBoundary) {
|
||||
double secondsUntilChange = Math.max(matchTime - nextBoundary, 0.0);
|
||||
double secondsUntilOpen = hubActive ? 0.0 : secondsUntilChange;
|
||||
String status = hubActive ? "OPEN" : "CLOSED";
|
||||
return new HubWindow(hubActive, status, shift, secondsUntilOpen, secondsUntilChange);
|
||||
}
|
||||
|
||||
private Boolean getRedInactiveFirst(String gameData) {
|
||||
if (gameData.isEmpty()) {
|
||||
return null;
|
||||
}
|
||||
|
||||
return switch (gameData.charAt(0)) {
|
||||
case 'R' -> true;
|
||||
case 'B' -> false;
|
||||
default -> null;
|
||||
};
|
||||
}
|
||||
|
||||
private String getFirstInactiveAlliance(String gameData) {
|
||||
if (gameData.isEmpty()) {
|
||||
return "None";
|
||||
}
|
||||
|
||||
return switch (gameData.charAt(0)) {
|
||||
case 'R' -> "Red";
|
||||
case 'B' -> "Blue";
|
||||
default -> "Invalid";
|
||||
};
|
||||
}
|
||||
|
||||
private record HubWindow(
|
||||
boolean hubActive,
|
||||
String status,
|
||||
String shift,
|
||||
double secondsUntilHubOpens,
|
||||
double secondsUntilHubChanges
|
||||
) {}
|
||||
}
|
||||
Reference in New Issue
Block a user