diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 93516d6..dbf71af 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,24 +1,41 @@ 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; public final class Constants { + private Constants() {} + public static final boolean LEDUsing = false; - public enum RobotStatus{ + + public enum RobotStatus { Stopped, AllTelop, PassingBall, AutoAimming, - Climbing, CrossingBump, CrossingTrench } - public class Shooter { + + public static class Shooter { + // 机构 CAN ID 目前是占位值；季后赛最终接线确认后，只需要改这里。 + public static final int leftFlywheelID = 41; + public static final int rightFlywheelID = 42; + public static final int leftConveyorID = 43; + public static final int rightConveyorID = 45; + public static final int backboardMotorID = 44; + + public static final double flywheelVoltsPerRps = 0.12; + public static final double flywheelIdleVoltage = 0.0; + public static final double shooterConveyorVoltage = 6.0; + public static final double shooterConveyorReverseVoltage = -4.0; + public static final double flywheelSpeedToleranceRps = 5.0; + public static final double defaultFlywheelTargetRps = 64.0; + public static final double conveyorSpeed = 150; public static final double conveyorSpeedForAuto = 10; + public static final Slot0Configs flyWheelSlot0Configs = new Slot0Configs() .withKP(0.0).withKI(0).withKD(0) .withKV(0.099).withKA(0.0).withKS(0.0); @@ -31,18 +48,18 @@ 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 class backboardPositionPID { public static final double kP = 0.3; public static final double kI = 0.0; public static final double kD = 0.0; - } - public static final double backboardSpeedMax = 60; //也就是output - // 背板的上下限位角度 - public static final double backboardUpLimit = 3000.0; // 背板上限角度（根据实际需求设置） - public static final double backboardDownLimit = 0.0; // 背板下限角度（根据实际需求设置） + + public static final double backboardSpeedMax = 60; + 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; @@ -50,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 { @@ -73,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 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 = true; // 是否使用 MetaTag2（如果你们的 pipeline 配置了这个功能的话） - public static final int climbPipelineIndex = 8; + 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; @@ -117,56 +111,55 @@ public final class Constants { public static final double RedPassingBallPosX = 13.355; public static final double BluePassingBallPosX = 3.184; } - - public class Intaker { + public static class Intaker { + // 进球机构 CAN ID 同样是占位值，真实 ID 确认后集中在这里修改。 + public static final int leftIntakeRollerID = 47; + public static final int rightIntakeRollerID = 46; + public static final int leftIntakeRotaterID = 48; + public static final int rightIntakeRotaterID = 49; - 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 intakeRollerVoltage = 6.0; + public static final double outtakeRollerVoltage = -5.0; + public static final double holdRollerVoltage = 1.0; - public static final Slot0Configs intakeGetterSlot0Configs = 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 double intakeRotaterStowPosition = 0.0; + public static final double intakeRotaterGroundPosition = -7.6; + public static final double intakeRotaterHandoffPosition = -2.5; + public static final double intakeRotaterSafePosition = 0.0; + public static final double intakeRotaterManualVoltage = 2.0; - 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 intakeRotaterUpLimit = 0.0; + public static final double intakeRotaterDownLimit = -7.6; + public static final double intakeRotaterVelocity = 5; + public static final double IntakeRotaterGravityFF = 0.0; - public static final double IntakeGetterSpeedforAuto = 0.5; - // 单位RPS - public static final double positionToleranceRotations = 0.00; - //rotater容差 + public static final Slot0Configs intakeGetterSlot0Configs = new Slot0Configs() + .withKP(0.00).withKI(0.00).withKD(0.00) + .withKV(0.00).withKA(0.00).withKS(0.00); - } - public static class Climber { + 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); - public static final int motorID = 49; + public static final double IntakeGetterSpeedforAuto = 0.5; + public static final double positionToleranceRotations = 0.00; + } - public static final boolean inverted = false; + public static class Conveyor { + public static final int mainConveyorID = 50; + public static final double feedVoltage = 6.0; + public static final double reverseVoltage = -5.0; + public static final double holdVoltage = 0.8; + } - 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 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 class Superstructure { + // 状态机时间和门控容差。真车基础动作验证后，需要现场调这些值。 + 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; } } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java index fe215d7..2d90120 100644 --- a/src/main/java/frc/robot/Main.java +++ b/src/main/java/frc/robot/Main.java @@ -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; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 17bcdae..6e19cca 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -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; import edu.wpi.first.math.geometry.Pose2d; @@ -30,9 +30,13 @@ public class Robot extends TimedRobot { @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 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4e782ad..17e530d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,35 +9,35 @@ 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.LedBindings; -import frc.robot.controls.ClimberControls; -import frc.robot.controls.Autocommand; -import frc.robot.subsystems.ClimberSubsystem; import frc.robot.utils.GameData2026; +import frc.robot.utils.LedBindings; + public class RobotContainer { /* ====================== */ /* 手柄 */ /* ====================== */ - // 使用2个 Xbox 手柄（端口 0,1） + // 使用两个 Xbox 手柄：0 号给底盘驾驶，1 号给机构操作员。 private final CommandXboxController controllerlower = new CommandXboxController(0); private final CommandXboxController controllerupper = new CommandXboxController(1); @@ -45,60 +45,61 @@ public class RobotContainer { /* 子系统 */ /* ====================== */ - // 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, robotStatusManager); + + // 操作员意图层替代旧的射击/进球分离控制类，让复杂动作判断集中留在 Superstructure。 + private final OperatorControls operatorControls = new OperatorControls(superstructure, shooterSubsystem); private final DriveGainsTuner driveGainsTuner = new DriveGainsTuner(drivetrain); private final GameData2026 gameData2026 = new GameData2026(); - 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 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 SendableChooser 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, driveControls, - shooterControls + operatorControls ); SignalLogger.enableAutoLogging(false); autoChooser = AutoBuilder.buildAutoChooser(); @@ -114,85 +115,88 @@ 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()); - - //左保险：按住刹车 + // 左保险：按住刹车。 controllerlower.leftBumper() - .onTrue(Commands.runOnce(driveControls::emergencyStop)) - .whileTrue(driveControls.brakeWhileHeld()); - - // 右保险：按住加速（Boost） + .onTrue(Commands.runOnce(driveControls::emergencyStop)) + .whileTrue(driveControls.brakeWhileHeld()); + + // 右保险：按住开启加速模式，松开后恢复普通速度。 controllerlower.rightBumper() .onTrue(Commands.runOnce(() -> driveControls.setBoostEnabled(true))) .onFalse(Commands.runOnce(() -> driveControls.setBoostEnabled(false))); - // Back：重置场地坐标系角度 + // Back 键：重置场地坐标系角度。 controllerlower.back().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); - - // 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); - System.out.println("Try using shooter for metatag2"); - })); - controllerlower.b().whileTrue(drivetrain.run(()->{drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Intaker); - System.out.println("Try using intaker for metatag2"); - })); - //LM 按下时autoaimming，松开时alltelop + controllerlower.a().whileTrue(drivetrain.run(() -> { + drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Shooter); + System.out.println("Try using shooter for metatag2"); + })); + controllerlower.b().whileTrue(drivetrain.run(() -> { + drivetrain.forceUsingLimelightmt2WithllName(Constants.Limelight.LIMELIGHT_NAME_Intaker); + System.out.println("Try using intaker for metatag2"); + })); + + // 预留的左摇杆自动瞄准切换，当前保持注释，避免改变驾驶员已习惯的按键。 // controllerlower.leftStick().onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming)); // controllerlower.leftStick().onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); - //双操均可触发autoaimming模式，松开时alltelop - Trigger autoAim = controllerupper.rightBumper();//.or(controllerlower.rightStick()); + // 上操作员右保险：按住进入自动瞄准模式，松开回到全手动模式。 + Trigger autoAim = controllerupper.rightBumper(); // 可按需要再并入驾驶员左摇杆。 autoAim.onTrue(robotStatusManager.setStatusCommand(RobotStatus.AutoAimming)); autoAim.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); - //双操均可触发passingball模式，松开时alltelop - Trigger autoAim2 = controllerupper.leftBumper();//.or(controllerlower.leftStick()); + // 上操作员左保险：按住进入传球模式，松开回到全手动模式。 + Trigger autoAim2 = controllerupper.leftBumper(); // 可按需要再并入驾驶员左摇杆。 autoAim2.onTrue(robotStatusManager.setStatusCommand(RobotStatus.PassingBall)); autoAim2.onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); controllerlower.leftTrigger() - .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench)) - .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); - - controllerlower.rightTrigger() - .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump)) - .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); + .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingTrench)) + .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); - controllerlower.a() - .onTrue(robotStatusManager.setStatusCommand(RobotStatus.Climbing)) - .onFalse(robotStatusManager.setStatusCommand(RobotStatus.AllTelop)); + controllerlower.rightTrigger() + .onTrue(robotStatusManager.setStatusCommand(RobotStatus.CrossingBump)) + .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())); - controllerupper.start().onTrue(Commands.runOnce(() -> shooterControls.resetBackboardCommand())); - + private void configueShooter() { + // 射击相关按钮只请求 Superstructure 状态；是否喂球由 Superstructure + // 根据飞轮速度、底盘瞄准和进球旋转机构位置统一判断。 + controllerupper.rightTrigger() + .onTrue(operatorControls.requestShootCommand()) + .onFalse(operatorControls.requestIdleCommand()); + controllerupper.y() + .onTrue(operatorControls.requestPrepShootCommand()) + .onFalse(operatorControls.requestIdleCommand()); + controllerupper.b() + .onTrue(operatorControls.requestEjectCommand()) + .onFalse(operatorControls.requestIdleCommand()); + controllerupper.povLeft().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelSpeedOffset(-1))); + controllerupper.povRight().onTrue(Commands.runOnce(() -> operatorControls.adjustFlywheelSpeedOffset(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() { LedBindings.bindModeIndicators(leds); } - - public Command getAutonomousCommand() { + public Command getAutonomousCommand() { return autoChooser.getSelected(); } @@ -208,17 +212,12 @@ public class RobotContainer { gameData2026.periodic(); } - // private void configueClimber() { - // climberSubsystem.setDefaultCommand(climberControls.defaultClimberCommand()); - // } - private void configueIntaker() { - IntakerSubsystem.setDefaultCommand(intakerControls.defaultIntakerCommand()); - //controllerupper.a().onTrue(intakerControls.resetIntakerRotaterEncoderCommand(IntakerSubsystem)); + // 进球相关按钮也只请求 Superstructure 状态。 + // A 键是校准动作，只应在进球机构处于已知零点姿态时按下。 + controllerupper.leftTrigger() + .onTrue(operatorControls.requestIntakeCommand()) + .onFalse(operatorControls.requestIdleCommand()); + controllerupper.a().onTrue(operatorControls.resetIntakeRotaterEncoderCommand(intakeRotaterSubsystem)); } - - // public ClimberSubsystem getClimberSubsystem() { - // return climberSubsystem; - // } - } diff --git a/src/main/java/frc/robot/controls/Autocommand.java b/src/main/java/frc/robot/controls/Autocommand.java index 99c0708..c79d392 100644 --- a/src/main/java/frc/robot/controls/Autocommand.java +++ b/src/main/java/frc/robot/controls/Autocommand.java @@ -2,188 +2,69 @@ 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; +import frc.robot.subsystems.Superstructure; - public class Autocommand extends SequentialCommandGroup{ - - public static void preNameCommands( - // ClimberSubsystem climber, - IntakerSubsystem intaker, - CommandSwerveDrivetrain drivetrain, - ShooterSubsystem shooter, - DriveControls driveControls, - ShooterControls shooterControls +/** + * 注册 PathPlanner 的 NamedCommands。 + * + * 自动路径只请求机器人级动作，不直接设置 shooter、intake 或 conveyor 电机； + * 这样自动和手动都会走同一套 Superstructure 状态机。 + */ +public class Autocommand extends SequentialCommandGroup { + public static void preNameCommands( + Superstructure superstructure, + DriveControls driveControls, + 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", startIntake(superstructure)); + NamedCommands.registerCommand("intaker_up", idle(superstructure)); + NamedCommands.registerCommand("shoot", shoot(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("AutoShoot", operatorControls.autoShootToHubCommand()); + 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 start_intake(IntakerSubsystem intaker) { - return new InstantCommand(() -> { - intaker.setIntakerGetterSpeed(Constants.Intaker.IntakeGetterSpeedforAuto); // 启动吸球 - }, intaker); - } - 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 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)) - - .andThen(new InstantCommand(() -> { - intaker.stopIntakerotater(); // 停止 - }, intaker)); - } - 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(); - } - - - + public static Command startIntake(Superstructure superstructure) { + return superstructure.requestIntakeCommand(); } + 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(); + } + // 自动射击和手动射击使用同一套 gated shooting 流程。 + public static Command shoot(Superstructure superstructure) { + return Commands.sequence( + superstructure.requestPrepShootCommand(), + Commands.waitUntil(superstructure::canShoot).withTimeout(1.5), + superstructure.requestShootCommand(), + Commands.waitSeconds(1.2), + superstructure.requestIdleCommand() + ); + } + public static Command autoAim(DriveControls driveControls) { + return driveControls.autoAimCommand(); + } - + public static Command shake(DriveControls driveControls) { + return driveControls.shakeCommand(); + } +} diff --git a/src/main/java/frc/robot/controls/ClimberControls.java b/src/main/java/frc/robot/controls/ClimberControls.java deleted file mode 100644 index 358e6da..0000000 --- a/src/main/java/frc/robot/controls/ClimberControls.java +++ /dev/null @@ -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); - } - -} diff --git a/src/main/java/frc/robot/controls/DriveControls.java b/src/main/java/frc/robot/controls/DriveControls.java index 85eb28d..c07f11f 100644 --- a/src/main/java/frc/robot/controls/DriveControls.java +++ b/src/main/java/frc/robot/controls/DriveControls.java @@ -151,9 +151,6 @@ public class DriveControls { case AllTelop: 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) { @@ -405,16 +402,6 @@ 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() { return drivetrain.applyRequest(() -> { double[] distanceAndRotation = calculateDistanceAndRotationToHub(); diff --git a/src/main/java/frc/robot/controls/IntakerControls.java b/src/main/java/frc/robot/controls/IntakerControls.java deleted file mode 100644 index 0937c78..0000000 --- a/src/main/java/frc/robot/controls/IntakerControls.java +++ /dev/null @@ -1,96 +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); - - 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); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/controls/OperatorControls.java b/src/main/java/frc/robot/controls/OperatorControls.java new file mode 100644 index 0000000..b5a3861 --- /dev/null +++ b/src/main/java/frc/robot/controls/OperatorControls.java @@ -0,0 +1,93 @@ +package frc.robot.controls; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +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 requestPrepShootCommand() { + return superstructure.requestPrepShootCommand(); + } + + public Command requestShootCommand() { + return superstructure.requestShootCommand(); + } + + public Command requestEjectCommand() { + return superstructure.requestEjectCommand(); + } + + public void adjustFlywheelSpeedOffset(double offset) { + superstructure.adjustFlywheelSpeedOffset(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 resetIntakeRotaterEncoderCommand(IntakeRotaterSubsystem intakeRotater) { + return Commands.runOnce(intakeRotater::zeroPosition, intakeRotater); + } + + // PathPlanner 自动射击辅助：先预备到所有射击条件满足，再请求喂球， + // 最后回到空闲状态。真正的“是否能射”判断仍然放在 Superstructure。 + public Command autoShootToHubCommand() { + return Commands.sequence( + superstructure.requestPrepShootCommand(), + Commands.waitUntil(superstructure::canShoot).withTimeout(1.5), + superstructure.requestShootCommand(), + Commands.waitSeconds(1.2), + superstructure.requestIdleCommand() + ); + } + + // 保留旧背板归零/回零命令，方便调参和现场恢复。 + public Command resetBackboard0Command() { + return Commands.sequence( + Commands.runOnce(() -> shooterSubsystem.setBackboardPosition(0.0), shooterSubsystem), + Commands.run( + () -> { + if (shooterSubsystem.isBackboardAtTarget()) { + shooterSubsystem.backboardMotor.set(0.0); + } else { + shooterSubsystem.outputBackboard(); + } + }, + shooterSubsystem + ).until(shooterSubsystem::isBackboardAtTarget), + Commands.runOnce(() -> shooterSubsystem.backboardMotor.set(0.0), shooterSubsystem) + ); + } +} diff --git a/src/main/java/frc/robot/controls/ShooterControls.java b/src/main/java/frc/robot/controls/ShooterControls.java deleted file mode 100644 index ac98a70..0000000 --- a/src/main/java/frc/robot/controls/ShooterControls.java +++ /dev/null @@ -1,225 +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(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(){ - // 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); - })); - } -} - diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java deleted file mode 100644 index d2c3284..0000000 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ /dev/null @@ -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); - } -} diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 585e787..27878ab 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -1,4 +1,5 @@ package frc.robot.subsystems; + import java.util.Optional; import java.util.function.Supplier; @@ -15,58 +16,56 @@ 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; - + private NetworkTable positioningNetworkTable; private NetworkTable ll_shooter_NT; private NetworkTable ll_intaker_NT; private NetworkTable driveNetworkTable; - + private final StructPublisher posePub = - NetworkTableInstance.getDefault() - .getStructTopic("/Odometry/RobotPose", Pose2d.struct) - .publish(); + NetworkTableInstance.getDefault() + .getStructTopic("/Odometry/RobotPose", Pose2d.struct) + .publish(); private Field2d m_field; private Field2d shooter_ll_field; private Field2d intaker_ll_field; - public CommandSwerveDrivetrain( SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules @@ -86,12 +85,9 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su ll_intaker_NT = NetworkTableInstance.getDefault().getTable("Positioning/limelight-intaker"); 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,32 +118,70 @@ 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( new SwerveRequest.ApplyRobotSpeeds().withSpeeds(scaled) - ); + ); } - + public void stop() { setControl( new SwerveRequest.ApplyRobotSpeeds().withSpeeds(new ChassisSpeeds(0, 0, 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 kMaxVisionJumpMeters = 1.0; 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 request) { return run(() -> this.setControl(request.get())); @@ -160,10 +194,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su this.setControl(requestSupplier.get()); }); } - + public void forceUsingLimelightmt2() { LimelightHelpers.PoseEstimate mt2 = - LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter); + LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(Constants.Limelight.LIMELIGHT_NAME_Shooter); if (mt2 == null || mt2.tagCount == 0) { return; @@ -172,10 +206,10 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su var stdDevs = VecBuilder.fill(0.7, 0.7, 9999999); addVisionMeasurement(mt2.pose, mt2.timestampSeconds, stdDevs); } - + public void forceUsingLimelightmt2WithllName(String llString) { LimelightHelpers.PoseEstimate mt2 = - LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(llString); + LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(llString); if (mt2 == null || mt2.tagCount == 0) { return; @@ -184,14 +218,15 @@ 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; LimelightHelpers.PoseEstimate mt2_shooter = - getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Shooter); - + getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Shooter); + LimelightHelpers.PoseEstimate mt2_intaker = - getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Intaker); + getLimelightPoseEstimate(Constants.Limelight.LIMELIGHT_NAME_Intaker); boolean shooterAvailable = ifLimelightPositioningAvailable(mt2_shooter, current); boolean intakerAvailable = ifLimelightPositioningAvailable(mt2_intaker, current); @@ -199,35 +234,34 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su ll_intaker_NT.getEntry("available").setBoolean(intakerAvailable); ll_shooter_NT.getEntry("TagCount").setNumber(mt2_shooter != null ? mt2_shooter.tagCount : 0); ll_intaker_NT.getEntry("TagCount").setNumber(mt2_intaker != null ? mt2_intaker.tagCount : 0); - + 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()); @@ -245,7 +279,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su mt2_final = shooterDistance <= intakerDistance ? mt2_shooter : mt2_intaker; } - + Pose2d mt2Pose = mt2_final.pose; double distance = current.getTranslation().getDistance(mt2Pose.getTranslation()); @@ -258,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); } @@ -273,156 +307,28 @@ 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(); // 若你有这个信号 - 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; - // } + // 旋转太快时拒绝该帧，避免高速转动时的视觉跳变。 + double omegaDegPerSec = getPigeon2().getAngularVelocityZWorld().getValueAsDouble(); + if (Math.abs(omegaDegPerSec) > kMaxOmegaDegPerSec) { + 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); @@ -431,20 +337,19 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su 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 ); @@ -460,15 +365,13 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su m_hasAppliedOperatorPerspective = true; }); } - } - - - /** - * 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) { @@ -476,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. - *

- * 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( @@ -498,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 samplePoseAt(double timestampSeconds) { diff --git a/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java b/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java new file mode 100644 index 0000000..99cc837 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ConveyorSubsystem.java @@ -0,0 +1,60 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; + +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; + +/** + * 主 conveyor 的开环控制封装。 + * + * 本赛季没有球检测传感器，因此这里仅报告电机状态； + * 所有动作顺序判断都放在 Superstructure。 + */ +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 voltage = 0.0; + + public ConveyorSubsystem() { + mainConveyor = new TalonFX(Constants.Conveyor.mainConveyorID); + } + + public void feedToShooter() { + setVoltage(Constants.Conveyor.feedVoltage); + } + + public void reverse() { + setVoltage(Constants.Conveyor.reverseVoltage); + } + + public void hold() { + setVoltage(Constants.Conveyor.holdVoltage); + } + + public void stop() { + setVoltage(0.0); + } + + public void setPercent(double percent) { + setVoltage(percent * 12.0); + } + + public void setVoltage(double volts) { + voltage = MathUtil.clamp(volts, -12.0, 12.0); + mainConveyor.setControl(voltageRequest.withOutput(voltage)); + } + + @Override + public void periodic() { + table.getEntry("voltage").setDouble(voltage); + table.getEntry("velocity").setDouble(mainConveyor.getVelocity().getValueAsDouble()); + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeRollerSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeRollerSubsystem.java new file mode 100644 index 0000000..e188409 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeRollerSubsystem.java @@ -0,0 +1,77 @@ +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; + public final TalonFX rightIntakeRoller; + + private final VoltageOut voltageRequest = new VoltageOut(0); + private final Follower rightFollower; + private final NetworkTable table = NetworkTableInstance.getDefault().getTable("IntakeRoller"); + + private double leftVoltage = 0.0; + private double rightVoltage = 0.0; + + public IntakeRollerSubsystem() { + leftIntakeRoller = new TalonFX(Constants.Intaker.leftIntakeRollerID); + rightIntakeRoller = new TalonFX(Constants.Intaker.rightIntakeRollerID); + rightFollower = new Follower(leftIntakeRoller.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50); + followLeft(); + } + + public void intake() { + setVoltage(Constants.Intaker.intakeRollerVoltage, -Constants.Intaker.intakeRollerVoltage); + } + + public void outtake() { + setVoltage(Constants.Intaker.outtakeRollerVoltage, -Constants.Intaker.outtakeRollerVoltage); + } + + public void hold() { + setVoltage(Constants.Intaker.holdRollerVoltage, -Constants.Intaker.holdRollerVoltage); + } + + public void stop() { + setVoltage(0.0, 0.0); + } + + public void setPercent(double left, double right) { + setVoltage(left * 12.0, right * 12.0); + } + + public void setVoltage(double left, double right) { + // rightVoltage 仅用于 dashboard 显示；真实硬件通过反向 follower 跟随左侧电机。 + leftVoltage = MathUtil.clamp(left, -12.0, 12.0); + rightVoltage = MathUtil.clamp(right, -12.0, 12.0); + leftIntakeRoller.setControl(voltageRequest.withOutput(leftVoltage)); + followLeft(); + } + + private void followLeft() { + rightIntakeRoller.setControl(rightFollower); + } + + @Override + public void periodic() { + table.getEntry("leftVoltage").setDouble(leftVoltage); + table.getEntry("rightVoltage").setDouble(rightVoltage); + table.getEntry("leftVelocity").setDouble(leftIntakeRoller.getVelocity().getValueAsDouble()); + table.getEntry("rightVelocity").setDouble(rightIntakeRoller.getVelocity().getValueAsDouble()); + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java new file mode 100644 index 0000000..1034b57 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeRotaterSubsystem.java @@ -0,0 +1,145 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.MotorAlignmentValue; + +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; + +/** + * 进球旋转臂的位置闭环和开环控制。 + * + * 机构使用 TalonFX 集成编码器。归零前只允许 SAFE 或受限手动移动， + * 避免相对编码器未校准时发生大幅度意外旋转。 + */ +public class IntakeRotaterSubsystem extends SubsystemBase { + public enum IntakePosition { + STOW(Constants.Intaker.intakeRotaterStowPosition), + GROUND_INTAKE(Constants.Intaker.intakeRotaterGroundPosition), + HANDOFF(Constants.Intaker.intakeRotaterHandoffPosition), + SAFE(Constants.Intaker.intakeRotaterSafePosition); + + public final double rotations; + + IntakePosition(double rotations) { + this.rotations = rotations; + } + } + + public final TalonFX leftIntakeRotater; + public final TalonFX rightIntakeRotater; + + private final PositionVoltage positionRequest = new PositionVoltage(0); + private final VoltageOut voltageRequest = new VoltageOut(0); + private final Follower rightFollower; + private final NetworkTable table = NetworkTableInstance.getDefault().getTable("IntakeRotater"); + + private IntakePosition goal = IntakePosition.SAFE; + private boolean zeroed = false; + private double manualVoltage = 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(); + } + + public void setGoal(IntakePosition position) { + // 相对编码器没有对准机械零点前，禁止执行大幅度位置闭环移动。 + goal = position; + if (!zeroed && position != IntakePosition.SAFE) { + stop(); + return; + } + + double target = MathUtil.clamp( + position.rotations, + Constants.Intaker.intakeRotaterDownLimit, + Constants.Intaker.intakeRotaterUpLimit + ); + leftIntakeRotater.setControl(positionRequest.withPosition(target)); + manualVoltage = 0.0; + followLeft(); + } + + public void stow() { + setGoal(IntakePosition.STOW); + } + + public void deployToGround() { + setGoal(IntakePosition.GROUND_INTAKE); + } + + public void moveToHandoff() { + setGoal(IntakePosition.HANDOFF); + } + + public boolean atGoal() { + return Math.abs(getPosition() - goal.rotations) <= Constants.Intaker.positionToleranceRotations; + } + + public void zeroPosition() { + // 只有 intake 物理上处于已知零点姿态时才调用此方法。 + leftIntakeRotater.setPosition(0.0); + rightIntakeRotater.setPosition(0.0); + zeroed = true; + goal = IntakePosition.STOW; + stow(); + } + + public boolean isZeroed() { + return zeroed; + } + + public void clearZeroed() { + zeroed = false; + goal = IntakePosition.SAFE; + stop(); + } + + public double getPosition() { + return leftIntakeRotater.getPosition().getValueAsDouble(); + } + + public IntakePosition getGoal() { + return goal; + } + + public void setOpenLoopVoltage(double voltage) { + // 归零前允许手动小幅移动，但输出会被限制到较温和的范围。 + if (!zeroed && Math.abs(voltage) > Constants.Intaker.intakeRotaterManualVoltage) { + voltage = Math.copySign(Constants.Intaker.intakeRotaterManualVoltage, voltage); + } + manualVoltage = MathUtil.clamp(voltage, -12.0, 12.0); + leftIntakeRotater.setControl(voltageRequest.withOutput(manualVoltage)); + followLeft(); + } + + public void stop() { + manualVoltage = 0.0; + leftIntakeRotater.setControl(voltageRequest.withOutput(0.0)); + followLeft(); + } + + private void followLeft() { + rightIntakeRotater.setControl(rightFollower); + } + + @Override + public void periodic() { + table.getEntry("goal").setString(goal.name()); + table.getEntry("zeroed").setBoolean(zeroed); + table.getEntry("position").setDouble(getPosition()); + table.getEntry("atGoal").setBoolean(atGoal()); + table.getEntry("manualVoltage").setDouble(manualVoltage); + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java b/src/main/java/frc/robot/subsystems/IntakerSubsystem.java deleted file mode 100644 index 91a556f..0000000 --- a/src/main/java/frc/robot/subsystems/IntakerSubsystem.java +++ /dev/null @@ -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); - } -} diff --git a/src/main/java/frc/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/robot/subsystems/LimelightSubsystem.java new file mode 100644 index 0000000..3e4d70c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LimelightSubsystem.java @@ -0,0 +1,55 @@ +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 getShooterSetpointByDistance() { + // 初始测试用临时映射；飞轮和背板调好后应替换为实测射击表。 + double distance = getDistanceToTarget(); + return Constants.Shooter.defaultFlywheelTargetRps + (distance - 2.5) * 4.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 6aa831b..9d18915 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -3,15 +3,15 @@ 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; @@ -19,158 +19,261 @@ import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +/** + * Shooter 总成的硬件封装。 + * + * 飞轮和 shooter 内部 conveyor 都使用电压开环输出。 + * 背板控制沿用旧代码中的电机、编码器和 PID 调参流程。 + */ 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; + // 保留旧字段别名，让原有 dashboard tuner 在重构后仍能指向左飞轮和左 conveyor。 + public final TalonFX flywheelMotorLeft; + public final TalonFX conveyorMotor; - private NetworkTable shooterNetworkTable; + public final TalonFXS backboardMotor; + public final Encoder backboardEncoder; + public final ProfiledPIDController backboardPID; + private final VoltageOut flywheelVoltageRequest = new VoltageOut(0); + private final VoltageOut conveyorVoltageRequest = new VoltageOut(0); private final VelocityVoltage velocityRequest = new VelocityVoltage(0); + private final Follower flywheelFollower; + private final Follower conveyorFollower; + + private final NetworkTable shooterNetworkTable; + + private double targetFlywheelRps = 0.0; + private double targetLeftFlywheelRps = 0.0; + private double targetRightFlywheelRps = 0.0; + private double shooterConveyorPercent = 0.0; - // 初始化各个部件 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); - - var configs2 = new MotorOutputConfigs(); - configs2.Inverted = InvertedValue.Clockwise_Positive; - conveyorMotor.getConfigurator().apply(configs2); - - // 初始化背板电机和编码器 - backboardMotor = new TalonFXS(44); + flywheelMotorLeft = leftFlywheel; + conveyorMotor = leftConveyor; + + 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 motorOutputConfigs = new MotorOutputConfigs(); + motorOutputConfigs.Inverted = InvertedValue.Clockwise_Positive; + leftFlywheel.getConfigurator().apply(motorOutputConfigs); + leftConveyor.getConfigurator().apply(motorOutputConfigs); + + flywheelFollower = new Follower(leftFlywheel.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50); + conveyorFollower = new Follower(leftConveyor.getDeviceID(), MotorAlignmentValue.Opposed).withUpdateFreqHz(50); + setRightFollowLeft(); + + 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.setTolerance(10.0); - + + 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"); - } - public void setRightFollowLeft(){ - flywheelMotorRight.setControl(flyWheelFollower); + public void setRightFollowLeft() { + // 成对机构机械方向相反，因此右侧电机以反向 follower 跟随左侧电机。 + rightFlywheel.setControl(flywheelFollower); + 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 setFlywheelVelocity(double leftRps, double rightRps) { + // 对外接口使用 RPS，便于 Superstructure 阅读；实际下发仍是电压开环。 + targetLeftFlywheelRps = leftRps; + targetRightFlywheelRps = rightRps; + targetFlywheelRps = (Math.abs(leftRps) + Math.abs(rightRps)) / 2.0; - // 设置飞轮的速度 - public void setFlywheelSpeedByRPS(double targetSpeed) { - flywheelMotorLeft.setControl(velocityRequest.withVelocity(targetSpeed)); //RPS 转每秒 + double requestedVoltage = targetFlywheelRps * Constants.Shooter.flywheelVoltsPerRps; + requestedVoltage = MathUtil.clamp(requestedVoltage, -12.0, 12.0); + leftFlywheel.setControl(flywheelVoltageRequest.withOutput(requestedVoltage)); setRightFollowLeft(); - // System.out.println("set flywheel "+targetSpeed); } - // 设置传动的速度 + public void setFlywheelVelocityByDistance(double distance) { + // 临时距离映射。拿到真实射击数据后，应替换为实测表或重新调常量。 + double clampedDistance = MathUtil.clamp(distance, 1.2, 5.0); + double targetRps = Constants.Shooter.defaultFlywheelTargetRps + (clampedDistance - 2.5) * 4.0; + setFlywheelVelocity(targetRps, targetRps); + } + + public void setFlywheelSpeedByRPS(double targetSpeed) { + setFlywheelVelocity(targetSpeed, targetSpeed); + } + + public void setFlywheelVoltage(double volts) { + targetLeftFlywheelRps = 0.0; + targetRightFlywheelRps = 0.0; + targetFlywheelRps = 0.0; + leftFlywheel.setControl(flywheelVoltageRequest.withOutput(MathUtil.clamp(volts, -12.0, 12.0))); + setRightFollowLeft(); + } + + public void runShooterConveyor(double percent) { + shooterConveyorPercent = MathUtil.clamp(percent, -1.0, 1.0); + leftConveyor.setControl(conveyorVoltageRequest.withOutput(shooterConveyorPercent * 12.0)); + setRightFollowLeft(); + } + + public void stopShooterConveyor() { + runShooterConveyor(0.0); + } + public void setConveyorSpeedByRPS(double speed) { - conveyorMotor.setControl(velocityRequest.withVelocity(speed)); // 设置传动电机速度 - } - public void setConveyorSpeedOpen(double speed){ - conveyorMotor.set(speed); + leftConveyor.setControl(velocityRequest.withVelocity(speed)); + setRightFollowLeft(); } - //调试用 - public void setBackboardSpeedByRPS(double speed){ - if (speed > 0 && speed < 10) { - speed = 10; + public void setConveyorSpeedOpen(double speed) { + runShooterConveyor(speed); + } + + public void stopFlywheel() { + setFlywheelVoltage(Constants.Shooter.flywheelIdleVoltage); + } + + public void stopAll() { + stopFlywheel(); + stopShooterConveyor(); + backboardMotor.set(0); + } + + public void stopMotors() { + stopAll(); + } + + public boolean atSpeed() { + // 虽然飞轮控制是电压开环，但仍读取 TalonFX 速度作为喂球门控。 + if (targetFlywheelRps <= 1.0) { + return false; + } + return Math.abs(getLeftFlywheelVelocity() - targetLeftFlywheelRps) <= Constants.Shooter.flywheelSpeedToleranceRps + && Math.abs(Math.abs(getRightFlywheelVelocity()) - Math.abs(targetRightFlywheelRps)) <= Constants.Shooter.flywheelSpeedToleranceRps; + } + + 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 getTargetFlywheelRps() { + return targetFlywheelRps; + } + + public void setBackboardSpeedByRPS(double speed) { + if (speed > 0 && speed < 10) { + speed = 10; } else if (speed < 0 && speed > -10) { - speed = -10; + speed = -10; } backboardMotor.setControl(velocityRequest.withVelocity(speed)); } - // 设置背板的目标位置，通过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控制器来计算背板的运动速度 + public void outputBackboard() { + // 背板仍使用旧外部编码器和 ProfiledPID；Superstructure 只选择目标位置。 double pidOutput = backboardPID.calculate(getBackboardPosition()); - pidOutput = Math.max(-Constants.Shooter.backboardSpeedMax, Math.min(Constants.Shooter.backboardSpeedMax, pidOutput)); // 限制输出范围 - + pidOutput = MathUtil.clamp(pidOutput, -Constants.Shooter.backboardSpeedMax, Constants.Shooter.backboardSpeedMax); + if (Math.abs(pidOutput) < 0.8) { - pidOutput = 0.0; - }//防止抖动 - // // 设置背板电机的速度，根据PID输出进行调整 + pidOutput = 0.0; + } + shooterNetworkTable.getEntry("backboardPIDOutput").setDouble(pidOutput); setBackboardSpeedByRPS(pidOutput); } - // 停止所有电机 - public void stopMotors() { - flywheelMotorLeft.set(0); - flywheelMotorRight.set(0); - conveyorMotor.set(0); - backboardMotor.set(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 conveyorWaitForAcceleration() { + stopShooterConveyor(); } - public void conveyorRun(){ - setConveyorSpeedByRPS(Constants.Shooter.conveyorSpeed); + + public void conveyorRun() { + runShooterConveyor(Constants.Superstructure.shooterFeedPercent); } - public void resetbackboardencoder(){ + + public void resetbackboardencoder() { backboardEncoder.reset(); } - public double getConveyorStatorCurrent(){ - double statorCurrent = conveyorMotor.getStatorCurrent().getValueAsDouble(); - return statorCurrent; + + public double getConveyorStatorCurrent() { + return leftConveyor.getStatorCurrent().getValueAsDouble(); + } + + public double getDistanceToHub() { + return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToHub").getDouble(0.0); + } + + public double getDistanceToPassball() { + return NetworkTableInstance.getDefault().getTable("AutoControl").getEntry("distanceToPassball").getDouble(0.0); } @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("flywheelSpeed").setDouble(getAverageFlywheelVelocity()); + shooterNetworkTable.getEntry("leftFlywheelSpeed").setDouble(getLeftFlywheelVelocity()); + shooterNetworkTable.getEntry("rightFlywheelSpeed").setDouble(getRightFlywheelVelocity()); + shooterNetworkTable.getEntry("flywheelTargetSpeed").setDouble(targetFlywheelRps); + shooterNetworkTable.getEntry("flywheelAtSpeed").setBoolean(atSpeed()); + shooterNetworkTable.getEntry("shooterConveyorPercent").setDouble(shooterConveyorPercent); + shooterNetworkTable.getEntry("conveyorSpeed").setDouble(leftConveyor.getVelocity().getValueAsDouble()); shooterNetworkTable.getEntry("backboardCurrentRate").setDouble(getBackboardPosition()); shooterNetworkTable.getEntry("backboardTargetRate").setDouble(backboardPID.getSetpoint().position); shooterNetworkTable.getEntry("isBackboardAtTarget").setBoolean(isBackboardAtTarget()); @@ -178,17 +281,4 @@ public class ShooterSubsystem extends SubsystemBase { shooterNetworkTable.getEntry("distancetopassball").setDouble(getDistanceToPassball()); 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); - } } diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java new file mode 100644 index 0000000..520a2df --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Superstructure.java @@ -0,0 +1,342 @@ +package frc.robot.subsystems; + +import edu.wpi.first.math.geometry.Rotation2d; +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; +import frc.robot.subsystems.IntakeRotaterSubsystem.IntakePosition; + +/** + * 统一协调所有得分相关机构。 + * + * subsystem 只暴露硬件级动作，OperatorControls 只表达操作员意图； + * shooter、intake、conveyor、底盘瞄准和 Limelight 距离如何配合， + * 统一由这里决定。 + */ +public class Superstructure extends SubsystemBase { + // 操作员或自动程序请求的目标状态。 + public enum WantedState { + IDLE, + INTAKE, + PREP_SHOOT, + SHOOT, + EJECT, + MANUAL + } + + // 机器人内部实际执行状态，用于动作排序和仪表盘诊断。 + public enum SystemState { + DISABLED, + STOWED, + DEPLOYING_INTAKE, + INTAKING, + SPINNING_UP, + AIMING, + READY_TO_SHOOT, + SHOOTING, + CLEANUP, + 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 flywheelSpeedOffsetRps = 0.0; + private double backboardPositionOffset = 0.0; + + 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 requestPrepShoot() { + wantedState = WantedState.PREP_SHOOT; + } + + public void requestShoot() { + wantedState = WantedState.SHOOT; + } + + 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 requestPrepShootCommand() { + return Commands.runOnce(this::requestPrepShoot, this); + } + + public Command requestShootCommand() { + return Commands.runOnce(this::requestShoot, this); + } + + 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 canShoot() { + // 本赛季不使用球检测传感器，因此射击门控只检查机器人能实际测量的条件。 + return shooter.atSpeed() + && drive.isAimed(); + } + + public void adjustFlywheelSpeedOffset(double offsetRps) { + flywheelSpeedOffsetRps += offsetRps; + } + + public void adjustBackboardPositionOffset(double offset) { + backboardPositionOffset += offset; + } + + public void resetOffsets() { + flywheelSpeedOffsetRps = 0.0; + backboardPositionOffset = 0.0; + } + + public void resetBackboardEncoder() { + shooter.resetbackboardencoder(); + } + + @Override + public void periodic() { + // 禁用状态优先级最高，防止旧按钮输入或自动请求在禁用后继续驱动机构。 + if (DriverStation.isDisabled()) { + setSystemState(SystemState.DISABLED); + stopAllMechanisms(); + publishTelemetry(); + return; + } + + updateAimingSetpoint(); + + switch (wantedState) { + case IDLE -> handleIdle(); + case INTAKE -> handleIntake(); + case PREP_SHOOT -> handlePrepShoot(false); + case SHOOT -> handlePrepShoot(true); + case EJECT -> handleEject(); + case MANUAL -> handleManual(); + } + + publishTelemetry(); + } + + private void handleIdle() { + // 安全默认状态：传球不动，intake 收回，背板保持受控。 + conveyor.hold(); + intakeRoller.stop(); + intakeRotater.stop(); + shooter.stopFlywheel(); + shooter.stopShooterConveyor(); + holdBackboardAtZero(); + + setSystemState(SystemState.STOWED); + } + + private void handleIntake() { + if (systemState != SystemState.DEPLOYING_INTAKE + && systemState != SystemState.INTAKING) { + setSystemState(SystemState.DEPLOYING_INTAKE); + } + + shooter.stopFlywheel(); + shooter.stopShooterConveyor(); + intakeRotater.deployToGround(); + intakeRoller.intake(); + conveyor.feedToShooter(); + holdBackboardAtZero(); + + if (intakeRotater.atGoal()) { + setSystemState(SystemState.INTAKING); + } + + // 本赛季没有球检测，因此 intake 通过时间退出。 + // 真车确认 roller/conveyor 速度后，需要现场调整这个时间。 + if (systemState == SystemState.INTAKING && timeInState() > 1.0) { + wantedState = WantedState.IDLE; + setSystemState(SystemState.STOWED); + } + } + + 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); + } + + intakeRotater.moveToHandoff(); + intakeRoller.hold(); + conveyor.hold(); + shooter.stopShooterConveyor(); + // 距离到射速的映射由 LimelightSubsystem 提供；操作员 offset 用于现场微调， + // 但不会绕过 Superstructure 状态机。 + double targetRps = limelight.getShooterSetpointByDistance() + flywheelSpeedOffsetRps; + shooter.setFlywheelVelocity(targetRps, targetRps); + holdBackboardForShot(); + + if (shooter.atSpeed()) { + setSystemState(SystemState.AIMING); + } + + if (shooter.atSpeed() && drive.isAimed() && intakeRotater.atGoal()) { + setSystemState(SystemState.READY_TO_SHOOT); + } + + if (feedWhenReady && canShoot()) { + setSystemState(SystemState.SHOOTING); + } + + if (systemState == SystemState.SHOOTING) { + // 只有 canShoot() 通过后才允许喂球。 + conveyor.feedToShooter(); + shooter.runShooterConveyor(Constants.Superstructure.shooterFeedPercent); + + if (timeInState() > Constants.Superstructure.shootTimeoutSeconds) { + setSystemState(SystemState.CLEANUP); + } + } + + if (systemState == SystemState.CLEANUP) { + conveyor.stop(); + shooter.stopShooterConveyor(); + if (timeInState() > Constants.Superstructure.cleanupSeconds) { + wantedState = WantedState.IDLE; + setSystemState(SystemState.STOWED); + } + } + } + + private void handleEject() { + // Eject 会反转整条球路，但飞轮保持停止。 + setSystemState(SystemState.EJECTING); + intakeRotater.deployToGround(); + intakeRoller.outtake(); + conveyor.reverse(); + shooter.runShooterConveyor(Constants.Superstructure.shooterReversePercent); + shooter.stopFlywheel(); + holdBackboardAtZero(); + } + + private void handleManual() { + setSystemState(SystemState.MANUAL_OVERRIDE); + shooter.stopFlywheel(); + shooter.stopShooterConveyor(); + conveyor.stop(); + intakeRoller.stop(); + } + + private void updateAimingSetpoint() { + // Superstructure 可以更新底盘目标朝向，但不直接下发 swerve 模块或底盘速度命令。 + if (limelight.hasTarget()) { + drive.setTargetHeading(drive.getRotation().plus(Rotation2d.fromDegrees(limelight.getTargetYaw()))); + } + } + + private void holdBackboardAtZero() { + shooter.setBackboardPosition(0.0); + if (shooter.isBackboardAtTarget()) { + shooter.backboardMotor.set(0.0); + } else { + shooter.outputBackboard(); + } + } + + private void holdBackboardForShot() { + shooter.setBackboardPosition(175.0 + backboardPositionOffset); + if (shooter.isBackboardAtTarget()) { + shooter.backboardMotor.set(0.0); + } else { + shooter.outputBackboard(); + } + } + + 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("canShoot").setBoolean(canShoot()); + table.getEntry("shooterAtSpeed").setBoolean(shooter.atSpeed()); + table.getEntry("driveAimed").setBoolean(drive.isAimed()); + table.getEntry("intakeAtGoal").setBoolean(intakeRotater.atGoal()); + table.getEntry("flywheelSpeedOffsetRps").setDouble(flywheelSpeedOffsetRps); + table.getEntry("backboardPositionOffset").setDouble(backboardPositionOffset); + } +} +