From 7ed3a5e4b0a2ab3d6733d44f94d8b567d9da345e Mon Sep 17 00:00:00 2001 From: James Guleno Date: Mon, 23 Mar 2026 20:47:26 -0700 Subject: [PATCH] deprecate left/right side of subsystems in implementation and constants, refine constants, relocate subsystem current/goal states into respective subsystem files, add booleans in RobotContainer.java to switch between real/io implementations --- .../java/org/team5924/frc2026/Constants.java | 220 +------- .../org/team5924/frc2026/RobotContainer.java | 471 ++++-------------- .../java/org/team5924/frc2026/RobotState.java | 31 -- .../commands/shooter/AutoScoreCommands.java | 10 +- .../shooter/ManualShooterCommands.java | 11 - .../frc2026/subsystems/flywheel/Flywheel.java | 57 +-- .../subsystems/flywheel/FlywheelIOSim.java | 4 +- .../flywheel/FlywheelIOTalonFX.java | 193 +++---- .../pivots/intakePivot/IntakePivot.java | 15 +- .../pivots/shooterHood/ShooterHood.java | 68 +-- .../pivots/shooterHood/ShooterHoodIOSim.java | 10 +- .../shooterHood/ShooterHoodIOTalonFX.java | 193 +++---- .../subsystems/rollers/hopper/Hopper.java | 4 +- .../subsystems/rollers/indexer/Indexer.java | 4 +- .../subsystems/rollers/intake/Intake.java | 4 +- .../frc2026/subsystems/turret/Turret.java | 213 -------- .../frc2026/subsystems/turret/TurretIO.java | 79 --- .../subsystems/turret/TurretIOSim.java | 79 --- .../subsystems/turret/TurretIOTalonFX.java | 386 -------------- .../subsystems/vision/VisionConstants.java | 12 +- .../frc2026/util/LaunchCalculator.java | 32 +- .../frc2026/util/LauncherConstants.java | 10 +- 22 files changed, 326 insertions(+), 1780 deletions(-) delete mode 100644 src/main/java/org/team5924/frc2026/subsystems/turret/Turret.java delete mode 100644 src/main/java/org/team5924/frc2026/subsystems/turret/TurretIO.java delete mode 100644 src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOSim.java delete mode 100644 src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOTalonFX.java diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index c73c683..a62cd52 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -34,8 +34,6 @@ import edu.wpi.first.math.util.Units; import com.ctre.phoenix6.signals.SensorDirectionValue; -import edu.wpi.first.math.geometry.Translation3d; - import edu.wpi.first.wpilibj.RobotBase; /** @@ -207,9 +205,14 @@ public final class Indexer { /* General Subsystems */ - public final class GeneralShooterHood { + public final class ShooterHood { public static final String BUS = "rio"; public static final double SIM_MOI = 0.001; + public static final int CAN_ID = 34; + + /* CANCoder */ + public static final int CANCODER_ID = 36; + public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; // spur = hood driving gear, mechanism = shooter hood gear public static final double MOTOR_TO_CANCODER = (40.0 / 12.0) * (24.0 / 17.0); @@ -252,25 +255,34 @@ public final class GeneralShooterHood { .withForwardSoftLimitEnable(false) .withReverseSoftLimitEnable(false); - public static final FeedbackConfigs GENERAL_FEEDBACK_CONFIGS = + public static final FeedbackConfigs FEEDBACK_CONFIGS = new FeedbackConfigs() .withSensorToMechanismRatio(CANCODER_TO_MECHANISM) .withRotorToSensorRatio(MOTOR_TO_CANCODER) - .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder); + .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) + .withFeedbackRemoteSensorID(CANCODER_ID) + .withFeedbackRotorOffset(-CANCODER_ABSOLUTE_OFFSET); - public static final MagnetSensorConfigs GENERAL_CANCODER_CONFIGS = + public static final MagnetSensorConfigs CANCODER_CONFIGS = new MagnetSensorConfigs() .withAbsoluteSensorDiscontinuityPoint(1.0) - .withSensorDirection(SensorDirectionValue.Clockwise_Positive); + .withSensorDirection(SensorDirectionValue.Clockwise_Positive) + .withMagnetOffset(-1 * CANCODER_ABSOLUTE_OFFSET); } - public final class GeneralFlywheel { + public final class Flywheel { + public static final int CAN_ID = 30; + public static final int BEAM_BREAK_ID = 0; // TODO: update later + + public static final int FOLLOWER_CAN_ID = 31; + public static final double FOLLOWER_SIM_MOI = 0.001; + public static final double EPSILON_VELOCITY = 2.5; public static final double MOTOR_TO_MECHANISM = 15.0 / 26.0; public static final String BUS = "rio"; public static final double SIM_MOI = 0.001; - public static final TalonFXConfiguration GENERAL_CONFIG = + public static final TalonFXConfiguration CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() @@ -280,6 +292,12 @@ public final class GeneralFlywheel { new MotorOutputConfigs() .withInverted(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Coast)); + + public static final TalonFXConfiguration FOLLOWER_CONFIG = + CONFIG + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive)); public static final FeedbackConfigs FEEDBACK_CONFIGS = new FeedbackConfigs() @@ -287,188 +305,4 @@ public final class GeneralFlywheel { .withSensorToMechanismRatio(MOTOR_TO_MECHANISM) .withRotorToSensorRatio(1.0); } - - public final class GeneralTurret { - public static final String BUS = "rio"; - - public static final double SIM_MOI = 0.001; - - public static final double MOTOR_TO_CANCODER = (20.0 / 12.0); - public static final double CANCODER_TO_MECHANISM = (135.0 / 20.0); - public static final double MOTOR_TO_MECHANISM = MOTOR_TO_CANCODER * CANCODER_TO_MECHANISM; - - public static final double EPSILON_RADS = Units.degreesToRadians(10.0); - public static final double STATE_TIMEOUT = 1.0; - - /* Configs */ - public static final TalonFXConfiguration GENERAL_CONFIG = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - .withSupplyCurrentLimit(60) - .withStatorCurrentLimit(10) - .withStatorCurrentLimitEnable(true)) - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.Clockwise_Positive) - .withNeutralMode(NeutralModeValue.Brake)); - - public static final SoftwareLimitSwitchConfigs GENERAL_SOFTWARE_LIMIT_CONFIGS = - new SoftwareLimitSwitchConfigs() - .withForwardSoftLimitEnable(false) - .withReverseSoftLimitEnable(false); - - public static final FeedbackConfigs GENERAL_FEEDBACK_CONFIGS = - new FeedbackConfigs() - .withSensorToMechanismRatio(CANCODER_TO_MECHANISM) - .withRotorToSensorRatio(MOTOR_TO_CANCODER) - .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder); - - public static final MagnetSensorConfigs GENERAL_CANCODER_CONFIGS = - new MagnetSensorConfigs() - .withAbsoluteSensorDiscontinuityPoint(1) - .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive); - } - - /* Left */ - public final class ShooterHoodLeft { - public static final int CAN_ID = 34; - - /* CANCoder */ - public static final int CANCODER_ID = 36; - public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; - - public static final FeedbackConfigs FEEDBACK_CONFIGS = - GeneralShooterHood.GENERAL_FEEDBACK_CONFIGS - .withFeedbackRemoteSensorID(CANCODER_ID) - .withFeedbackRotorOffset(-CANCODER_ABSOLUTE_OFFSET); - - public static final MagnetSensorConfigs CANCODER_CONFIGS = - GeneralShooterHood.GENERAL_CANCODER_CONFIGS - .withMagnetOffset(-1 * CANCODER_ABSOLUTE_OFFSET); - } - - public final class FlywheelLeaderLeft { - public static final int CAN_ID = 30; - public static final int BEAM_BREAK_PORT = 0; // TODO: update later - - public static final TalonFXConfiguration CONFIG = GeneralFlywheel.GENERAL_CONFIG; - } - - public final class FlywheelFollowerLeft { - public static final int CAN_ID = 31; - public static final double SIM_MOI = 0.001; - public static final int BEAM_BREAK_ID = 0; - - public static final TalonFXConfiguration CONFIG = - GeneralFlywheel.GENERAL_CONFIG - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive)); - } - - public final class TurretLeft { - public static final int CAN_ID = 20; - - // +x -> forward; +y -> left - public static final Translation3d ROBOT_TO_TURRET = - new Translation3d( - Units.inchesToMeters(4.5), - Units.inchesToMeters(7.505), - Units.inchesToMeters(15.340)); - - /* CANCoder */ - public static final int CANCODER_ID = 22; - public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; - - public static final double MIN_POSITION_ROTATIONS = 0 - Units.radiansToRotations(GeneralTurret.EPSILON_RADS); - public static final double MAX_POSITION_ROTATIONS = 150.0 / 360.0 + Units.radiansToRotations(GeneralTurret.EPSILON_RADS); - - public static final double MIN_POSITION_RADS = Units.rotationsToRadians(MIN_POSITION_ROTATIONS); - public static final double MAX_POSITION_RADS = Units.rotationsToRadians(MAX_POSITION_ROTATIONS); - - public static final SoftwareLimitSwitchConfigs SOFTWARE_LIMIT_CONFIGS = - GeneralTurret.GENERAL_SOFTWARE_LIMIT_CONFIGS - .withReverseSoftLimitThreshold(MIN_POSITION_ROTATIONS) - .withForwardSoftLimitThreshold(MAX_POSITION_ROTATIONS); - - public static final FeedbackConfigs FEEDBACK_CONFIGS = - GeneralTurret.GENERAL_FEEDBACK_CONFIGS - .withFeedbackRemoteSensorID(CANCODER_ID) - .withFeedbackRotorOffset(-CANCODER_ABSOLUTE_OFFSET); - - public static final MagnetSensorConfigs CANCODER_CONFIGS = - GeneralTurret.GENERAL_CANCODER_CONFIGS - .withMagnetOffset(-1 * CANCODER_ABSOLUTE_OFFSET); - } - - /* Right */ - public final class ShooterHoodRight { - public static final int CAN_ID = 35; - - /* CANCoder */ - public static final int CANCODER_ID = 37; - public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; - - public static final FeedbackConfigs FEEDBACK_CONFIGS = - GeneralShooterHood.GENERAL_FEEDBACK_CONFIGS - .withFeedbackRemoteSensorID(CANCODER_ID) - .withFeedbackRotorOffset(-CANCODER_ABSOLUTE_OFFSET); - - public static final MagnetSensorConfigs CANCODER_CONFIGS = - GeneralShooterHood.GENERAL_CANCODER_CONFIGS - .withMagnetOffset(-1 * CANCODER_ABSOLUTE_OFFSET); - } - - public final class FlywheelLeaderRight { - public static final int CAN_ID = 32; - public static final int BEAM_BREAK_PORT = 0; // TODO: update later - - public static final TalonFXConfiguration CONFIG = GeneralFlywheel.GENERAL_CONFIG; - } - - public final class FlywheelFollowerRight { - public static final int CAN_ID = 33; - - public static final TalonFXConfiguration CONFIG = - GeneralFlywheel.GENERAL_CONFIG - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive)); - } - - public final class TurretRight { - public static final int CAN_ID = 21; - - // +x -> forward; +y -> left - public static final Translation3d ROBOT_TO_TURRET = - new Translation3d( - Units.inchesToMeters(4.5), - Units.inchesToMeters(-7.505), - Units.inchesToMeters(15.340)); - - /* CANCoder */ - public static final int CANCODER_ID = 23; - public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; - - public static final double MIN_POSITION_ROTATIONS = -150.0 / 360.0 - Units.radiansToRotations(GeneralTurret.EPSILON_RADS); - public static final double MAX_POSITION_ROTATIONS = 0.0 + Units.radiansToRotations(GeneralTurret.EPSILON_RADS); - - public static final double MIN_POSITION_RADS = Units.rotationsToRadians(MIN_POSITION_ROTATIONS); - public static final double MAX_POSITION_RADS = Units.rotationsToRadians(MAX_POSITION_ROTATIONS); - - public static final SoftwareLimitSwitchConfigs SOFTWARE_LIMIT_CONFIGS = - GeneralTurret.GENERAL_SOFTWARE_LIMIT_CONFIGS - .withReverseSoftLimitThreshold(MIN_POSITION_ROTATIONS) - .withForwardSoftLimitThreshold(MAX_POSITION_ROTATIONS); - - public static final FeedbackConfigs FEEDBACK_CONFIGS = - GeneralTurret.GENERAL_FEEDBACK_CONFIGS - .withFeedbackRemoteSensorID(CANCODER_ID) - .withFeedbackRotorOffset(-CANCODER_ABSOLUTE_OFFSET); - - public static final MagnetSensorConfigs CANCODER_CONFIGS = - GeneralTurret.GENERAL_CANCODER_CONFIGS - .withMagnetOffset(-1 * CANCODER_ABSOLUTE_OFFSET); - } } diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index 1007604..34c972f 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -34,29 +34,15 @@ import org.team5924.frc2026.generated.TunerConstants; import org.team5924.frc2026.subsystems.drive.Drive; import org.team5924.frc2026.subsystems.drive.GyroIO; +import org.team5924.frc2026.subsystems.drive.GyroIOPigeon2; import org.team5924.frc2026.subsystems.drive.GyroIOSim; import org.team5924.frc2026.subsystems.drive.ModuleIO; +import org.team5924.frc2026.subsystems.drive.ModuleIOTalonFX; import org.team5924.frc2026.subsystems.drive.ModuleIOTalonFXSim; -import org.team5924.frc2026.subsystems.vision.Vision; -import org.team5924.frc2026.subsystems.vision.VisionConstants; -import org.team5924.frc2026.subsystems.vision.VisionIO; -import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVision; -import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVisionSim; -import org.team5924.frc2026.subsystems.rollers.hopper.Hopper; -import org.team5924.frc2026.subsystems.rollers.hopper.Hopper.HopperState; -import org.team5924.frc2026.subsystems.rollers.hopper.HopperIO; -import org.team5924.frc2026.subsystems.rollers.indexer.Indexer; -import org.team5924.frc2026.subsystems.rollers.indexer.Indexer.IndexerState; -import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIO; -import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIOTalonFX; -import org.team5924.frc2026.subsystems.rollers.intake.Intake; -import org.team5924.frc2026.subsystems.rollers.intake.Intake.IntakeState; -import org.team5924.frc2026.subsystems.rollers.intake.IntakeIO; -import org.team5924.frc2026.subsystems.rollers.intake.IntakeIOSim; import org.team5924.frc2026.subsystems.flywheel.Flywheel; -import org.team5924.frc2026.subsystems.flywheel.Flywheel.FlywheelState; import org.team5924.frc2026.subsystems.flywheel.FlywheelIO; import org.team5924.frc2026.subsystems.flywheel.FlywheelIOSim; +import org.team5924.frc2026.subsystems.flywheel.FlywheelIOTalonFX; import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivot; import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivot.IntakePivotState; import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivotIO; @@ -65,16 +51,30 @@ import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHoodIO; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHoodIOSim; +import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHoodIOTalonFX; +import org.team5924.frc2026.subsystems.rollers.hopper.Hopper; +import org.team5924.frc2026.subsystems.rollers.hopper.HopperIO; import org.team5924.frc2026.subsystems.rollers.hopper.HopperIOSim; +import org.team5924.frc2026.subsystems.rollers.hopper.HopperIOTalonFX; +import org.team5924.frc2026.subsystems.rollers.indexer.Indexer; +import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIO; import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIOSim; -import org.team5924.frc2026.subsystems.turret.Turret; -import org.team5924.frc2026.subsystems.turret.TurretIO; -import org.team5924.frc2026.subsystems.turret.TurretIOSim; +import org.team5924.frc2026.subsystems.rollers.indexer.IndexerIOTalonFX; +import org.team5924.frc2026.subsystems.rollers.intake.Intake; +import org.team5924.frc2026.subsystems.rollers.intake.Intake.IntakeState; +import org.team5924.frc2026.subsystems.rollers.intake.IntakeIO; +import org.team5924.frc2026.subsystems.rollers.intake.IntakeIOSim; +import org.team5924.frc2026.subsystems.rollers.intake.IntakeIOTalonFX; +import org.team5924.frc2026.subsystems.vision.Vision; +import org.team5924.frc2026.subsystems.vision.VisionConstants; +import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVision; +import org.team5924.frc2026.subsystems.vision.VisionIOPhotonVisionSim; public class RobotContainer { // Subsystems private final Drive drive; private SwerveDriveSimulation driveSimulation = null; + private Vision vision; private final Intake intake; @@ -82,13 +82,18 @@ public class RobotContainer { private final Hopper hopper; private final Indexer indexer; - private final ShooterHood shooterHoodRight; - private final Flywheel flywheelRight; - private final Turret turretRight; + private final ShooterHood shooterHood; + private final Flywheel flywheel; + + // Real/IO implementation + private final boolean realDrive = true; + private final boolean realIntake = true; + private final boolean realIntakePivot = true; + private final boolean realHopper = true; + private final boolean realIndexer = true; - private final ShooterHood shooterHoodLeft; - private final Flywheel flywheelLeft; - private final Turret turretLeft; + private final boolean realShooterHood = true; + private final boolean realFlywheel = true; // Controller private final CommandXboxController driveController = new CommandXboxController(0); @@ -105,13 +110,21 @@ public RobotContainer() { // -------------------------- real -------------------------- drive = - new Drive( - new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - (pose) -> {}); + realDrive + ? new Drive( + new GyroIOPigeon2(), + new ModuleIOTalonFX(TunerConstants.FrontLeft), + new ModuleIOTalonFX(TunerConstants.FrontRight), + new ModuleIOTalonFX(TunerConstants.BackLeft), + new ModuleIOTalonFX(TunerConstants.BackRight), + (pose) -> {}) + : new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + (pose) -> {}); vision = new Vision( @@ -121,41 +134,23 @@ public RobotContainer() { new VisionIOPhotonVision( VisionConstants.FRONT_RIGHT_NAME, VisionConstants.FRONT_RIGHT_TRANSFORM)); - // intake = new Intake(new IntakeIOTalonFX()); - intakePivot = new IntakePivot(new IntakePivotIOTalonFX()); - // hopper = new Hopper(new HopperIOTalonFX()); - // indexer = new Indexer(new IndexerIOTalonFX()); - - // shooterHoodLeft = new ShooterHood(new ShooterHoodIOTalonFX(true), true); - // flywheelLeft = new Flywheel(new FlywheelIOTalonFX(true), true); - // turretLeft = new Turret(new TurretIOTalonFX(true), true); - - // shooterHoodRight = new ShooterHood(new ShooterHoodIOTalonFX(false), false); - // flywheelRight = new Flywheel(new FlywheelIOTalonFX(false), false); - // turretRight = new Turret(new TurretIOTalonFX(false), false); - - // ---------------------------- IO ---------------------------- - // drive = - // new Drive( - // new GyroIOPigeon2(), - // new ModuleIOTalonFX(TunerConstants.FrontLeft), - // new ModuleIOTalonFX(TunerConstants.FrontRight), - // new ModuleIOTalonFX(TunerConstants.BackLeft), - // new ModuleIOTalonFX(TunerConstants.BackRight), - // (pose) -> {}); - - intake = new Intake(new IntakeIO() {}); - // intakePivot = new IntakePivot(new IntakePivotIO() {}); - hopper = new Hopper(new HopperIO() {}); - indexer = new Indexer(new IndexerIO() {}); - - shooterHoodLeft = new ShooterHood(new ShooterHoodIO() {}, true); - flywheelLeft = new Flywheel(new FlywheelIO() {}, true); - turretLeft = new Turret(new TurretIO() {}, true); - - shooterHoodRight = new ShooterHood(new ShooterHoodIO() {}, false); - flywheelRight = new Flywheel(new FlywheelIO() {}, false); - turretRight = new Turret(new TurretIO() {}, false); + intake = realIntake ? new Intake(new IntakeIOTalonFX()) : new Intake(new IntakeIO() {}); + intakePivot = + realIntakePivot + ? new IntakePivot(new IntakePivotIOTalonFX()) + : new IntakePivot(new IntakePivotIO() {}); + hopper = realHopper ? new Hopper(new HopperIOTalonFX()) : new Hopper(new HopperIO() {}); + indexer = + realIndexer ? new Indexer(new IndexerIOTalonFX()) : new Indexer(new IndexerIO() {}); + + shooterHood = + realShooterHood + ? new ShooterHood(new ShooterHoodIOTalonFX()) + : new ShooterHood(new ShooterHoodIO() {}); + flywheel = + realFlywheel + ? new Flywheel(new FlywheelIOTalonFX()) + : new Flywheel(new FlywheelIO() {}); break; case SIM: @@ -191,13 +186,8 @@ public RobotContainer() { hopper = new Hopper(new HopperIOSim()); indexer = new Indexer(new IndexerIOSim()); - shooterHoodLeft = new ShooterHood(new ShooterHoodIOSim(true), true); - flywheelLeft = new Flywheel(new FlywheelIOSim(), true); - turretLeft = new Turret(new TurretIOSim(true), true); - - shooterHoodRight = new ShooterHood(new ShooterHoodIOSim(false), false); - flywheelRight = new Flywheel(new FlywheelIOSim(), false); - turretRight = new Turret(new TurretIOSim(false), false); + shooterHood = new ShooterHood(new ShooterHoodIOSim()); + flywheel = new Flywheel(new FlywheelIOSim()); break; @@ -219,13 +209,8 @@ public RobotContainer() { hopper = new Hopper(new HopperIO() {}); // TODO: Add replay IO implementation indexer = new Indexer(new IndexerIO() {}); - shooterHoodLeft = new ShooterHood(new ShooterHoodIO() {}, true); - flywheelLeft = new Flywheel(new FlywheelIO() {}, true); - turretLeft = new Turret(new TurretIO() {}, true); - - shooterHoodRight = new ShooterHood(new ShooterHoodIO() {}, false); - flywheelRight = new Flywheel(new FlywheelIO() {}, false); - turretRight = new Turret(new TurretIO() {}, false); + shooterHood = new ShooterHood(new ShooterHoodIO() {}); + flywheel = new Flywheel(new FlywheelIO() {}); break; } @@ -349,61 +334,21 @@ private void configureButtonBindings() { hopper.setDefaultCommand( Commands.run(() -> hopper.setGoalState(Hopper.HopperState.ON), hopper)); - // flywheelLeft.setDefaultCommand( - // Commands.runOnce( - // () -> flywheelLeft.setGoalState(FlywheelState.OFF), - // flywheelLeft)); - - // flywheelRight.setDefaultCommand( - // Commands.runOnce( - // () -> flywheelRight.setGoalState(FlywheelState.OFF), - // flywheelRight)); - - // // ### intake pivot down/stow - - intakePivot.setDefaultCommand( - Commands.run( - () -> { - intakePivot.setGoalState(IntakePivotState.MANUAL); - intakePivot.setInput(operatorController.getLeftX()); - }, - intakePivot)); - - operatorController - .leftBumper() - .onTrue( - Commands.runOnce( - () -> { - intakePivot.setGoalState(IntakePivotState.DOWN); - // intake.setGoalState(IntakeState.INTAKE); - }, - intakePivot /*, - intake*/)); - operatorController - .rightBumper() - .onTrue( - Commands.runOnce( - () -> { - intakePivot.setGoalState(IntakePivotState.STOW); - // intake.setGoalState(IntakeState.OFF); - }, - intakePivot /*, - intake*/)); - - // // // ### intake pivot spit + /* ### intake ### */ driveController - .leftTrigger() + .leftBumper() .onTrue( Commands.runOnce( () -> { intakePivot.setGoalState(IntakePivotState.DOWN); - intake.setGoalState(IntakeState.SPITOUT); + intake.setGoalState(IntakeState.INTAKE); }, intakePivot, intake)); + driveController - .leftTrigger() - .onFalse( + .rightBumper() + .onTrue( Commands.runOnce( () -> { intakePivot.setGoalState(IntakePivotState.STOW); @@ -412,268 +357,38 @@ private void configureButtonBindings() { intakePivot, intake)); - // // ### indexing - operatorController - .a() - .onTrue( - Commands.runOnce( - () -> { - // intakePivot.setGoalState(IntakePivotState.SHOOTING); - indexer.setGoalState(Indexer.IndexerState.INDEXING); - }, - // intakePivot, - indexer)); - operatorController - .a() - .onFalse( - Commands.runOnce( - () -> { - // intakePivot.setGoalState(IntakePivotState.STOW); - indexer.setGoalState(Indexer.IndexerState.OFF); - }, - // intakePivot, - indexer)); - - // // ### shooting voltages - // operatorController - // .a() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.B4); - // flywheelLeft.setGoalState(FlywheelState.B4); - // }, - // flywheelLeft, - // flywheelRight)); - - // operatorController - // .b() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.B6); - // flywheelLeft.setGoalState(FlywheelState.B6); - // }, - // flywheelLeft, - // flywheelRight)); - - // operatorController - // .x() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.B8); - // flywheelLeft.setGoalState(FlywheelState.B8); - // }, - // flywheelLeft, - // flywheelRight)); - - // operatorController - // .y() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.B12); - // flywheelLeft.setGoalState(FlywheelState.B12); - // }, - // flywheelLeft, - // flywheelRight)); - - // operatorController - // .rightTrigger() - // .onTrue( - // Commands.runOnce( - // () -> { - // flywheelRight.setGoalState(FlywheelState.OFF); - // flywheelLeft.setGoalState(FlywheelState.OFF); - // }, - // flywheelLeft, - // flywheelRight)); - - // driveController - // .povUp() - // .onTrue( - // DriveCommands.driveFacingHub( - // drive, () -> -driveController.getLeftY(), () -> -driveController.getLeftX())); - - // // AJGAOEIBAWIBAEROBIAEROBIJAROIBHAEBOIAERBIOHAERBHIOAERb - // // operatorController - // // .leftTrigger() - // // .onTrue( - // // AutoScoreCommands.runTrackTargetCommand( - // // turretLeft, shooterHoodLeft, flywheelLeft, true)); - // // operatorController - // // .leftTrigger() - // // .onTrue( - // // AutoScoreCommands.runTrackTargetCommand( - // // turretRight, shooterHoodRight, flywheelRight, false)); - - // // --------------------------------------------------------- - - // // turretLeft.setDefaultCommand( - // // Commands.run( - // // () -> { - // // turretLeft.setGoalState(TurretState.MANUAL); - // // turretLeft.setInput(driveController.getRightX()); - // // }, - // // turretLeft)); + // manual intake + intakePivot.setDefaultCommand( + Commands.run( + () -> { + intakePivot.setGoalState(IntakePivotState.MANUAL); + intakePivot.setInput(operatorController.getLeftX()); + }, + intakePivot)); + // // ### intake pivot spit // driveController // .leftTrigger() // .onTrue( // Commands.runOnce( // () -> { - // turretLeft.setGoalState(TurretState.ZERO); + // intakePivot.setGoalState(IntakePivotState.DOWN); + // intake.setGoalState(IntakeState.SPITOUT); // }, - // turretLeft)); - + // intakePivot, + // intake)); // driveController - // .rightTrigger() - // .onTrue( + // .leftTrigger() + // .onFalse( // Commands.runOnce( // () -> { - // turretLeft.setGoalState(TurretState.NINETY); + // intakePivot.setGoalState(IntakePivotState.STOW); + // intake.setGoalState(IntakeState.OFF); // }, - // turretLeft)); - - // // --------------------------------------------------------- - - // shooterHoodLeft.setDefaultCommand( - // Commands.run( - // () -> { - // shooterHoodLeft.setGoalState(ShooterHoodState.MANUAL); - // shooterHoodLeft.setInput(operatorController.getRightY()); - // }, - // shooterHoodLeft, - // shooterHoodRight)); - - operatorController - .leftTrigger() - .onTrue( - Commands.runOnce( - () -> { - flywheelRight.setGoalState(FlywheelState.OFF); - }, - flywheelRight)); - - operatorController - .rightTrigger() - .onTrue( - Commands.runOnce( - () -> { - flywheelRight.setGoalState(FlywheelState.FAST_LAUNCH); - }, - flywheelRight)); - - operatorController - .y() - .onTrue( - Commands.runOnce( - () -> { - flywheelRight.setGoalState(FlywheelState.SLOW_LAUNCH); - }, - flywheelRight)); - - operatorController - .povDown() - .onTrue( - Commands.runOnce( - () -> { - flywheelRight.updateSetpointState(-5.0); - }, - flywheelRight)); - - operatorController - .povUp() - .onTrue( - Commands.runOnce( - () -> { - flywheelRight.updateSetpointState(5.0); - }, - flywheelRight)); - - // // driveController - // // .rightTrigger() - // // .onTrue( - // // Commands.runOnce( - // // () -> { - // // // shooterHoodLeft.setGoalState(ShooterHoodState.OFF); - // // // shooterHoodRight.setGoalState(ShooterHoodState.OFF); - // // // turretLeft.setGoalState(Turret.TurretState.OFF); - // // // turretRight.setGoalState(Turret.TurretState.OFF); - // // flywheelLeft.setGoalState(FlywheelState.BUMPER_SHOOTING); - // // flywheelRight.setGoalState(FlywheelState.BUMPER_SHOOTING); - // // }, - // // shooterHoodLeft, - // // shooterHoodRight, - // // turretLeft, - // // turretRight, - // // flywheelLeft, - // // flywheelRight)); - - // // operatorController - // // .leftBumper() - // // .onTrue( - // // Commands.runOnce( - // // () -> { - // // hopper.setGoalState(HopperState.ON); - // // indexer.setGoalState(IndexerState.INDEXING); - // // flywheelLeft.setGoalState(FlywheelState.BUMPER_SHOOTING); - // // flywheelRight.setGoalState(FlywheelState.BUMPER_SHOOTING); - // // })); - - // // operatorController - // // .leftBumper() - // // .onFalse( - // // Commands.runOnce( - // // () -> { - // // hopper.setGoalState(HopperState.OFF); - // // indexer.setGoalState(IndexerState.OFF); - // // flywheelLeft.setGoalState(FlywheelState.OFF); - // // flywheelRight.setGoalState(FlywheelState.OFF); - // // })); - - // // operatorController - // // .leftTrigger() - // // .onTrue(Commands.runOnce(() -> intake.setGoalState(IntakeState.INTAKE))); - // // operatorController - // // .leftTrigger() - // // .onFalse(Commands.runOnce(() -> intake.setGoalState(IntakeState.OFF))); - - // // operatorController - // // .rightBumper() - // // .onTrue( - // // Commands.runOnce( - // // () -> { - // // flywheelRight.setGoalState(FlywheelState.MANUAL); - // // })); - - // // operatorController - // // .rightBumper() - // // .onFalse( - // // Commands.runOnce( - // // () -> { - // // flywheelRight.setGoalState(FlywheelState.OFF); - // // })); - - // // shooterHoodRight.setDefaultCommand( - // // ShooterCommands.manualShooterHood(shooterHoodRight, () -> - // // operatorController.getRightY())); - // // shooterHoodLeft.setDefaultCommand( - // // ShooterCommands.manualShooterHood(shooterHoodLeft, () -> - // // operatorController.getRightY())); - // // turretRight.setDefaultCommand( - // // ShooterCommands.manualTurret(turretRight, () -> operatorController.getLeftX())); - // // turretLeft.setDefaultCommand( - // // ShooterCommands.manualTurret(turretLeft, () -> operatorController.getRightX())); - - // // shooterHoodLeft.setDefaultCommand(getAutonomousCommand()); // todo - // // shooterHoodRight.setDefaultCommand(getAutonomousCommand()); // todo - // // turretLeft.setDefaultCommand(getAutonomousCommand()); // todo - // // turretRight.setDefaultCommand(getAutonomousCommand()); // todo - // // flywheelLeft.setDefaultCommand(getAutonomousCommand()); // todo - // // flywheelRight.setDefaultCommand(getAutonomousCommand()); // todo + // intakePivot, + // intake)); + // TODO: shooting, auto shooting } /** diff --git a/src/main/java/org/team5924/frc2026/RobotState.java b/src/main/java/org/team5924/frc2026/RobotState.java index 0ceb4d9..b1accf9 100644 --- a/src/main/java/org/team5924/frc2026/RobotState.java +++ b/src/main/java/org/team5924/frc2026/RobotState.java @@ -22,13 +22,6 @@ import lombok.Getter; import lombok.Setter; import org.littletonrobotics.junction.AutoLogOutput; -import org.team5924.frc2026.subsystems.flywheel.Flywheel.FlywheelState; -import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivot.IntakePivotState; -import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood.ShooterHoodState; -import org.team5924.frc2026.subsystems.rollers.hopper.Hopper.HopperState; -import org.team5924.frc2026.subsystems.rollers.indexer.Indexer.IndexerState; -import org.team5924.frc2026.subsystems.rollers.intake.Intake.IntakeState; -import org.team5924.frc2026.subsystems.turret.Turret.TurretState; @Getter public class RobotState { @@ -73,28 +66,4 @@ public void resetPose(Pose2d pose) { @Getter @Setter private Rotation2d yawPosition = new Rotation2d(); @Getter @Setter private double yawVelocityRadPerSec = 0.0; - - /* ### Intake ### */ - @Getter @Setter private IntakeState intakeState = IntakeState.OFF; - - /* ### Intake ### */ - @Getter @Setter private IntakePivotState intakePivotState = IntakePivotState.OFF; - - /* ### Hopper ### */ - @Getter @Setter private HopperState hopperState = HopperState.OFF; - - /* ### Indexer ### */ - @Getter @Setter private IndexerState indexerState = IndexerState.OFF; - - // Turret - @Getter @Setter private TurretState leftTurretState = TurretState.OFF; - @Getter @Setter private TurretState rightTurretState = TurretState.OFF; - - /*### Flywheel ### */ - @Getter @Setter private FlywheelState leftFlywheelState = FlywheelState.OFF; - @Getter @Setter private FlywheelState rightFlywheelState = FlywheelState.OFF; - - /*### Shooter Hood ### */ - @Getter @Setter private ShooterHoodState leftShooterHoodState = ShooterHoodState.OFF; - @Getter @Setter private ShooterHoodState rightShooterHoodState = ShooterHoodState.OFF; } diff --git a/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java b/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java index 78ae3c5..dc8b8f6 100644 --- a/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/shooter/AutoScoreCommands.java @@ -22,8 +22,6 @@ import org.team5924.frc2026.subsystems.flywheel.Flywheel.FlywheelState; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood.ShooterHoodState; -import org.team5924.frc2026.subsystems.turret.Turret; -import org.team5924.frc2026.subsystems.turret.Turret.TurretState; import org.team5924.frc2026.util.LaunchCalculator; import org.team5924.frc2026.util.LaunchCalculator.LaunchingParameters; @@ -43,23 +41,19 @@ public class AutoScoreCommands { // } private AutoScoreCommands() {} - public static Command runTrackTargetCommand( - Turret turret, ShooterHood shooterHood, Flywheel flywheel, boolean isLeft) { + public static Command runTrackTargetCommand(ShooterHood shooterHood, Flywheel flywheel) { return Commands.run( () -> { - LaunchingParameters launchParams = LaunchCalculator.getInstance().getParameters(isLeft); + LaunchingParameters launchParams = LaunchCalculator.getInstance().getParameters(); - turret.setAutoInput(launchParams.turretRadians()); shooterHood.setAutoInput(launchParams.hoodAngle()); flywheel.setAutoInput(launchParams.flywheelSpeed()); - turret.setGoalState(TurretState.AUTO); shooterHood.setGoalState(ShooterHoodState.AUTO); flywheel.setGoalState(FlywheelState.AUTO); LaunchCalculator.getInstance().clearLaunchingParameters(); }, - turret, shooterHood, flywheel); } diff --git a/src/main/java/org/team5924/frc2026/commands/shooter/ManualShooterCommands.java b/src/main/java/org/team5924/frc2026/commands/shooter/ManualShooterCommands.java index ee50abf..962f264 100644 --- a/src/main/java/org/team5924/frc2026/commands/shooter/ManualShooterCommands.java +++ b/src/main/java/org/team5924/frc2026/commands/shooter/ManualShooterCommands.java @@ -21,8 +21,6 @@ import java.util.function.DoubleSupplier; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood.ShooterHoodState; -import org.team5924.frc2026.subsystems.turret.Turret; -import org.team5924.frc2026.subsystems.turret.Turret.TurretState; public class ManualShooterCommands { @@ -36,13 +34,4 @@ public static Command manualShooterHood(ShooterHood hood, DoubleSupplier inputSu }, hood); } - - public static Command manualTurret(Turret turret, DoubleSupplier inputSupplier) { - return Commands.run( - () -> { - turret.setGoalState(TurretState.MANUAL); - turret.setInput(inputSupplier.getAsDouble()); - }, - turret); - } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java index e595765..1599f4e 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/Flywheel.java @@ -25,13 +25,11 @@ import lombok.Setter; import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; -import org.team5924.frc2026.RobotState; import org.team5924.frc2026.util.EqualsUtil; import org.team5924.frc2026.util.LoggedTunableNumber; public class Flywheel extends SubsystemBase { private final FlywheelIO io; - private final boolean isLeft; private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); @@ -60,41 +58,36 @@ public enum FlywheelState { private final DoubleSupplier velocityRotationsPerSec; } - private final String side; - private final Alert flywheelMotorDisconnected; protected final Alert overheatAlert; @Getter private FlywheelState goalState = FlywheelState.OFF; + private FlywheelState currentState = FlywheelState.OFF; private boolean isAtSetpoint = false; @Setter private double autoInput = 0.0; - public Flywheel(FlywheelIO io, boolean isLeft) { - side = isLeft ? "Left" : "Right"; - this.isLeft = isLeft; + public Flywheel(FlywheelIO io) { this.io = io; this.goalState = FlywheelState.OFF; this.flywheelMotorDisconnected = - new Alert(side + " Flywheel Motor Disconnected!", Alert.AlertType.kWarning); + new Alert("Flywheel Motor Disconnected!", Alert.AlertType.kWarning); - overheatAlert = new Alert(side + " Flywheel motor overheating!", Alert.AlertType.kWarning); + overheatAlert = new Alert("Flywheel motor overheating!", Alert.AlertType.kWarning); } @Override public void periodic() { io.periodicUpdates(); io.updateInputs(inputs); - Logger.processInputs("Flywheel/" + side, inputs); + Logger.processInputs("Flywheel/", inputs); - Logger.recordOutput("Flywheel/" + side + "/GoalState", goalState.toString()); - Logger.recordOutput( - "Flywheel/" + side + "/CurrentState", getRespectiveFlywheelState().toString()); + Logger.recordOutput("Flywheel/GoalState", goalState.toString()); + Logger.recordOutput("Flywheel/CurrentState", currentState.toString()); Logger.recordOutput( - "Flywheel/" + side + "/TargetVelocityRotationsPerSec", getTargetVelocityRotationsPerSec()); - Logger.recordOutput( - "Flywheel/" + side + "/CurrentVelocityRotationsPerSec", inputs.velocityRotationsPerSec); - Logger.recordOutput("Flywheel/" + side + "/IsAtSetpoint", isAtSetpoint); + "Flywheel/TargetVelocityRotationsPerSec", getTargetVelocityRotationsPerSec()); + Logger.recordOutput("Flywheel/CurrentVelocityRotationsPerSec", inputs.velocityRotationsPerSec); + Logger.recordOutput("Flywheel/IsAtSetpoint", isAtSetpoint); flywheelMotorDisconnected.set(!inputs.motorConnected); @@ -123,14 +116,14 @@ public void setGoalState(FlywheelState goalState) { this.goalState = goalState; switch (goalState) { - case MANUAL -> setRespectiveFlywheelState(FlywheelState.MANUAL); + case MANUAL -> currentState = FlywheelState.MANUAL; case MOVING -> DriverStation.reportError( "Flywheel: MOVING is an invalid goal state; it is a transition state!!", null); - case OFF -> setRespectiveFlywheelState(FlywheelState.OFF); - case B4, B6, B8, B12 -> setRespectiveFlywheelState(goalState); - case AUTO -> setRespectiveFlywheelState(FlywheelState.AUTO); - default -> setRespectiveFlywheelState(FlywheelState.MOVING); + case OFF -> currentState = FlywheelState.OFF; + case B4, B6, B8, B12 -> currentState = goalState; + case AUTO -> currentState = FlywheelState.AUTO; + default -> currentState = FlywheelState.MOVING; } } @@ -138,7 +131,7 @@ public boolean isAtSetpoint() { return EqualsUtil.epsilonEquals( getTargetVelocityRotationsPerSec(), inputs.velocityRotationsPerSec, - Constants.GeneralFlywheel.EPSILON_VELOCITY); + Constants.Flywheel.EPSILON_VELOCITY); } private double getTargetVelocityRotationsPerSec() { @@ -150,11 +143,10 @@ private double getTargetVelocityRotationsPerSec() { private void handleCurrentState() { isAtSetpoint = isAtSetpoint(); - switch (getRespectiveFlywheelState()) { + switch (currentState) { case MOVING -> { setVelocity(getTargetVelocityRotationsPerSec()); - if (isAtSetpoint() && goalState != FlywheelState.AUTO) - setRespectiveFlywheelState(goalState); + if (isAtSetpoint() && goalState != FlywheelState.AUTO) currentState = goalState; } case MANUAL -> handleManualState(); case OFF -> stop(); @@ -177,17 +169,6 @@ private void handleManualState() { public void updateSetpointState(double change) { autoInput = inputs.setpointVelocityRotationsPerSec + change; - setGoalState(FlywheelState.SETPOINT); - } - - private void setRespectiveFlywheelState(FlywheelState state) { - if (isLeft) RobotState.getInstance().setLeftFlywheelState(state); - else RobotState.getInstance().setRightFlywheelState(state); - } - - private FlywheelState getRespectiveFlywheelState() { - return isLeft - ? RobotState.getInstance().getLeftFlywheelState() - : RobotState.getInstance().getRightFlywheelState(); + goalState = FlywheelState.SETPOINT; } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java index eced248..c3fa25d 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOSim.java @@ -34,9 +34,7 @@ public FlywheelIOSim() { sim = new DCMotorSim( LinearSystemId.createDCMotorSystem( - gearbox, - Constants.GeneralFlywheel.SIM_MOI, - Constants.GeneralFlywheel.MOTOR_TO_MECHANISM), + gearbox, Constants.Flywheel.SIM_MOI, Constants.Flywheel.MOTOR_TO_MECHANISM), gearbox); } diff --git a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java index 22664cd..4568a83 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/flywheel/FlywheelIOTalonFX.java @@ -36,11 +36,7 @@ import edu.wpi.first.units.measure.Voltage; import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; -import org.team5924.frc2026.Constants.FlywheelFollowerLeft; -import org.team5924.frc2026.Constants.FlywheelFollowerRight; -import org.team5924.frc2026.Constants.FlywheelLeaderLeft; -import org.team5924.frc2026.Constants.FlywheelLeaderRight; -import org.team5924.frc2026.Constants.GeneralFlywheel; +import org.team5924.frc2026.Constants.Flywheel; import org.team5924.frc2026.util.Elastic; import org.team5924.frc2026.util.Elastic.Notification; import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; @@ -60,35 +56,20 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final MotionMagicConfigs motionMagicConfigs; private double setpointVelocityRotationsPerSec; - /* Gains Left */ - private final LoggedTunableNumber kPLeft = new LoggedTunableNumber("Flywheel/Left/kP", 0.5); - private final LoggedTunableNumber kILeft = new LoggedTunableNumber("Flywheel/Left/kI", 0.0); - private final LoggedTunableNumber kDLeft = new LoggedTunableNumber("Flywheel/Left/kD", 0.0); - private final LoggedTunableNumber kSLeft = new LoggedTunableNumber("Flywheel/Left/kS", 0.25); - private final LoggedTunableNumber kVLeft = new LoggedTunableNumber("Flywheel/Left/kV", 0.0705); - private final LoggedTunableNumber kALeft = new LoggedTunableNumber("Flywheel/Left/kA", 0.0); - - private final LoggedTunableNumber motionCruiseVelocityLeft = - new LoggedTunableNumber("Flywheel/Left/MotionCruiseVelocity", 10.0); - private final LoggedTunableNumber motionAccelerationLeft = - new LoggedTunableNumber("Flywheel/Left/MotionAcceleration", 100.0); - private final LoggedTunableNumber motionJerkLeft = - new LoggedTunableNumber("Flywheel/Left/MotionJerk", 0.0); - - /* Gains Right */ - private final LoggedTunableNumber kPRight = new LoggedTunableNumber("Flywheel/Right/kP", 0.5); - private final LoggedTunableNumber kIRight = new LoggedTunableNumber("Flywheel/Right/kI", 0.0); - private final LoggedTunableNumber kDRight = new LoggedTunableNumber("Flywheel/Right/kD", 0.0); - private final LoggedTunableNumber kSRight = new LoggedTunableNumber("Flywheel/Right/kS", 0.25); - private final LoggedTunableNumber kVRight = new LoggedTunableNumber("Flywheel/Right/kV", 0.0705); - private final LoggedTunableNumber kARight = new LoggedTunableNumber("Flywheel/Right/kA", 0.0); - - private final LoggedTunableNumber motionCruiseVelocityRight = - new LoggedTunableNumber("Flywheel/Right/MotionCruiseVelocity", 10.0); - private final LoggedTunableNumber motionAccelerationRight = - new LoggedTunableNumber("Flywheel/Right/MotionAcceleration", 100.0); - private final LoggedTunableNumber motionJerkRight = - new LoggedTunableNumber("Flywheel/Right/MotionJerk", 0.0); + /* Gains */ + private final LoggedTunableNumber kP = new LoggedTunableNumber("Flywheel/kP", 0.5); + private final LoggedTunableNumber kI = new LoggedTunableNumber("Flywheel/kI", 0.0); + private final LoggedTunableNumber kD = new LoggedTunableNumber("Flywheel/kD", 0.0); + private final LoggedTunableNumber kS = new LoggedTunableNumber("Flywheel/kS", 0.25); + private final LoggedTunableNumber kV = new LoggedTunableNumber("Flywheel/kV", 0.0705); + private final LoggedTunableNumber kA = new LoggedTunableNumber("Flywheel/kA", 0.0); + + private final LoggedTunableNumber motionCruiseVelocity = + new LoggedTunableNumber("Flywheel/MotionCruiseVelocity", 10.0); + private final LoggedTunableNumber motionAcceleration = + new LoggedTunableNumber("Flywheel/MotionAcceleration", 100.0); + private final LoggedTunableNumber motionJerk = + new LoggedTunableNumber("Flywheel/MotionJerk", 0.0); /* Status Signals */ private final StatusSignal position; @@ -105,25 +86,10 @@ public class FlywheelIOTalonFX implements FlywheelIO { private final VoltageOut voltageOut; private final MotionMagicVelocityVoltage motionMagicVelocity; - private final Runnable periodicUpdateSlot0; - private final Runnable periodicUpdateMotionMagic; + public FlywheelIOTalonFX() { + leaderTalon = new TalonFX(Flywheel.CAN_ID, new CANBus(Flywheel.BUS)); - private final boolean isLeft; - private final String side; - - public FlywheelIOTalonFX(boolean isLeft) { - this.isLeft = isLeft; - side = isLeft ? "Left" : "Right"; - - leaderTalon = - new TalonFX( - isLeft ? FlywheelLeaderLeft.CAN_ID : FlywheelLeaderRight.CAN_ID, - new CANBus(GeneralFlywheel.BUS)); - - followerTalon = - new TalonFX( - isLeft ? FlywheelFollowerLeft.CAN_ID : FlywheelFollowerRight.CAN_ID, - new CANBus(GeneralFlywheel.BUS)); + followerTalon = new TalonFX(Flywheel.FOLLOWER_CAN_ID, new CANBus(Flywheel.BUS)); leaderConfig = leaderTalon.getConfigurator(); followerConfig = followerTalon.getConfigurator(); @@ -134,42 +100,20 @@ public FlywheelIOTalonFX(boolean isLeft) { motionMagicConfigs = new MotionMagicConfigs(); updateMotionMagicConfigs(); - periodicUpdateSlot0 = - () -> { - updateSlot0Configs(); - - StatusCode statusCode = leaderConfig.apply(slot0Configs); - if (!statusCode.isOK()) { - Logger.recordOutput("Flywheel/" + side + "/UpdateSlot0Report", statusCode); - } - }; - - periodicUpdateMotionMagic = - () -> { - updateMotionMagicConfigs(); - - StatusCode statusCode = leaderConfig.apply(motionMagicConfigs); - if (!statusCode.isOK()) { - Logger.recordOutput("Flywheel/" + side + "/UpdateStatusCodeReport", statusCode); - } - }; - // Apply Configs StatusCode[] statusArray = new StatusCode[10]; - statusArray[0] = - leaderConfig.apply(isLeft ? FlywheelLeaderLeft.CONFIG : FlywheelLeaderRight.CONFIG); + statusArray[0] = leaderConfig.apply(Flywheel.CONFIG); statusArray[1] = leaderConfig.apply(Constants.GENERIC_OPEN_LOOP_RAMPS_CONFIGS); statusArray[2] = leaderConfig.apply(Constants.GENERIC_CLOSED_LOOP_RAMPS_CONFIGS); - statusArray[3] = leaderConfig.apply(GeneralFlywheel.FEEDBACK_CONFIGS); + statusArray[3] = leaderConfig.apply(Flywheel.FEEDBACK_CONFIGS); statusArray[4] = leaderConfig.apply(slot0Configs); statusArray[5] = leaderConfig.apply(motionMagicConfigs); - statusArray[6] = - followerConfig.apply(isLeft ? FlywheelFollowerLeft.CONFIG : FlywheelFollowerRight.CONFIG); + statusArray[6] = followerConfig.apply(Flywheel.FOLLOWER_CONFIG); statusArray[7] = followerConfig.apply(Constants.GENERIC_OPEN_LOOP_RAMPS_CONFIGS); statusArray[8] = followerConfig.apply(Constants.GENERIC_CLOSED_LOOP_RAMPS_CONFIGS); - statusArray[9] = followerConfig.apply(GeneralFlywheel.FEEDBACK_CONFIGS); + statusArray[9] = followerConfig.apply(Flywheel.FEEDBACK_CONFIGS); boolean isErrorPresent = false; for (StatusCode s : statusArray) if (!s.isOK()) isErrorPresent = true; @@ -177,16 +121,11 @@ public FlywheelIOTalonFX(boolean isLeft) { if (isErrorPresent) Elastic.sendNotification( new Notification( - NotificationLevel.WARNING, - side + "Flywheel Configs", - "Error in " + side + " shooter flywheel configs!")); + NotificationLevel.WARNING, "Flywheel Configs", "Error in shooter flywheel configs!")); - Logger.recordOutput("Flywheel/" + side + "/InitConfReport", statusArray); + Logger.recordOutput("Flywheel/InitConfReport", statusArray); - followerTalon.setControl( - isLeft - ? new Follower(FlywheelLeaderLeft.CAN_ID, MotorAlignmentValue.Opposed) - : new Follower(FlywheelLeaderRight.CAN_ID, MotorAlignmentValue.Opposed)); + followerTalon.setControl(new Follower(Flywheel.CAN_ID, MotorAlignmentValue.Opposed)); // Get select status signals and set update frequency position = leaderTalon.getPosition(); @@ -250,55 +189,51 @@ public void periodicUpdates() { } private void updateSlot0Configs() { - if (isLeft) { - slot0Configs.kP = kPLeft.get(); - slot0Configs.kI = kILeft.get(); - slot0Configs.kD = kDLeft.get(); - slot0Configs.kS = kSLeft.get(); - slot0Configs.kV = kVLeft.get(); - slot0Configs.kA = kALeft.get(); - } else { - slot0Configs.kP = kPRight.get(); - slot0Configs.kI = kIRight.get(); - slot0Configs.kD = kDRight.get(); - slot0Configs.kS = kSRight.get(); - slot0Configs.kV = kVRight.get(); - slot0Configs.kA = kARight.get(); - } + slot0Configs.kP = kP.get(); + slot0Configs.kI = kI.get(); + slot0Configs.kD = kD.get(); + slot0Configs.kS = kS.get(); + slot0Configs.kV = kV.get(); + slot0Configs.kA = kA.get(); } private void updateMotionMagicConfigs() { - if (isLeft) { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationLeft.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityLeft.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkLeft.get(); - } else { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationRight.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityRight.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkRight.get(); - } + motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get(); + motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocity.get(); + motionMagicConfigs.MotionMagicJerk = motionJerk.get(); } private void updateLoggedTunableNumbers() { - if (isLeft) { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPLeft, kILeft, kDLeft, kSLeft, kVLeft, kALeft); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationLeft, - motionCruiseVelocityLeft, - motionJerkLeft); - } else { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPRight, kIRight, kDRight, kSRight, kVRight, kARight); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationRight, - motionCruiseVelocityRight, - motionJerkRight); - } + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + updateSlot0Configs(); + + StatusCode statusCode = leaderConfig.apply(slot0Configs); + if (!statusCode.isOK()) { + Logger.recordOutput("Flywheel/UpdateSlot0Report", statusCode); + } + }, + kP, + kI, + kD, + kS, + kV, + kA); + + LoggedTunableNumber.ifChanged( + hashCode() + 1, + () -> { + updateMotionMagicConfigs(); + + StatusCode statusCode = leaderConfig.apply(motionMagicConfigs); + if (!statusCode.isOK()) { + Logger.recordOutput("Flywheel/UpdateStatusCodeReport", statusCode); + } + }, + motionAcceleration, + motionCruiseVelocity, + motionJerk); } @Override diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivot.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivot.java index 526a87d..aff7f93 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivot.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivot.java @@ -26,7 +26,6 @@ import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; import org.team5924.frc2026.FieldState; -import org.team5924.frc2026.RobotState; import org.team5924.frc2026.util.EqualsUtil; import org.team5924.frc2026.util.LoggedTunableNumber; @@ -59,6 +58,7 @@ public enum IntakePivotState { protected final Alert overheatAlert; @Getter private IntakePivotState goalState = IntakePivotState.OFF; + private IntakePivotState currentState = IntakePivotState.OFF; private boolean isAtSetpoint = false; private double lastStateChange = 0.0; private double timeSinceLastStateChange = 0.0; @@ -84,8 +84,7 @@ public void periodic() { handleCurrentState(); Logger.recordOutput("IntakePivot/GoalState", goalState.toString()); - Logger.recordOutput( - "IntakePivot/CurrentState", RobotState.getInstance().getIntakePivotState().toString()); + Logger.recordOutput("IntakePivot/CurrentState", currentState.toString()); Logger.recordOutput("IntakePivot/TargetRads", goalState.rads.getAsDouble()); Logger.recordOutput("IntakePivot/CurrentRads", inputs.positionRads); Logger.recordOutput("IntakePivot/IsAtSetpoint", isAtSetpoint); @@ -111,12 +110,12 @@ public void setGoalState(IntakePivotState goalState) { this.goalState = goalState; switch (goalState) { - case MANUAL -> RobotState.getInstance().setIntakePivotState(IntakePivotState.MANUAL); + case MANUAL -> currentState = IntakePivotState.MANUAL; case MOVING -> DriverStation.reportError( "IntakePivot: MOVING is an invalid goal state; it is a transition state!!", null); - case OFF -> RobotState.getInstance().setIntakePivotState(IntakePivotState.OFF); - default -> RobotState.getInstance().setIntakePivotState(IntakePivotState.MOVING); + case OFF -> currentState = IntakePivotState.OFF; + default -> currentState = IntakePivotState.MOVING; } lastStateChange = FieldState.getInstance().getTime(); @@ -134,10 +133,10 @@ private void handleCurrentState() { timeSinceLastStateChange = FieldState.getInstance().getTime() - lastStateChange; isAtSetpoint = isAtSetpoint(); - switch (RobotState.getInstance().getIntakePivotState()) { + switch (currentState) { case MOVING -> { setPosition(goalState.getRads().getAsDouble()); - if (isAtSetpoint) RobotState.getInstance().setIntakePivotState(goalState); + if (isAtSetpoint) currentState = goalState; } case MANUAL -> handleManualState(); case OFF -> stop(); diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java index 88011da..77b8c66 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHood.java @@ -26,13 +26,11 @@ import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; import org.team5924.frc2026.FieldState; -import org.team5924.frc2026.RobotState; import org.team5924.frc2026.util.EqualsUtil; import org.team5924.frc2026.util.LoggedTunableNumber; public class ShooterHood extends SubsystemBase { private final ShooterHoodIO io; - private final boolean isLeft; private final ShooterHoodIOInputsAutoLogged inputs = new ShooterHoodIOInputsAutoLogged(); @@ -60,14 +58,13 @@ public enum ShooterHoodState { private final DoubleSupplier rads; } - private final String side; - private final Alert motorDisconnectedAlert; protected final Alert overheatAlert; private final Alert notImplementedAlert; private boolean showNotImplementedAlert; - @Getter private ShooterHoodState goalState; + @Getter private ShooterHoodState goalState = ShooterHoodState.OFF; + private ShooterHoodState currentState = ShooterHoodState.OFF; private boolean isAtSetpoint = false; private double lastStateChange = 0.0; private double timeSinceLastStateChange = 0.0; @@ -75,38 +72,33 @@ public enum ShooterHoodState { @Setter private double input; @Setter private double autoInput = 0.0; - public ShooterHood(ShooterHoodIO io, boolean isLeft) { - side = isLeft ? "Left" : "Right"; + public ShooterHood(ShooterHoodIO io) { this.io = io; - this.isLeft = isLeft; goalState = ShooterHoodState.OFF; motorDisconnectedAlert = - new Alert(side + " Shooter Hood Motor Disconnected!", Alert.AlertType.kWarning); - notImplementedAlert = - new Alert(side + " Auto Shooting not yet implemented!", Alert.AlertType.kWarning); - overheatAlert = new Alert(side + " Shooter Hood motor overheating!", Alert.AlertType.kWarning); + new Alert("Shooter Hood Motor Disconnected!", Alert.AlertType.kWarning); + notImplementedAlert = new Alert("Auto Shooting not yet implemented!", Alert.AlertType.kWarning); + overheatAlert = new Alert("Shooter Hood motor overheating!", Alert.AlertType.kWarning); } @Override public void periodic() { io.periodicUpdates(); io.updateInputs(inputs); - Logger.processInputs("ShooterHood/" + side, inputs); + Logger.processInputs("ShooterHood", inputs); motorDisconnectedAlert.set(!inputs.motorConnected); overheatAlert.set(inputs.tempCelsius > Constants.OVERHEAT_THRESHOLD); handleCurrentState(); - Logger.recordOutput("ShooterHood/" + side + "/GoalState", goalState.toString()); - Logger.recordOutput( - "ShooterHood/" + side + "/CurrentState", getRespectiveShooterHoodState().toString()); - Logger.recordOutput("ShooterHood/" + side + "/TargetRads", getTargetRads()); - Logger.recordOutput("ShooterHood/" + side + "/CurrentRads", inputs.positionRads); - Logger.recordOutput("ShooterHood/" + side + "/IsAtSetpoint", isAtSetpoint); - Logger.recordOutput( - "ShooterHood/" + side + "/TimeSinceLastStateChange", timeSinceLastStateChange); + Logger.recordOutput("ShooterHood/GoalState", goalState.toString()); + Logger.recordOutput("ShooterHood/CurrentState", currentState.toString()); + Logger.recordOutput("ShooterHood/TargetRads", getTargetRads()); + Logger.recordOutput("ShooterHood/CurrentRads", inputs.positionRads); + Logger.recordOutput("ShooterHood/IsAtSetpoint", isAtSetpoint); + Logger.recordOutput("ShooterHood/TimeSinceLastStateChange", timeSinceLastStateChange); } public void runVolts(double volts) { @@ -128,15 +120,13 @@ public void setGoalState(ShooterHoodState goalState) { this.goalState = goalState; switch (goalState) { - case MANUAL, AUTO_SHOOTING, NEUTRAL_SHUFFLING, OPPONENT_SHUFFLING -> - setRespectiveShooterHoodState(goalState); + case MANUAL, AUTO_SHOOTING, NEUTRAL_SHUFFLING, OPPONENT_SHUFFLING -> currentState = goalState; case MOVING -> DriverStation.reportError( - side + " Shooter Hood: MOVING is an invalid goal state; it is a transition state!!", - null); - case AUTO -> setRespectiveShooterHoodState(ShooterHoodState.MOVING); - case OFF -> setRespectiveShooterHoodState(ShooterHoodState.OFF); - default -> setRespectiveShooterHoodState(ShooterHoodState.MOVING); + "Shooter Hood: MOVING is an invalid goal state; it is a transition state!!", null); + case AUTO -> currentState = ShooterHoodState.MOVING; + case OFF -> currentState = ShooterHoodState.OFF; + default -> currentState = ShooterHoodState.MOVING; } lastStateChange = FieldState.getInstance().getTime(); @@ -144,10 +134,10 @@ public void setGoalState(ShooterHoodState goalState) { @SuppressWarnings("unused") public boolean isAtSetpoint() { - return (!Constants.GeneralShooterHood.ENABLE_TIMEOUT - || timeSinceLastStateChange > Constants.GeneralShooterHood.STATE_TIMEOUT) + return (!Constants.ShooterHood.ENABLE_TIMEOUT + || timeSinceLastStateChange > Constants.ShooterHood.STATE_TIMEOUT) && EqualsUtil.epsilonEquals( - inputs.positionRads, getTargetRads(), Constants.GeneralShooterHood.EPSILON_RADS); + inputs.positionRads, getTargetRads(), Constants.ShooterHood.EPSILON_RADS); } private double getTargetRads() { @@ -159,11 +149,10 @@ private void handleCurrentState() { isAtSetpoint = isAtSetpoint(); showNotImplementedAlert = false; - switch (getRespectiveShooterHoodState()) { + switch (currentState) { case MOVING -> { setPosition(getTargetRads()); - if (isAtSetpoint() && goalState != ShooterHoodState.AUTO) - setRespectiveShooterHoodState(goalState); + if (isAtSetpoint() && goalState != ShooterHoodState.AUTO) currentState = goalState; } case AUTO_SHOOTING, NEUTRAL_SHUFFLING, OPPONENT_SHUFFLING -> { showNotImplementedAlert = true; // TODO: handle this sometime @@ -188,15 +177,4 @@ private void handleManualState() { runVolts(ShooterHoodState.MANUAL.getRads().getAsDouble() * input); } - - private void setRespectiveShooterHoodState(ShooterHoodState state) { - if (isLeft) RobotState.getInstance().setLeftShooterHoodState(state); - else RobotState.getInstance().setRightShooterHoodState(state); - } - - private ShooterHoodState getRespectiveShooterHoodState() { - return isLeft - ? RobotState.getInstance().getLeftShooterHoodState() - : RobotState.getInstance().getRightShooterHoodState(); - } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOSim.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOSim.java index b04277e..5b37ea9 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOSim.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOSim.java @@ -29,13 +29,11 @@ public class ShooterHoodIOSim implements ShooterHoodIO { private double appliedVoltage = 0.0; private double setpoint = 0.0; - public ShooterHoodIOSim(boolean isLeft) { + public ShooterHoodIOSim() { sim = new DCMotorSim( LinearSystemId.createDCMotorSystem( - gearbox, - Constants.GeneralShooterHood.SIM_MOI, - Constants.GeneralShooterHood.MOTOR_TO_MECHANISM), + gearbox, Constants.ShooterHood.SIM_MOI, Constants.ShooterHood.MOTOR_TO_MECHANISM), gearbox); } @@ -63,9 +61,7 @@ public void runVolts(double volts) { public void setPosition(double rads) { rads = MathUtil.clamp( - rads, - Constants.GeneralShooterHood.MIN_POSITION_RADS, - Constants.GeneralShooterHood.MAX_POSITION_RADS); + rads, Constants.ShooterHood.MIN_POSITION_RADS, Constants.ShooterHood.MAX_POSITION_RADS); setpoint = rads; sim.setAngle(rads); } diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java index 19e6a16..6cf5c7a 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/shooterHood/ShooterHoodIOTalonFX.java @@ -38,9 +38,7 @@ import edu.wpi.first.wpilibj.DriverStation; import org.littletonrobotics.junction.Logger; import org.team5924.frc2026.Constants; -import org.team5924.frc2026.Constants.GeneralShooterHood; -import org.team5924.frc2026.Constants.ShooterHoodLeft; -import org.team5924.frc2026.Constants.ShooterHoodRight; +import org.team5924.frc2026.Constants.ShooterHood; import org.team5924.frc2026.util.Elastic; import org.team5924.frc2026.util.Elastic.Notification; import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; @@ -59,36 +57,20 @@ public class ShooterHoodIOTalonFX implements ShooterHoodIO { private final MotionMagicConfigs motionMagicConfigs; private double setpointRads; - /* Gains Left */ - private final LoggedTunableNumber kPLeft = new LoggedTunableNumber("ShooterHood/Left/kP", 250.0); - private final LoggedTunableNumber kILeft = new LoggedTunableNumber("ShooterHood/Left/kI", 0.0); - private final LoggedTunableNumber kDLeft = new LoggedTunableNumber("ShooterHood/Left/kD", 5.0); - private final LoggedTunableNumber kSLeft = new LoggedTunableNumber("ShooterHood/Left/kS", 0.0); - private final LoggedTunableNumber kVLeft = new LoggedTunableNumber("ShooterHood/Left/kV", 0.0); - private final LoggedTunableNumber kALeft = new LoggedTunableNumber("ShooterHood/Left/kA", 0.0); - - private final LoggedTunableNumber motionCruiseVelocityLeft = - new LoggedTunableNumber("ShooterHood/Left/MotionCruiseVelocity", 90.0); - private final LoggedTunableNumber motionAccelerationLeft = - new LoggedTunableNumber("ShooterHood/Left/MotionAcceleration", 900.0); - private final LoggedTunableNumber motionJerkLeft = - new LoggedTunableNumber("ShooterHood/Left/MotionJerk", 0.0); - - /* Gains Right */ - private final LoggedTunableNumber kPRight = - new LoggedTunableNumber("ShooterHood/Right/kP", 250.0); - private final LoggedTunableNumber kIRight = new LoggedTunableNumber("ShooterHood/Right/kI", 0.0); - private final LoggedTunableNumber kDRight = new LoggedTunableNumber("ShooterHood/Right/kD", 5.0); - private final LoggedTunableNumber kSRight = new LoggedTunableNumber("ShooterHood/Right/kS", 0.0); - private final LoggedTunableNumber kVRight = new LoggedTunableNumber("ShooterHood/Right/kV", 0.0); - private final LoggedTunableNumber kARight = new LoggedTunableNumber("ShooterHood/Right/kA", 0.0); - - private final LoggedTunableNumber motionCruiseVelocityRight = - new LoggedTunableNumber("ShooterHood/Right/MotionCruiseVelocity", 90.0); - private final LoggedTunableNumber motionAccelerationRight = - new LoggedTunableNumber("ShooterHood/Right/MotionAcceleration", 900.0); - private final LoggedTunableNumber motionJerkRight = - new LoggedTunableNumber("ShooterHood/Right/MotionJerk", 0.0); + /* Gains */ + private final LoggedTunableNumber kP = new LoggedTunableNumber("ShooterHood/kP", 250.0); + private final LoggedTunableNumber kI = new LoggedTunableNumber("ShooterHood/kI", 0.0); + private final LoggedTunableNumber kD = new LoggedTunableNumber("ShooterHood/kD", 5.0); + private final LoggedTunableNumber kS = new LoggedTunableNumber("ShooterHood/kS", 0.0); + private final LoggedTunableNumber kV = new LoggedTunableNumber("ShooterHood/kV", 0.0); + private final LoggedTunableNumber kA = new LoggedTunableNumber("ShooterHood/kA", 0.0); + + private final LoggedTunableNumber motionCruiseVelocity = + new LoggedTunableNumber("ShooterHood/MotionCruiseVelocity", 90.0); + private final LoggedTunableNumber motionAcceleration = + new LoggedTunableNumber("ShooterHood/MotionAcceleration", 900.0); + private final LoggedTunableNumber motionJerk = + new LoggedTunableNumber("ShooterHood/MotionJerk", 0.0); /* Status Signals */ private final StatusSignal position; @@ -111,24 +93,9 @@ public class ShooterHoodIOTalonFX implements ShooterHoodIO { private final PositionVoltage positionOut; private final MotionMagicTorqueCurrentFOC motionMagicCurrent; - private final Runnable periodicUpdateSlot0; - private final Runnable periodicUpdateMotionMagic; - - private final boolean isLeft; - private final String side; - - public ShooterHoodIOTalonFX(boolean isLeft) { - this.isLeft = isLeft; - side = isLeft ? "Left" : "Right"; - - talon = - new TalonFX( - isLeft ? ShooterHoodLeft.CAN_ID : ShooterHoodRight.CAN_ID, - new CANBus(GeneralShooterHood.BUS)); - cancoder = - new CANcoder( - isLeft ? ShooterHoodLeft.CANCODER_ID : ShooterHoodRight.CANCODER_ID, - new CANBus(GeneralShooterHood.BUS)); + public ShooterHoodIOTalonFX() { + talon = new TalonFX(ShooterHood.CAN_ID, new CANBus(ShooterHood.BUS)); + cancoder = new CANcoder(ShooterHood.CAN_ID, new CANBus(ShooterHood.BUS)); talonConfig = talon.getConfigurator(); @@ -138,42 +105,17 @@ public ShooterHoodIOTalonFX(boolean isLeft) { motionMagicConfigs = new MotionMagicConfigs(); updateMotionMagicConfigs(); - periodicUpdateSlot0 = - () -> { - updateSlot0Configs(); - - StatusCode statusCode = talon.getConfigurator().apply(slot0Configs); - if (!statusCode.isOK()) { - Logger.recordOutput("ShooterHood/" + side + "/UpdateSlot0Report", statusCode); - } - }; - - periodicUpdateMotionMagic = - () -> { - updateMotionMagicConfigs(); - - StatusCode statusCode = talon.getConfigurator().apply(motionMagicConfigs); - if (!statusCode.isOK()) { - Logger.recordOutput("ShooterHood/" + side + "/UpdateStatusCodeReport", statusCode); - } - }; - // Apply Configs StatusCode[] statusArray = new StatusCode[8]; - statusArray[0] = talonConfig.apply(GeneralShooterHood.CONFIG); + statusArray[0] = talonConfig.apply(ShooterHood.CONFIG); statusArray[1] = talonConfig.apply(slot0Configs); statusArray[2] = talonConfig.apply(motionMagicConfigs); statusArray[3] = talonConfig.apply(Constants.GENERIC_OPEN_LOOP_RAMPS_CONFIGS); statusArray[4] = talonConfig.apply(Constants.GENERIC_CLOSED_LOOP_RAMPS_CONFIGS); - statusArray[5] = talonConfig.apply(GeneralShooterHood.SOFTWARE_LIMIT_CONFIGS); - statusArray[6] = - talonConfig.apply( - isLeft ? ShooterHoodLeft.FEEDBACK_CONFIGS : ShooterHoodRight.FEEDBACK_CONFIGS); - statusArray[7] = - cancoder - .getConfigurator() - .apply(isLeft ? ShooterHoodLeft.CANCODER_CONFIGS : ShooterHoodRight.CANCODER_CONFIGS); + statusArray[5] = talonConfig.apply(ShooterHood.SOFTWARE_LIMIT_CONFIGS); + statusArray[6] = talonConfig.apply(ShooterHood.FEEDBACK_CONFIGS); + statusArray[7] = cancoder.getConfigurator().apply(ShooterHood.CANCODER_CONFIGS); boolean isErrorPresent = false; for (StatusCode s : statusArray) if (!s.isOK()) isErrorPresent = true; @@ -182,10 +124,10 @@ public ShooterHoodIOTalonFX(boolean isLeft) { Elastic.sendNotification( new Notification( NotificationLevel.WARNING, - side + " Shooter Hood Configs", - "Error in applying " + side + " Shooter Hood configs!")); + "Shooter Hood Configs", + "Error in applying Shooter Hood configs!")); - Logger.recordOutput("ShooterHood/" + side + "/InitConfReport", statusArray); + Logger.recordOutput("ShooterHood/InitConfReport", statusArray); // Get select status signals and set update frequency position = talon.getPosition(); @@ -287,55 +229,51 @@ public void periodicUpdates() { } private void updateSlot0Configs() { - if (isLeft) { - slot0Configs.kP = kPLeft.get(); - slot0Configs.kI = kILeft.get(); - slot0Configs.kD = kDLeft.get(); - slot0Configs.kS = kSLeft.get(); - slot0Configs.kV = kVLeft.get(); - slot0Configs.kA = kALeft.get(); - } else { - slot0Configs.kP = kPRight.get(); - slot0Configs.kI = kIRight.get(); - slot0Configs.kD = kDRight.get(); - slot0Configs.kS = kSRight.get(); - slot0Configs.kV = kVRight.get(); - slot0Configs.kA = kARight.get(); - } + slot0Configs.kP = kP.get(); + slot0Configs.kI = kI.get(); + slot0Configs.kD = kD.get(); + slot0Configs.kS = kS.get(); + slot0Configs.kV = kV.get(); + slot0Configs.kA = kA.get(); } private void updateMotionMagicConfigs() { - if (isLeft) { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationLeft.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityLeft.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkLeft.get(); - } else { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationRight.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityRight.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkRight.get(); - } + motionMagicConfigs.MotionMagicAcceleration = motionAcceleration.get(); + motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocity.get(); + motionMagicConfigs.MotionMagicJerk = motionJerk.get(); } private void updateLoggedTunableNumbers() { - if (isLeft) { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPLeft, kILeft, kDLeft, kSLeft, kVLeft, kALeft); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationLeft, - motionCruiseVelocityLeft, - motionJerkLeft); - } else { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPRight, kIRight, kDRight, kSRight, kVRight, kARight); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationRight, - motionCruiseVelocityRight, - motionJerkRight); - } + LoggedTunableNumber.ifChanged( + hashCode(), + () -> { + updateSlot0Configs(); + + StatusCode statusCode = talon.getConfigurator().apply(slot0Configs); + if (!statusCode.isOK()) { + Logger.recordOutput("ShooterHood/UpdateSlot0Report", statusCode); + } + }, + kP, + kI, + kD, + kS, + kV, + kA); + + LoggedTunableNumber.ifChanged( + hashCode() + 1, + () -> { + updateMotionMagicConfigs(); + + StatusCode statusCode = talon.getConfigurator().apply(motionMagicConfigs); + if (!statusCode.isOK()) { + Logger.recordOutput("ShooterHood/UpdateStatusCodeReport", statusCode); + } + }, + motionAcceleration, + motionCruiseVelocity, + motionJerk); } @Override @@ -366,8 +304,7 @@ public void stop() { } private double clampRads(double rads) { - return MathUtil.clamp( - rads, GeneralShooterHood.MIN_POSITION_RADS, GeneralShooterHood.MAX_POSITION_RADS); + return MathUtil.clamp(rads, ShooterHood.MIN_POSITION_RADS, ShooterHood.MAX_POSITION_RADS); } private double radsToMotorPosition(double rads) { diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/Hopper.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/Hopper.java index 9227bfa..955e9e9 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/Hopper.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/hopper/Hopper.java @@ -19,7 +19,6 @@ import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; -import org.team5924.frc2026.RobotState; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller.VoltageState; import org.team5924.frc2026.util.LoggedTunableNumber; @@ -38,6 +37,7 @@ public enum HopperState implements VoltageState { } private HopperState goalState = HopperState.OFF; + private HopperState currentState = HopperState.OFF; public Hopper(HopperIO io) { super("Hopper", io); @@ -45,7 +45,7 @@ public Hopper(HopperIO io) { public void setGoalState(HopperState goalState) { this.goalState = goalState; - RobotState.getInstance().setHopperState(goalState); + currentState = goalState; } @Override diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java index 6554f00..2d21815 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/indexer/Indexer.java @@ -19,7 +19,6 @@ import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; -import org.team5924.frc2026.RobotState; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller.VoltageState; import org.team5924.frc2026.util.LoggedTunableNumber; @@ -36,6 +35,7 @@ public enum IndexerState implements VoltageState { } private IndexerState goalState = IndexerState.OFF; + private IndexerState currentState = IndexerState.OFF; public Indexer(IndexerIO indexerIO) { super("Indexer", indexerIO); @@ -43,7 +43,7 @@ public Indexer(IndexerIO indexerIO) { public void setGoalState(IndexerState goalState) { this.goalState = goalState; - RobotState.getInstance().setIndexerState(goalState); + currentState = goalState; } @Override diff --git a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java index 2aba387..2484660 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java +++ b/src/main/java/org/team5924/frc2026/subsystems/rollers/intake/Intake.java @@ -19,7 +19,6 @@ import java.util.function.DoubleSupplier; import lombok.Getter; import lombok.RequiredArgsConstructor; -import org.team5924.frc2026.RobotState; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller; import org.team5924.frc2026.subsystems.rollers.generic.GenericRoller.VoltageState; import org.team5924.frc2026.util.LoggedTunableNumber; @@ -38,6 +37,7 @@ public enum IntakeState implements VoltageState { } private IntakeState goalState = IntakeState.OFF; + private IntakeState currentState = IntakeState.OFF; public Intake(IntakeIO io) { super("Intake", io); @@ -45,6 +45,6 @@ public Intake(IntakeIO io) { public void setGoalState(IntakeState goalState) { this.goalState = goalState; - RobotState.getInstance().setIntakeState(goalState); + currentState = goalState; } } diff --git a/src/main/java/org/team5924/frc2026/subsystems/turret/Turret.java b/src/main/java/org/team5924/frc2026/subsystems/turret/Turret.java deleted file mode 100644 index ef32c6a..0000000 --- a/src/main/java/org/team5924/frc2026/subsystems/turret/Turret.java +++ /dev/null @@ -1,213 +0,0 @@ -/* - * Turret.java - */ - -/* - * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2026.subsystems.turret; - -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import java.util.function.DoubleSupplier; -import lombok.Getter; -import lombok.Setter; -import org.littletonrobotics.junction.Logger; -import org.team5924.frc2026.Constants; -import org.team5924.frc2026.FieldState; -import org.team5924.frc2026.RobotState; -import org.team5924.frc2026.util.EqualsUtil; -import org.team5924.frc2026.util.LoggedTunableNumber; - -public class Turret extends SubsystemBase { - - private final TurretIO io; - private final TurretIOInputsAutoLogged inputs = new TurretIOInputsAutoLogged(); - - @Setter private double input; - - private final boolean isLeft; - - public enum TurretState { - OFF(() -> 0.0), - MOVING(() -> 0.0), - - // voltage at which the example subsystem motor moves when controlled by the operator - MANUAL(new LoggedTunableNumber("Turret/Volts/OperatorVoltage", 1.0)), - - NINETY(new LoggedTunableNumber("Turret/Volts/Ninety", Math.PI / 2)), - - ZERO(() -> 0.0), - - AUTO(() -> 0.0); - - @Getter private final DoubleSupplier rads; - - TurretState(DoubleSupplier rads) { - this.rads = rads; - } - } - - @Getter private TurretState goalState = TurretState.OFF; - - private final Alert motorDisconnected; - - protected final Alert overheatAlert; - - private double lastStateChange = 0.0; - - private double exceedBoundsDirection; - private boolean shouldContinue; - - private final String side; - - @Setter private double autoInput = 0.0; - - public Turret(TurretIO io, boolean isLeft) { - side = isLeft ? "Left" : "Right"; - this.io = io; - this.goalState = TurretState.OFF; - this.motorDisconnected = - new Alert(side + " Turret Motor Disconnected!", Alert.AlertType.kWarning); - this.isLeft = isLeft; - - overheatAlert = new Alert(side + " Turret motor overheating!", Alert.AlertType.kWarning); - } - - @Override - public void periodic() { - io.periodicUpdates(); - io.updateInputs(inputs); - Logger.processInputs("Turret/" + side, inputs); - - Logger.recordOutput("Turret/" + side + "/GoalState", goalState.toString()); - Logger.recordOutput("Turret/" + side + "/CurrentState", getRespectiveTurretState()); - Logger.recordOutput("Turret/" + side + "/TargetRads", goalState.rads.getAsDouble()); - Logger.recordOutput("Turret/" + side + "/CurrentRads", inputs.positionRads); - Logger.recordOutput("Turret/" + side + "/ExceedBoundsDirection", exceedBoundsDirection); - Logger.recordOutput("Turret/" + side + "/ShouldContinue", shouldContinue); - - handleCurrentState(); - - motorDisconnected.set(!inputs.motorConnected); - - boolean isOverheating = inputs.tempCelsius > Constants.OVERHEAT_THRESHOLD; - overheatAlert.set(isOverheating); - } - - public boolean isAtSetpoint() { - // return RobotState.getTime() - lastStateChange > Constants.GeneralTurret.STATE_TIMEOUT - return EqualsUtil.epsilonEquals( - inputs.setpointRads, inputs.positionRads, Constants.GeneralTurret.EPSILON_RADS); - } - - private void handleCurrentState() { - TurretState currentState = - isLeft - ? RobotState.getInstance().getLeftTurretState() - : RobotState.getInstance().getRightTurretState(); - switch (currentState) { - case MOVING -> { - if (isAtSetpoint() && goalState != TurretState.AUTO) setRespectiveTurretState(goalState); - } - case MANUAL -> handleManualState(); - case OFF -> io.stop(); - case NINETY -> { - io.setPosition(goalState.rads.getAsDouble() * (isLeft ? 1 : -1)); - } - default -> io.setPosition(goalState.rads.getAsDouble()); - } - } - - private void handleManualState() { - if (!goalState.equals(TurretState.MANUAL)) return; - - if (Math.abs(input) <= Constants.JOYSTICK_DEADZONE) { - io.runVolts(0); - return; - } - - tryRunVolts(TurretState.MANUAL.getRads().getAsDouble() * input); - } - - public void tryRunVolts(double volts) { - // if (!(shouldContinue = shouldContinueInDirection(volts, inputs.turretPositionRads))) return; - - io.runVolts(volts); - } - - public boolean shouldContinueInDirection(double volts, double rads) { - double voltDirection = Math.signum(volts); - return (voltDirection != (exceedBoundsDirection = exceedBoundsDirection(rads))); - } - - /** - * @param rads rads - * @return -1 for min bound, 0 for within, 1 for upper bound - */ - public double exceedBoundsDirection(double rads) { - double min = - isLeft ? Constants.TurretLeft.MIN_POSITION_RADS : Constants.TurretRight.MIN_POSITION_RADS; - double max = - isLeft ? Constants.TurretLeft.MAX_POSITION_RADS : Constants.TurretRight.MAX_POSITION_RADS; - if (rads <= min) return -1.0; - if (rads >= max) return 1.0; - return 0.0; - } - - public void setGoalState(TurretState goalState) { - if (this.goalState.equals(goalState)) return; - if (goalState.equals(TurretState.MANUAL) && Math.abs(input) <= Constants.JOYSTICK_DEADZONE) - return; - - if (goalState != TurretState.MOVING) this.goalState = goalState; - switch (goalState) { - case MANUAL: - setRespectiveTurretState(TurretState.MANUAL); - break; - case MOVING: - DriverStation.reportError( - side + " Turret: MOVING is an invalid goal state; it is a transition state!!", null); - break; - case OFF: - setRespectiveTurretState(TurretState.OFF); - io.stop(); - break; - case AUTO: - setRespectiveTurretState(TurretState.MOVING); - io.setPosition(autoInput); - break; - default: - setRespectiveTurretState(TurretState.MOVING); - io.setPosition(goalState.rads.getAsDouble()); - break; - } - - lastStateChange = FieldState.getInstance().getTime(); - } - - private void setRespectiveTurretState(TurretState state) { - if (isLeft) RobotState.getInstance().setLeftTurretState(state); - else RobotState.getInstance().setRightTurretState(state); - } - - private TurretState getRespectiveTurretState() { - return isLeft - ? RobotState.getInstance().getLeftTurretState() - : RobotState.getInstance().getRightTurretState(); - } - - public double getSetpoint() { - return inputs.setpointRads; - } -} diff --git a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIO.java b/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIO.java deleted file mode 100644 index 09f847b..0000000 --- a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIO.java +++ /dev/null @@ -1,79 +0,0 @@ -/* - * TurretIO.java - */ - -/* - * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2026.subsystems.turret; - -import org.littletonrobotics.junction.AutoLog; - -public interface TurretIO { - @AutoLog - public static class TurretIOInputs { - public boolean motorConnected = true; - public double position = 0.0; - public double positionRads = 0.0; - public double positionCancoder = 0.0; - public double velocityRadsPerSec = 0.0; - public double appliedVoltage = 0.0; - public double supplyCurrentAmps = 0.0; - public double torqueCurrentAmps = 0.0; - public double tempCelsius = 0.0; - - public double motionMagicVelocityTarget = 0.0; - public double motionMagicPositionTarget = 0.0; - - public double setpointRads = 0.0; - public double acceleration = 0.0; - - public boolean cancoderConnected = true; - public double cancoderAbsolutePosition = 0.0; - public double cancoderVelocity = 0.0; - public double cancoderSupplyVoltage = 0.0; - public double cancoderPositionRotations = 0.0; - } - - /** - * Updates the inputs object with the latest data from hardware - * - * @param inputs Inputs to update - */ - public default void updateInputs(TurretIOInputs inputs) {} - - /** Updates that are be called in turret periodic */ - public default void periodicUpdates() {} - - /** - * Sets the turret motor to the specified voltage - * - * @param volts number of volts - */ - public default void runVolts(double volts) {} - - /** - * Sets the turret motor to a specified angle - * - * @param rads target angle - */ - public default void setPosition(double rads) {} - - /** Holds the turret motor at a set position */ - public default void holdPosition(double rads) {} - - /** stops the motor */ - default void stop() {} - - /** Sets the turret position to specified rads from center */ - default void setPositionSetpoint(double radiansFromCenter, double radsPerSecond) {} -} diff --git a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOSim.java b/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOSim.java deleted file mode 100644 index d8669e2..0000000 --- a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOSim.java +++ /dev/null @@ -1,79 +0,0 @@ -/* - * TurretIOSim.java - */ - -/* - * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2026.subsystems.turret; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; -import org.team5924.frc2026.Constants; - -public class TurretIOSim implements TurretIO { - private final DCMotorSim sim; - private final DCMotor gearbox = DCMotor.getKrakenX60Foc(1); - private double appliedVoltage = 0.0; - private double setpoint = 0.0; - private final double minPositionRads; - private final double maxPositionRads; - - public TurretIOSim(boolean isLeft) { - sim = - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - gearbox, - Constants.GeneralTurret.MOTOR_TO_MECHANISM, - Constants.GeneralTurret.SIM_MOI), - gearbox); - minPositionRads = - isLeft ? Constants.TurretLeft.MIN_POSITION_RADS : Constants.TurretRight.MIN_POSITION_RADS; - maxPositionRads = - isLeft ? Constants.TurretLeft.MAX_POSITION_RADS : Constants.TurretRight.MAX_POSITION_RADS; - } - - @Override - public void updateInputs(TurretIOInputs inputs) { - if (DriverStation.isDisabled()) runVolts(0.0); - - sim.update(Constants.LOOP_PERIODIC_SECONDS); - inputs.motorConnected = true; - inputs.positionRads = sim.getAngularPositionRad(); - inputs.velocityRadsPerSec = sim.getAngularVelocityRadPerSec(); - inputs.appliedVoltage = appliedVoltage; - inputs.supplyCurrentAmps = sim.getCurrentDrawAmps(); - inputs.setpointRads = setpoint; - inputs.tempCelsius = 25.0; - } - - @Override - public void runVolts(double volts) { - appliedVoltage = MathUtil.clamp(volts, -12.0, 12.0); - sim.setInputVoltage(appliedVoltage); - } - - @Override - public void setPosition(double rads) { - rads = MathUtil.clamp(rads, minPositionRads, maxPositionRads); - setpoint = rads; - sim.setAngle(rads); - } - - @Override - public void stop() { - runVolts(0.0); - } -} diff --git a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOTalonFX.java deleted file mode 100644 index 7595a42..0000000 --- a/src/main/java/org/team5924/frc2026/subsystems/turret/TurretIOTalonFX.java +++ /dev/null @@ -1,386 +0,0 @@ -/* - * TurretIOTalonFX.java - */ - -/* - * Copyright (C) 2025-2026 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2026.subsystems.turret; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.StatusCode; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.MotionMagicConfigs; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfigurator; -import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; -import com.ctre.phoenix6.controls.PositionVoltage; -import com.ctre.phoenix6.controls.TorqueCurrentFOC; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Temperature; -import edu.wpi.first.units.measure.Voltage; -import edu.wpi.first.wpilibj.DriverStation; -import org.littletonrobotics.junction.Logger; -import org.team5924.frc2026.Constants; -import org.team5924.frc2026.Constants.GeneralTurret; -import org.team5924.frc2026.Constants.TurretLeft; -import org.team5924.frc2026.Constants.TurretRight; -import org.team5924.frc2026.util.Elastic; -import org.team5924.frc2026.util.Elastic.Notification; -import org.team5924.frc2026.util.Elastic.Notification.NotificationLevel; -import org.team5924.frc2026.util.LoggedTunableNumber; - -public class TurretIOTalonFX implements TurretIO { - /* Hardware */ - private final TalonFX turretTalon; - private final CANcoder turretCANCoder; - - /* Configurators */ - private TalonFXConfigurator turretTalonConfig; - - /* Configs */ - private final Slot0Configs slot0Configs; - private final MotionMagicConfigs motionMagicConfigs; - private double setpointRads; - - /* Gains Left */ - private final LoggedTunableNumber kPLeft = new LoggedTunableNumber("Turret/Left/kP", 25.0); - private final LoggedTunableNumber kILeft = new LoggedTunableNumber("Turret/Left/kI", 0.0); - private final LoggedTunableNumber kDLeft = new LoggedTunableNumber("Turret/Left/kD", 1.00); - private final LoggedTunableNumber kSLeft = new LoggedTunableNumber("Turret/Left/kS", 0.0); - private final LoggedTunableNumber kVLeft = new LoggedTunableNumber("Turret/Left/kV", 0.0); - private final LoggedTunableNumber kALeft = new LoggedTunableNumber("Turret/Left/kA", 0.00); - - private final LoggedTunableNumber motionCruiseVelocityLeft = - new LoggedTunableNumber("Turret/Left/MotionCruiseVelocity", 10.0); - private final LoggedTunableNumber motionAccelerationLeft = - new LoggedTunableNumber("Turret/Left/MotionAcceleration", 20.0); - private final LoggedTunableNumber motionJerkLeft = - new LoggedTunableNumber("Turret/Left/MotionJerk", 0.0); - - /* Gains Right */ - private final LoggedTunableNumber kPRight = new LoggedTunableNumber("Turret/Right/kP", 25.0); - private final LoggedTunableNumber kIRight = new LoggedTunableNumber("Turret/Right/kI", 0.0); - private final LoggedTunableNumber kDRight = new LoggedTunableNumber("Turret/Right/kD", 1.00); - private final LoggedTunableNumber kSRight = new LoggedTunableNumber("Turret/Right/kS", 0.0); - private final LoggedTunableNumber kVRight = new LoggedTunableNumber("Turret/Right/kV", 0.0); - private final LoggedTunableNumber kARight = new LoggedTunableNumber("Turret/Right/kA", 0.00); - - private final LoggedTunableNumber motionCruiseVelocityRight = - new LoggedTunableNumber("Turret/Right/MotionCruiseVelocity", 10.0); - private final LoggedTunableNumber motionAccelerationRight = - new LoggedTunableNumber("Turret/Right/MotionAcceleration", 100.0); - private final LoggedTunableNumber motionJerkRight = - new LoggedTunableNumber("Turret/Right/MotionJerk", 0.0); - - /* Status Signals */ - private final StatusSignal turretPosition; - private final StatusSignal turretVelocity; - private final StatusSignal turretAppliedVoltage; - private final StatusSignal turretSupplyCurrent; - private final StatusSignal turretTorqueCurrent; - private final StatusSignal turretTempCelsius; - - private final StatusSignal cancoderAbsolutePosition; - private final StatusSignal cancoderVelocity; - private final StatusSignal cancoderSupplyVoltage; - private final StatusSignal cancoderPositionRotations; - - private final StatusSignal closedLoopReferenceSlope; - private double prevClosedLoopReferenceSlope = 0.0; - private double prevReferenceSlopeTimestamp = 0.0; - - private final TorqueCurrentFOC currentOut; - private final PositionVoltage positionOut; - private final MotionMagicTorqueCurrentFOC motionMagicCurrent; - - private final double minPositionRads; - private final double maxPositionRads; - - private final Runnable periodicUpdateSlot0; - private final Runnable periodicUpdateMotionMagic; - - private final boolean isLeft; - private final String side; - - public TurretIOTalonFX(boolean isLeft) { - this.isLeft = isLeft; - side = isLeft ? "Left" : "Right"; - minPositionRads = isLeft ? TurretLeft.MIN_POSITION_RADS : TurretRight.MIN_POSITION_RADS; - maxPositionRads = isLeft ? TurretLeft.MAX_POSITION_RADS : TurretRight.MAX_POSITION_RADS; - - turretTalon = - new TalonFX(isLeft ? TurretLeft.CAN_ID : TurretRight.CAN_ID, new CANBus(GeneralTurret.BUS)); - turretCANCoder = - new CANcoder( - isLeft ? TurretLeft.CANCODER_ID : TurretRight.CANCODER_ID, - new CANBus(GeneralTurret.BUS)); - - turretTalonConfig = turretTalon.getConfigurator(); - - slot0Configs = new Slot0Configs(); - updateSlot0Configs(); - - motionMagicConfigs = new MotionMagicConfigs(); - updateMotionMagicConfigs(); - - periodicUpdateSlot0 = - () -> { - updateSlot0Configs(); - - StatusCode statusCode = turretTalonConfig.apply(slot0Configs); - if (!statusCode.isOK()) { - Logger.recordOutput("Turret/" + side + "/UpdateSlot0Report", statusCode); - } - }; - - periodicUpdateMotionMagic = - () -> { - updateMotionMagicConfigs(); - - StatusCode statusCode = turretTalonConfig.apply(motionMagicConfigs); - if (!statusCode.isOK()) { - Logger.recordOutput("Turret/" + side + "/UpdateStatusCodeReport", statusCode); - } - }; - - // Apply Configs - StatusCode[] statusArray = new StatusCode[8]; - - statusArray[0] = turretTalonConfig.apply(GeneralTurret.GENERAL_CONFIG); - statusArray[1] = turretTalonConfig.apply(Constants.GENERIC_OPEN_LOOP_RAMPS_CONFIGS); - statusArray[2] = turretTalonConfig.apply(Constants.GENERIC_CLOSED_LOOP_RAMPS_CONFIGS); - statusArray[3] = - turretTalonConfig.apply( - isLeft ? TurretLeft.SOFTWARE_LIMIT_CONFIGS : TurretRight.SOFTWARE_LIMIT_CONFIGS); - statusArray[4] = - turretTalonConfig.apply( - isLeft ? TurretLeft.FEEDBACK_CONFIGS : TurretRight.FEEDBACK_CONFIGS); - statusArray[5] = turretTalonConfig.apply(motionMagicConfigs); - statusArray[6] = turretTalonConfig.apply(slot0Configs); - statusArray[7] = - turretCANCoder - .getConfigurator() - .apply(isLeft ? TurretLeft.CANCODER_CONFIGS : TurretRight.CANCODER_CONFIGS); - - boolean isErrorPresent = false; - for (StatusCode s : statusArray) if (!s.isOK()) isErrorPresent = true; - - if (isErrorPresent) - Elastic.sendNotification( - new Notification( - NotificationLevel.WARNING, - side + " Turret Configs", - "Error in applying " + side + " Turret configs!")); - - Logger.recordOutput("Turret/" + side + "/InitConfReport", statusArray); - - // Get select status signals and set update frequency - turretPosition = turretTalon.getPosition(); - turretVelocity = turretTalon.getVelocity(); - turretAppliedVoltage = turretTalon.getMotorVoltage(); - turretSupplyCurrent = turretTalon.getSupplyCurrent(); - turretTorqueCurrent = turretTalon.getTorqueCurrent(); - turretTempCelsius = turretTalon.getDeviceTemp(); - - cancoderAbsolutePosition = turretCANCoder.getAbsolutePosition(); - cancoderVelocity = turretCANCoder.getVelocity(); - cancoderSupplyVoltage = turretCANCoder.getSupplyVoltage(); - cancoderPositionRotations = turretCANCoder.getPosition(); - - closedLoopReferenceSlope = turretTalon.getClosedLoopReferenceSlope(); - - BaseStatusSignal.setUpdateFrequencyForAll( - 100.0, - turretPosition, - turretVelocity, - turretAppliedVoltage, - turretSupplyCurrent, - turretTorqueCurrent, - turretTempCelsius, - cancoderAbsolutePosition, - cancoderVelocity, - cancoderSupplyVoltage, - cancoderPositionRotations, - closedLoopReferenceSlope); - - currentOut = new TorqueCurrentFOC(0.0); - positionOut = new PositionVoltage(0).withUpdateFreqHz(0.0).withEnableFOC(true).withSlot(0); - motionMagicCurrent = new MotionMagicTorqueCurrentFOC(0.0).withSlot(0); - - BaseStatusSignal.waitForAll(0.5, turretPosition, cancoderAbsolutePosition); - - turretTalon.setPosition(0.0); - turretCANCoder.setPosition(0.0); - } - - @Override - public void updateInputs(TurretIOInputs inputs) { - inputs.motorConnected = - BaseStatusSignal.refreshAll( - turretPosition, - turretVelocity, - turretAppliedVoltage, - turretSupplyCurrent, - turretTorqueCurrent, - turretTempCelsius, - closedLoopReferenceSlope) - .isOK(); - - inputs.cancoderConnected = - BaseStatusSignal.refreshAll( - cancoderAbsolutePosition, - cancoderVelocity, - cancoderSupplyVoltage, - cancoderPositionRotations) - .isOK(); - - inputs.position = - BaseStatusSignal.getLatencyCompensatedValueAsDouble(turretPosition, turretVelocity); - inputs.positionRads = Units.rotationsToRadians(inputs.position); - - inputs.velocityRadsPerSec = Units.rotationsToRadians(turretVelocity.getValueAsDouble()); - inputs.appliedVoltage = turretAppliedVoltage.getValueAsDouble(); - inputs.supplyCurrentAmps = turretSupplyCurrent.getValueAsDouble(); - inputs.torqueCurrentAmps = turretTorqueCurrent.getValueAsDouble(); - inputs.tempCelsius = turretTempCelsius.getValueAsDouble(); - - inputs.motionMagicVelocityTarget = - motorPositionToRads(turretTalon.getClosedLoopReferenceSlope().getValueAsDouble()); - inputs.motionMagicPositionTarget = - motorPositionToRads(turretTalon.getClosedLoopReference().getValueAsDouble()); - - inputs.setpointRads = setpointRads; - - double currentTime = closedLoopReferenceSlope.getTimestamp().getTime(); - double timeDiff = currentTime - prevReferenceSlopeTimestamp; - if (timeDiff > 0.0) { - inputs.acceleration = - (inputs.motionMagicVelocityTarget - prevClosedLoopReferenceSlope) / timeDiff; - } - prevClosedLoopReferenceSlope = inputs.motionMagicVelocityTarget; - prevReferenceSlopeTimestamp = currentTime; - - inputs.cancoderAbsolutePosition = cancoderAbsolutePosition.getValueAsDouble(); - inputs.cancoderVelocity = cancoderVelocity.getValueAsDouble(); - inputs.cancoderSupplyVoltage = cancoderSupplyVoltage.getValueAsDouble(); - inputs.cancoderPositionRotations = cancoderPositionRotations.getValueAsDouble(); - - inputs.positionCancoder = inputs.cancoderPositionRotations; - } - - @Override - public void periodicUpdates() { - updateLoggedTunableNumbers(); - } - - private void updateSlot0Configs() { - if (isLeft) { - slot0Configs.kP = kPLeft.get(); - slot0Configs.kI = kILeft.get(); - slot0Configs.kD = kDLeft.get(); - slot0Configs.kS = kSLeft.get(); - slot0Configs.kV = kVLeft.get(); - slot0Configs.kA = kALeft.get(); - } else { - slot0Configs.kP = kPRight.get(); - slot0Configs.kI = kIRight.get(); - slot0Configs.kD = kDRight.get(); - slot0Configs.kS = kSRight.get(); - slot0Configs.kV = kVRight.get(); - slot0Configs.kA = kARight.get(); - } - } - - private void updateMotionMagicConfigs() { - if (isLeft) { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationLeft.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityLeft.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkLeft.get(); - } else { - motionMagicConfigs.MotionMagicAcceleration = motionAccelerationRight.get(); - motionMagicConfigs.MotionMagicCruiseVelocity = motionCruiseVelocityRight.get(); - motionMagicConfigs.MotionMagicJerk = motionJerkRight.get(); - } - } - - private void updateLoggedTunableNumbers() { - if (isLeft) { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPLeft, kILeft, kDLeft, kSLeft, kVLeft, kALeft); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationLeft, - motionCruiseVelocityLeft, - motionJerkLeft); - } else { - LoggedTunableNumber.ifChanged( - hashCode(), periodicUpdateSlot0, kPRight, kIRight, kDRight, kSRight, kVRight, kARight); - LoggedTunableNumber.ifChanged( - hashCode() + 1, - periodicUpdateMotionMagic, - motionAccelerationRight, - motionCruiseVelocityRight, - motionJerkRight); - } - } - - @Override - public void runVolts(double volts) { - turretTalon.setControl(currentOut.withOutput(40 * volts)); - } - - @Override - public void setPosition(double rads) { - if (!DriverStation.isEnabled()) { - stop(); - return; - } - - setpointRads = clampRads(rads); - turretTalon.setControl(motionMagicCurrent.withPosition(radsToMotorPosition(setpointRads))); - } - - @Override - public void holdPosition(double rads) { - turretTalon.setControl(positionOut.withPosition(radsToMotorPosition(rads))); - } - - @Override - public void stop() { - turretTalon.stopMotor(); - } - - private double clampRads(double rads) { - return MathUtil.clamp(rads, minPositionRads, maxPositionRads); - } - - private double radsToMotorPosition(double rads) { - while (rads < minPositionRads) rads += 2 * Math.PI; - while (rads > maxPositionRads) rads -= 2 * Math.PI; - - if (rads < minPositionRads) turretPosition.getValueAsDouble(); - return Units.radiansToRotations(rads); - } - - private double motorPositionToRads(double motorPosition) { - return Units.rotationsToRadians(motorPosition); - } -} diff --git a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionConstants.java b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionConstants.java index c3385a3..9bace1f 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/vision/VisionConstants.java +++ b/src/main/java/org/team5924/frc2026/subsystems/vision/VisionConstants.java @@ -35,16 +35,16 @@ public class VisionConstants { public static final Transform3d FRONT_LEFT_TRANSFORM = new Transform3d( new Translation3d( - Units.inchesToMeters(10.402), - Units.inchesToMeters(12.042), - Units.inchesToMeters(8.401)), + Units.inchesToMeters(10.402), + Units.inchesToMeters(12.042), + Units.inchesToMeters(8.401)), new Rotation3d(0.0, Units.degreesToRadians(55.0), Units.degreesToRadians(-15.0))); public static final Transform3d FRONT_RIGHT_TRANSFORM = new Transform3d( new Translation3d( - Units.inchesToMeters(10.402), - Units.inchesToMeters(-12.042), - Units.inchesToMeters(8.711)), + Units.inchesToMeters(10.402), + Units.inchesToMeters(-12.042), + Units.inchesToMeters(8.711)), new Rotation3d(0.0, Units.degreesToRadians(55.0), Units.degreesToRadians(15.0))); // public static final ArrayList IGNORE_IDS = diff --git a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java index 91c2452..a1851dd 100644 --- a/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java +++ b/src/main/java/org/team5924/frc2026/util/LaunchCalculator.java @@ -22,7 +22,6 @@ import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; @@ -65,7 +64,6 @@ public record LaunchingParameters( double hoodAngle, double hoodVelocity, double flywheelSpeed, - double turretRadians, double distance, double distanceNoLookahead, double timeOfFlight, @@ -111,13 +109,13 @@ public record LaunchingParameters( new LaunchPreset( new LoggedTunableNumber( "LaunchCalculator/Presets/HoodMin/HoodAngle", - Units.radiansToDegrees(Constants.GeneralShooterHood.MIN_POSITION_RADS)), + Units.radiansToDegrees(Constants.ShooterHood.MIN_POSITION_RADS)), new LoggedTunableNumber("LaunchCalculator/Presets/HoodMin/FlywheelSpeed", 50)); public static final LaunchPreset hoodMaxPreset = new LaunchPreset( new LoggedTunableNumber( "LaunchCalculator/Presets/HoodMax/HoodAngle", - Units.radiansToDegrees(Constants.GeneralShooterHood.MAX_POSITION_RADS)), + Units.radiansToDegrees(Constants.ShooterHood.MAX_POSITION_RADS)), new LoggedTunableNumber("LaunchCalculator/Presets/HoodMax/FlywheelSpeed", 50)); public static final LoggedTunableNumber passingIdleSpeed = @@ -273,8 +271,7 @@ public static double getMaxTimeOfFlight() { return timeOfFlightMap.get(maxDistance); } - public LaunchingParameters getParameters(boolean isLeft) { - Transform3d robotToLauncher = isLeft ? robotToLauncherLeft : robotToLauncherRight; + public LaunchingParameters getParameters() { boolean passing = AllianceFlipUtil.applyX(RobotState.getInstance().getEstimatedPose().getX()) > FieldConstants.LinesVertical.hubCenter; @@ -331,12 +328,10 @@ public LaunchingParameters getParameters(boolean isLeft) { lookaheadLauncherToTargetDistance = target.getDistance(lookaheadPose.getTranslation()); } - double turretRads = getTurretAngle(lookaheadPose, target, isLeft).getRadians(); - // Account for launcher being off center Pose2d lookaheadRobotPose = lookaheadPose.transformBy(robotToLauncher.toTransform2d().inverse()); - Rotation2d driveAngle = getDriveAngleWithLauncherOffset(lookaheadRobotPose, target, isLeft); + Rotation2d driveAngle = getDriveAngleWithLauncherOffset(lookaheadRobotPose, target); // Calculate remaining parameters double hoodAngle = @@ -377,7 +372,6 @@ public LaunchingParameters getParameters(boolean isLeft) { hoodAngle + Units.degreesToRadians(hoodAngleOffsetDeg), hoodVelocity, flywheelVelocity, - turretRads, lookaheadLauncherToTargetDistance, launcherToTargetDistance, timeOfFlight, @@ -392,18 +386,8 @@ public LaunchingParameters getParameters(boolean isLeft) { return latestParameters; } - private static Rotation2d getTurretAngle( - Pose2d turretPose, Translation2d target, boolean isLeft) { - - Rotation2d turretToHub = target.minus(turretPose.getTranslation()).getAngle(); - Rotation2d turretRads = turretPose.getRotation().minus(turretToHub); - return turretRads; - } - private static Rotation2d getDriveAngleWithLauncherOffset( - Pose2d robotPose, Translation2d target, boolean isLeft) { - Transform3d robotToLauncher = isLeft ? robotToLauncherLeft : robotToLauncherRight; - + Pose2d robotPose, Translation2d target) { Rotation2d fieldToHubAngle = target.minus(robotPose.getTranslation()).getAngle(); Rotation2d hubAngle = new Rotation2d( @@ -461,8 +445,7 @@ public Translation2d getPassingTarget() { * @param forceBlue Always use the blue hub target * @return The target pose for the aimed robot. */ - public static Pose2d getStationaryAimedPose( - Translation2d robotTranslation, boolean forceBlue, boolean isLeft) { + public static Pose2d getStationaryAimedPose(Translation2d robotTranslation, boolean forceBlue) { // Calculate target Translation2d target = FieldConstants.Hub.topCenterPoint.toTranslation2d(); if (!forceBlue) { @@ -470,8 +453,7 @@ public static Pose2d getStationaryAimedPose( } return new Pose2d( - robotTranslation, - getDriveAngleWithLauncherOffset(robotTranslation.toPose2d(), target, isLeft)); + robotTranslation, getDriveAngleWithLauncherOffset(robotTranslation.toPose2d(), target)); } /** Adjusts the hood angle offset up or down the specified amount. */ diff --git a/src/main/java/org/team5924/frc2026/util/LauncherConstants.java b/src/main/java/org/team5924/frc2026/util/LauncherConstants.java index 4879bf4..7a1b170 100644 --- a/src/main/java/org/team5924/frc2026/util/LauncherConstants.java +++ b/src/main/java/org/team5924/frc2026/util/LauncherConstants.java @@ -18,16 +18,12 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; -import org.team5924.frc2026.Constants; +import edu.wpi.first.math.geometry.Translation3d; public class LauncherConstants { - public static Transform3d robotToLauncherLeft = + public static Transform3d robotToLauncher = new Transform3d( - Constants.TurretLeft.ROBOT_TO_TURRET, new Rotation3d(0.0, 0.0, Math.PI)); // TODO: update - - public static Transform3d robotToLauncherRight = - new Transform3d( - Constants.TurretRight.ROBOT_TO_TURRET, new Rotation3d(0.0, 0.0, Math.PI)); // TODO: update + new Translation3d(0.0, 0.0, 0.0), new Rotation3d(0.0, 0.0, Math.PI)); // TODO: update private LauncherConstants() {} }