diff --git a/src/main/deploy/pathplanner/autos/8 Balls Middle Start Blue.auto b/src/main/deploy/pathplanner/autos/8 Balls Middle Start Blue.auto index 9e033bf..b686ed8 100644 --- a/src/main/deploy/pathplanner/autos/8 Balls Middle Start Blue.auto +++ b/src/main/deploy/pathplanner/autos/8 Balls Middle Start Blue.auto @@ -13,13 +13,13 @@ { "type": "named", "data": { - "name": "shoot" + "name": "shoot_hub" } }, { "type": "named", "data": { - "name": "shoot" + "name": "shoot_hub" } } ] diff --git a/src/main/deploy/pathplanner/autos/8 Balls Middle Start Red.auto b/src/main/deploy/pathplanner/autos/8 Balls Middle Start Red.auto index b945973..f3da4f1 100644 --- a/src/main/deploy/pathplanner/autos/8 Balls Middle Start Red.auto +++ b/src/main/deploy/pathplanner/autos/8 Balls Middle Start Red.auto @@ -13,7 +13,7 @@ { "type": "named", "data": { - "name": "shoot" + "name": "shoot_hub" } } ] diff --git a/src/main/deploy/pathplanner/autos/Copy of Shoot and Depot Blue.auto b/src/main/deploy/pathplanner/autos/Copy of Shoot and Depot Blue.auto index 378069d..91521ab 100644 --- a/src/main/deploy/pathplanner/autos/Copy of Shoot and Depot Blue.auto +++ b/src/main/deploy/pathplanner/autos/Copy of Shoot and Depot Blue.auto @@ -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" } }, { diff --git a/src/main/deploy/pathplanner/autos/Depot Auto Blue.auto b/src/main/deploy/pathplanner/autos/Depot Auto Blue.auto index 306d117..22e7b17 100644 --- a/src/main/deploy/pathplanner/autos/Depot Auto Blue.auto +++ b/src/main/deploy/pathplanner/autos/Depot Auto Blue.auto @@ -76,7 +76,7 @@ { "type": "named", "data": { - "name": "shoot" + "name": "shoot_hub" } } ] diff --git a/src/main/deploy/pathplanner/autos/Depot Auto Red.auto b/src/main/deploy/pathplanner/autos/Depot Auto Red.auto index 4285f0c..aafc0cf 100644 --- a/src/main/deploy/pathplanner/autos/Depot Auto Red.auto +++ b/src/main/deploy/pathplanner/autos/Depot Auto Red.auto @@ -76,7 +76,7 @@ { "type": "named", "data": { - "name": "shoot" + "name": "shoot_hub" } } ] diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto index 2bfb51c..597c328 100644 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -19,7 +19,7 @@ { "type": "named", "data": { - "name": "AutoShoot" + "name": "shoot_hub" } } ] diff --git a/src/main/deploy/pathplanner/autos/Shoot and Climb Blue.auto b/src/main/deploy/pathplanner/autos/Shoot and Climb Blue.auto index e6f9ac9..14d83d0 100644 --- a/src/main/deploy/pathplanner/autos/Shoot and Climb Blue.auto +++ b/src/main/deploy/pathplanner/autos/Shoot and Climb Blue.auto @@ -13,7 +13,7 @@ { "type": "named", "data": { - "name": "shoot" + "name": "shoot_hub" } }, { diff --git a/src/main/deploy/pathplanner/autos/Shoot and Climb Red.auto b/src/main/deploy/pathplanner/autos/Shoot and Climb Red.auto index 79862b3..4a85eed 100644 --- a/src/main/deploy/pathplanner/autos/Shoot and Climb Red.auto +++ b/src/main/deploy/pathplanner/autos/Shoot and Climb Red.auto @@ -26,7 +26,7 @@ { "type": "named", "data": { - "name": "shoot" + "name": "shoot_hub" } }, { diff --git a/src/main/deploy/pathplanner/autos/Shoot and Depot Blue.auto b/src/main/deploy/pathplanner/autos/Shoot and Depot Blue.auto index a561867..45c6376 100644 --- a/src/main/deploy/pathplanner/autos/Shoot and Depot Blue.auto +++ b/src/main/deploy/pathplanner/autos/Shoot and Depot Blue.auto @@ -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" } }, { diff --git a/src/main/deploy/pathplanner/autos/Shoot and Depot Red.auto b/src/main/deploy/pathplanner/autos/Shoot and Depot Red.auto index a176855..7405a68 100644 --- a/src/main/deploy/pathplanner/autos/Shoot and Depot Red.auto +++ b/src/main/deploy/pathplanner/autos/Shoot and Depot Red.auto @@ -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" } }, { diff --git a/src/main/deploy/pathplanner/autos/Shoot and Depot Without Climb Red.auto b/src/main/deploy/pathplanner/autos/Shoot and Depot Without Climb Red.auto index f8fafd7..b305fc7 100644 --- a/src/main/deploy/pathplanner/autos/Shoot and Depot Without Climb Red.auto +++ b/src/main/deploy/pathplanner/autos/Shoot and Depot Without Climb Red.auto @@ -32,7 +32,7 @@ { "type": "named", "data": { - "name": "shoot" + "name": "shoot_hub" } }, { @@ -93,7 +93,7 @@ { "type": "named", "data": { - "name": "AutoShoot" + "name": "shoot_hub" } }, { diff --git a/src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Blue.auto b/src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Blue.auto index a8f54e1..9dfbc69 100644 --- a/src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Blue.auto +++ b/src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Blue.auto @@ -32,7 +32,7 @@ { "type": "named", "data": { - "name": "shoot" + "name": "shoot_hub" } }, { @@ -93,7 +93,7 @@ { "type": "named", "data": { - "name": "AutoShoot" + "name": "shoot_hub" } }, { diff --git a/src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Red.auto b/src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Red.auto index 1297783..b1df431 100644 --- a/src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Red.auto +++ b/src/main/deploy/pathplanner/autos/Shoot and Depot and Climb Red.auto @@ -32,7 +32,7 @@ { "type": "named", "data": { - "name": "shoot" + "name": "shoot_hub" } }, { @@ -93,7 +93,7 @@ { "type": "named", "data": { - "name": "AutoShoot" + "name": "shoot_hub" } }, { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 507b813..d2acd0e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -10,15 +10,6 @@ public final class Constants { public static final boolean LEDUsing = false; - public enum RobotStatus { - Stopped, - AllTelop, - PassingBall, - AutoAimming, - CrossingBump, - CrossingTrench - } - public static class Shooter { // 机构 CAN ID 目前是占位值；季后赛最终接线确认后，只需要改这里。 public static final int leftFlywheelID = 41; @@ -166,10 +157,11 @@ public final class Constants { // 状态机时间和门控容差。真车基础动作验证后，需要现场调这些值。 public static final double shooterFeedPercent = 1.0; public static final double shooterReversePercent = -0.6; - public static final double cleanupSeconds = 0.3; public static final double shootTimeoutSeconds = 1.0; public static final double aimToleranceDegrees = 1.5; public static final double stationarySpeedToleranceMetersPerSecond = 0.5; + public static final double passBallFlywheelRps = Shooter.defaultFlywheelTargetRps; + public static final double passBallBackboardPosition = 175.0; // 射击时背板的目标位置（外接编码器计数）。 public static final double backboardShootPosition = 175.0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 35ca4d3..52f59bd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,7 +12,6 @@ import edu.wpi.first.wpilibj2.command.Commands; 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.OperatorControls; @@ -67,13 +66,11 @@ public class RobotContainer { // 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); + public final DriveControls driveControls = new DriveControls(drivetrain, controllerlower, superstructure); // 操作员意图层替代旧的射击/进球分离控制类，让复杂动作判断集中留在 Superstructure。 private final OperatorControls operatorControls = new OperatorControls(superstructure, shooterSubsystem); @@ -151,26 +148,19 @@ public class RobotContainer { })); // 预留的左摇杆自动瞄准切换，当前保持注释，避免改变驾驶员已习惯的按键。 - // controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming)); - // controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); + // controllerlower.leftStick().onTrue(operatorControls.requestAimHubCommand()); + // controllerlower.leftStick().onFalse(operatorControls.requestIdleCommand()); // 上操作员右保险：按住进入自动瞄准模式，松开回到全手动模式。 Trigger autoAim = controllerupper.rightBumper(); // 可按需要再并入驾驶员左摇杆。 - autoAim.onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming)); - autoAim.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); + autoAim.onTrue(operatorControls.requestAimHubCommand()); + autoAim.onFalse(operatorControls.requestIdleCommand()); // 上操作员左保险：按住进入传球模式，松开回到全手动模式。 Trigger autoAim2 = controllerupper.leftBumper(); // 可按需要再并入驾驶员左摇杆。 - autoAim2.onTrue(robotStatusManager.setStatusCommand(RobotStatus.PassingBall)); - autoAim2.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); + autoAim2.onTrue(operatorControls.requestPassBallCommand()); + autoAim2.onFalse(operatorControls.requestIdleCommand()); - controllerlower.leftTrigger() - .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench)) - .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); - - controllerlower.rightTrigger() - .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump)) - .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); } private void configueShooter() { @@ -180,10 +170,7 @@ public class RobotContainer { // 用 teleop 门控避免自动赛阶段误触直接取消整个 PathPlanner 自动。 Trigger teleopOnly = RobotModeTriggers.teleop(); controllerupper.rightTrigger().and(teleopOnly) - .onTrue(operatorControls.requestShootCommand()) - .onFalse(operatorControls.requestIdleCommand()); - controllerupper.y().and(teleopOnly) - .onTrue(operatorControls.requestPrepShootCommand()) + .onTrue(operatorControls.requestShootHubCommand()) .onFalse(operatorControls.requestIdleCommand()); controllerupper.b().and(teleopOnly) .onTrue(operatorControls.requestEjectCommand()) diff --git a/src/main/java/frc/robot/RobotStatusManager.java b/src/main/java/frc/robot/RobotStatusManager.java deleted file mode 100644 index 67050b0..0000000 --- a/src/main/java/frc/robot/RobotStatusManager.java +++ /dev/null @@ -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)); - } -} diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java index f257500..c0b1b42 100644 --- a/src/main/java/frc/robot/controls/Autocommand.java +++ b/src/main/java/frc/robot/controls/Autocommand.java @@ -5,6 +5,7 @@ import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants; import frc.robot.subsystems.IntakeRotaterSubsystem; import frc.robot.subsystems.Superstructure; @@ -28,10 +29,9 @@ public class Autocommand extends SequentialCommandGroup { NamedCommands.registerCommand("stop_intake", idle(superstructure)); NamedCommands.registerCommand("intaker_down", lowerIntaker(intakeRotater)); NamedCommands.registerCommand("intaker_up", idle(superstructure)); - NamedCommands.registerCommand("shoot", shoot(superstructure)); + NamedCommands.registerCommand("shoot_hub", shootHub(superstructure)); NamedCommands.registerCommand("Shake", shake(driveControls)); NamedCommands.registerCommand("AutoAim", autoAim(driveControls)); - NamedCommands.registerCommand("AutoShoot", operatorControls.autoShootToHubCommand()); NamedCommands.registerCommand("reset_backboard0", operatorControls.resetBackboard0Command()); } @@ -56,12 +56,10 @@ public class Autocommand extends SequentialCommandGroup { } // 自动射击和手动射击使用同一套 gated shooting 流程。 - public static Command shoot(Superstructure superstructure) { + public static Command shootHub(Superstructure superstructure) { return Commands.sequence( - superstructure.requestPrepShootCommand(), - Commands.waitUntil(superstructure::canShoot).withTimeout(1.5), - superstructure.requestShootCommand(), - Commands.waitSeconds(1.2), + superstructure.requestShootHubCommand(), + Commands.waitSeconds(Constants.Superstructure.shootTimeoutSeconds), superstructure.requestIdleCommand() ); } diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index e7508aa..44805a0 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -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模式 + //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） + // 鏂板锛氬綋鍓嶆槸鍚︿娇鐢ㄥ満鍦板潗鏍囩郴锛堥粯璁?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,16 +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; double vx = -driver.getLeftY() * maxSpeed * driveScale; double vy = -driver.getLeftX() * maxSpeed * driveScale; double vomega = -driver.getRightX() * maxAngularRate * turnScale; - // ====== execute 内替换 limiter：只限加速、不限减速 ====== double limitedVx = limitAccelOnly(vx, lastVx, vxLimiter); double limitedVy = limitAccelOnly(vy, lastVy, vyLimiter); double limitedOmega = limitAccelOnly(vomega, lastOmega, omegaLimiter); @@ -123,105 +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]); - // 把朝向 hub 的目标角同步给底盘，供 Superstructure 读取 isAimed() 做射击门控； - // 数据来源与 AutoAimming 自动旋转完全相同，保证门控和实际瞄准一致。 - drivetrain.setTargetHeading( - drivetrain.getRotation().plus(Rotation2d.fromDegrees(distanceAndRotation[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]); - switch (robotStatusManager.getStatus()) { - 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]); + double[] activeAim = wantedState == WantedState.PASS_BALL ? passAim : hubAim; + drivetrain.setTargetHeading( + drivetrain.getRotation().plus(Rotation2d.fromDegrees(activeAim[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 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 = - 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); @@ -231,16 +166,12 @@ public class DriveControls { .withVelocityY(vy) .withRotationalRate(vomega); } - }, () -> fieldCentricEnabled || - robotStatusManager.getStatus() == RobotStatus.CrossingBump || - robotStatusManager.getStatus() == RobotStatus.CrossingTrench - ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC); + }, () -> fieldCentricEnabled ? DriveMode.FIELD_CENTRIC : DriveMode.ROBOT_CENTRIC); } - /** * - * 输出移动到距离两个目标之间较近的那个点的位移 - * @return 位移 有正负 + * 杈撳嚭绉诲姩鍒拌窛绂讳袱涓洰鏍囦箣闂磋緝杩戠殑閭ｄ釜鐐圭殑浣嶇Щ + * @return 浣嶇Щ 鏈夋璐? */ public double calculateDifferenceToTwoTarget(double current, double target1, double target2){ double x1 = target1 - current; @@ -248,16 +179,16 @@ public class DriveControls { 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] @@ -267,26 +198,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] @@ -296,12 +227,12 @@ public class DriveControls { angleDifference += 360; } - // 计算距离 + // 璁＄畻璺濈 double distance = Math.sqrt(deltaX * deltaX + deltaY * deltaY); - // 计算距离和旋转角度 + // 璁＄畻璺濈鍜屾棆杞搴? double[] distanceAndRotation = new double[]{distance, angleDifference}; - // 返回距离和角度差 + // 杩斿洖璺濈鍜岃搴﹀樊 return distanceAndRotation; } @@ -329,26 +260,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); } - /** 紧急停止：清零限速器，让速度输出立刻归零 */ + /** 绱ф€ュ仠姝細娓呴浂闄愰€熷櫒锛岃閫熷害杈撳嚭绔嬪埢褰掗浂 */ public void emergencyStop() { vxLimiter.reset(0.0); vyLimiter.reset(0.0); @@ -386,13 +317,13 @@ public class DriveControls { return maxAngularRate; } public void setTeamColors() { - Optional ally = DriverStation.getAlliance(); // 获取队伍颜色 + Optional ally = DriverStation.getAlliance(); // 鑾峰彇闃熶紞棰滆壊 if (ally.isPresent()) { - // 根据队伍颜色设置布尔值 + // 鏍规嵁闃熶紞棰滆壊璁剧疆甯冨皵鍊? if (ally.get() == Alliance.Red) { - isRedAlliance = true; // 红队 + isRedAlliance = true; // 绾㈤槦 } else if (ally.get() == Alliance.Blue) { - isRedAlliance = false; // 蓝队 + isRedAlliance = false; // 钃濋槦 } } else { isRedAlliance = true; @@ -438,20 +369,20 @@ public class DriveControls { switch (phase) { case 0: - vx = amplitude; // 前 + vx = amplitude; // 鍓? vy = 0.0; break; case 1: vx = 0.0; - vy = amplitude; // 右 + vy = amplitude; // 鍙? break; case 2: - vx = -amplitude; // 后 + vx = -amplitude; // 鍚? vy = 0.0; break; case 3: vx = 0.0; - vy = -amplitude; // 左 + vy = -amplitude; // 宸? break; } @@ -473,11 +404,11 @@ public class DriveControls { public void pushDistanceData(){ double[] distanceAndRotation = calculateDistanceAndRotationToHub(); - // 将距离值写入 NetworkTable + // 灏嗚窛绂诲€煎啓鍏?NetworkTable autoControlNetworkTable.getEntry("distanceToHub").setDouble(distanceAndRotation[0]); autoControlNetworkTable.getEntry("angleDifferenceToHub").setDouble(distanceAndRotation[1]); - // 自动阶段默认驾驶命令不运行，这里同样保持目标朝向最新， - // 让自动程序里的射击门控 isAimed() 有效。 + // 鑷姩闃舵榛樿椹鹃┒鍛戒护涓嶈繍琛岋紝杩欓噷鍚屾牱淇濇寔鐩爣鏈濆悜鏈€鏂帮紝 + // 璁╄嚜鍔ㄧ▼搴忛噷鐨勫皠鍑婚棬鎺?isAimed() 鏈夋晥銆? drivetrain.setTargetHeading( drivetrain.getRotation().plus(Rotation2d.fromDegrees(distanceAndRotation[1]))); } diff --git a/src/main/java/frc/robot/controls/OperatorControls.java b/src/main/java/frc/robot/controls/OperatorControls.java index 6bb1068..73c4f50 100644 --- a/src/main/java/frc/robot/controls/OperatorControls.java +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -2,6 +2,7 @@ package frc.robot.controls; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.Constants; import frc.robot.subsystems.IntakeRotaterSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.Superstructure; @@ -29,12 +30,16 @@ public class OperatorControls { return superstructure.requestIntakeCommand(); } - public Command requestPrepShootCommand() { - return superstructure.requestPrepShootCommand(); + public Command requestShootHubCommand() { + return superstructure.requestShootHubCommand(); } - public Command requestShootCommand() { - return superstructure.requestShootCommand(); + public Command requestAimHubCommand() { + return superstructure.requestAimHubCommand(); + } + + public Command requestPassBallCommand() { + return superstructure.requestPassBallCommand(); } public Command requestEjectCommand() { @@ -73,10 +78,8 @@ public class OperatorControls { // 最后回到空闲状态。真正的“是否能射”判断仍然放在 Superstructure。 public Command autoShootToHubCommand() { return Commands.sequence( - superstructure.requestPrepShootCommand(), - Commands.waitUntil(superstructure::canShoot).withTimeout(1.5), - superstructure.requestShootCommand(), - Commands.waitSeconds(1.2), + superstructure.requestShootHubCommand(), + Commands.waitSeconds(Constants.Superstructure.shootTimeoutSeconds), superstructure.requestIdleCommand() ); } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java index 2fe0b06..b4cbc95 100644 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -23,8 +23,9 @@ public class Superstructure extends SubsystemBase { public enum WantedState { IDLE, INTAKE, - PREP_SHOOT, - SHOOT, + SHOOT_HUB, + AIM_HUB, + PASS_BALL, EJECT, MANUAL } @@ -34,11 +35,8 @@ public class Superstructure extends SubsystemBase { DISABLED, STOWED, INTAKING, - SPINNING_UP, - AIMING, - READY_TO_SHOOT, + PREP_SHOOT, SHOOTING, - CLEANUP, EJECTING, MANUAL_OVERRIDE } @@ -56,6 +54,7 @@ public class Superstructure extends SubsystemBase { private double stateStartTimestamp = Timer.getFPGATimestamp(); private double flywheelSpeedOffsetRps = 0.0; private double backboardPositionOffset = 0.0; + private boolean shotCompletedThisRequest = false; public Superstructure( ShooterSubsystem shooter, @@ -85,12 +84,26 @@ public class Superstructure extends SubsystemBase { wantedState = WantedState.INTAKE; } - public void requestPrepShoot() { - wantedState = WantedState.PREP_SHOOT; + 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() { - wantedState = WantedState.SHOOT; + requestShootHub(); } public void requestEject() { @@ -109,12 +122,20 @@ public class Superstructure extends SubsystemBase { return Commands.runOnce(this::requestIntake, this); } - public Command requestPrepShootCommand() { - return Commands.runOnce(this::requestPrepShoot, 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 Commands.runOnce(this::requestShoot, this); + return requestShootHubCommand(); } public Command requestEjectCommand() { @@ -133,6 +154,10 @@ public class Superstructure extends SubsystemBase { return systemState; } + public boolean isShootCycleComplete() { + return shotCompletedThisRequest; + } + /* ====================== */ /* 射击门控 */ /* ====================== */ @@ -188,8 +213,9 @@ public class Superstructure extends SubsystemBase { switch (wantedState) { case IDLE -> handleIdle(); case INTAKE -> handleIntake(); - case PREP_SHOOT -> handlePrepShoot(false); - case SHOOT -> handlePrepShoot(true); + case SHOOT_HUB -> handleShootHub(); + case AIM_HUB -> handleAimHub(); + case PASS_BALL -> handlePassBall(); case EJECT -> handleEject(); case MANUAL -> handleManual(); } @@ -204,6 +230,7 @@ public class Superstructure extends SubsystemBase { shooter.stopFlywheel(); shooter.stopShooterConveyor(); shooter.holdBackboardAt(0.0); + setSystemState(SystemState.STOWED); } private void handleIntake() { @@ -218,24 +245,39 @@ public class Superstructure extends SubsystemBase { setSystemState(SystemState.INTAKING); } - private void handlePrepShoot(boolean feedWhenReady) { - if (systemState != SystemState.SPINNING_UP - && systemState != SystemState.AIMING - && systemState != SystemState.READY_TO_SHOOT - && systemState != SystemState.SHOOTING - && systemState != SystemState.CLEANUP) { - setSystemState(SystemState.SPINNING_UP); - } + private void handleAimHub() { + intakeRoller.stop(); + conveyor.stop(); + shooter.stopFlywheel(); + shooter.stopShooterConveyor(); + shooter.holdBackboardAt(0.0); + setSystemState(SystemState.STOWED); + } - intakeRotater.lowerForMatch(); + private void handleShootHub() { + double targetRps = limelight.getShooterSetpointByDistance() + flywheelSpeedOffsetRps; + double backboardPosition = Constants.Superstructure.backboardShootPosition + backboardPositionOffset; + handlePreparedFeed(targetRps, backboardPosition); + } + + private void handlePassBall() { + double targetRps = Constants.Superstructure.passBallFlywheelRps + flywheelSpeedOffsetRps; + double backboardPosition = Constants.Superstructure.passBallBackboardPosition + backboardPositionOffset; + handlePreparedFeed(targetRps, backboardPosition); + } + + private void handlePreparedFeed(double targetRps, double backboardPosition) { + if (systemState != SystemState.PREP_SHOOT + && systemState != SystemState.SHOOTING) { + setSystemState(SystemState.PREP_SHOOT); + } intakeRoller.stop(); conveyor.stop(); shooter.stopShooterConveyor(); // 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调， // 但不会绕过 Superstructure 状态机。 - double targetRps = limelight.getShooterSetpointByDistance() + flywheelSpeedOffsetRps; shooter.setFlywheelVelocity(targetRps); - shooter.holdBackboardAt(Constants.Superstructure.backboardShootPosition + backboardPositionOffset); + shooter.holdBackboardAt(backboardPosition); if (systemState == SystemState.SHOOTING) { // 只有 canShoot() 通过后才会进入本状态；喂球期间不再复查门控， @@ -245,17 +287,8 @@ public class Superstructure extends SubsystemBase { shooter.runShooterConveyor(Constants.Superstructure.shooterFeedPercent); if (timeInState() > Constants.Superstructure.shootTimeoutSeconds) { - setSystemState(SystemState.CLEANUP); - } - return; - } - - if (systemState == SystemState.CLEANUP) { - intakeRotater.lowerForMatch(); - conveyor.stop(); - shooter.stopShooterConveyor(); - if (timeInState() > Constants.Superstructure.cleanupSeconds) { - setSystemState(SystemState.STOWED); + shotCompletedThisRequest = true; + setSystemState(SystemState.PREP_SHOOT); } return; } @@ -266,14 +299,6 @@ public class Superstructure extends SubsystemBase { // 门控条件实时刷新：条件回落时状态同步回退，仪表盘能看到卡在哪一关。 if (canShoot()) { - setSystemState(SystemState.READY_TO_SHOOT); - } else if (shooter.atSpeed()) { - setSystemState(SystemState.AIMING); - } else { - setSystemState(SystemState.SPINNING_UP); - } - - if (feedWhenReady && systemState == SystemState.READY_TO_SHOOT) { setSystemState(SystemState.SHOOTING); } }