diff --git a/src/main/java/org/team5924/frc2026/Constants.java b/src/main/java/org/team5924/frc2026/Constants.java index 1e24839..465428c 100644 --- a/src/main/java/org/team5924/frc2026/Constants.java +++ b/src/main/java/org/team5924/frc2026/Constants.java @@ -143,13 +143,13 @@ public final class IntakePivot { public static final boolean ENABLE_TIMEOUT = false; /** how far the intake pivot physically rotates */ - public static final double MIN_POSITION_MULTI = - Units.radiansToRotations(IntakePivotState.DOWN.getRads().getAsDouble() - EPSILON_RADS); - public static final double MAX_POSITION_MULTI = - Units.radiansToRotations(IntakePivotState.STOW.getRads().getAsDouble() + EPSILON_RADS); + public static final double MIN_POSITION_ROTATIONS = + Units.radiansToRotations(IntakePivotState.DOWN.getRads().getAsDouble()); + public static final double MAX_POSITION_ROTATIONS = + Units.radiansToRotations(IntakePivotState.STOW.getRads().getAsDouble()); - public static final double MIN_POSITION_RADS = Units.rotationsToRadians(MIN_POSITION_MULTI); - public static final double MAX_POSITION_RADS = Units.rotationsToRadians(MAX_POSITION_MULTI); + public static final double MIN_POSITION_RADS = Units.rotationsToRadians(MIN_POSITION_ROTATIONS); + public static final double MAX_POSITION_RADS = Units.rotationsToRadians(MAX_POSITION_ROTATIONS); /* Configs */ public static final TalonFXConfiguration CONFIG = @@ -167,10 +167,10 @@ public final class IntakePivot { public static final SoftwareLimitSwitchConfigs SOFTWARE_LIMIT_CONFIGS = new SoftwareLimitSwitchConfigs() - .withReverseSoftLimitThreshold(MIN_POSITION_MULTI) - .withForwardSoftLimitThreshold(MAX_POSITION_MULTI) - .withForwardSoftLimitEnable(false) - .withReverseSoftLimitEnable(false); + .withReverseSoftLimitThreshold(MIN_POSITION_ROTATIONS) + .withForwardSoftLimitThreshold(MAX_POSITION_ROTATIONS) + .withForwardSoftLimitEnable(true) + .withReverseSoftLimitEnable(true); public static final FeedbackConfigs FEEDBACK_CONFIGS = new FeedbackConfigs() @@ -225,11 +225,11 @@ public final class GeneralShooterHood { public static final double STATE_TIMEOUT = 5.0; public static final boolean ENABLE_TIMEOUT = false; - public static final double MIN_POSITION_MULTI = 0.0 - Units.radiansToRotations(EPSILON_RADS); - public static final double MAX_POSITION_MULTI = 33.0 / 360.0 + Units.radiansToRotations(EPSILON_RADS); + public static final double MIN_POSITION_ROTATIONS = 0.0 - Units.radiansToRotations(EPSILON_RADS); + public static final double MAX_POSITION_ROTATIONS = 33.0 / 360.0 + Units.radiansToRotations(EPSILON_RADS); - public static final double MIN_POSITION_RADS = Units.rotationsToRadians(MIN_POSITION_MULTI); - public static final double MAX_POSITION_RADS = Units.rotationsToRadians(MAX_POSITION_MULTI); + public static final double MIN_POSITION_RADS = Units.rotationsToRadians(MIN_POSITION_ROTATIONS); + public static final double MAX_POSITION_RADS = Units.rotationsToRadians(MAX_POSITION_ROTATIONS); /* Configs */ public static final TalonFXConfiguration CONFIG = @@ -248,8 +248,8 @@ public final class GeneralShooterHood { public static final SoftwareLimitSwitchConfigs SOFTWARE_LIMIT_CONFIGS = new SoftwareLimitSwitchConfigs() - .withReverseSoftLimitThreshold(MIN_POSITION_MULTI) - .withForwardSoftLimitThreshold(MAX_POSITION_MULTI) + .withReverseSoftLimitThreshold(MIN_POSITION_ROTATIONS) + .withForwardSoftLimitThreshold(MAX_POSITION_ROTATIONS) .withForwardSoftLimitEnable(false) .withReverseSoftLimitEnable(false); @@ -382,16 +382,16 @@ public final class TurretLeft { public static final int CANCODER_ID = 22; public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; - public static final double MIN_POSITION_MULTI = 0 - Units.radiansToRotations(GeneralTurret.EPSILON_RADS); - public static final double MAX_POSITION_MULTI = 150.0 / 360.0 + Units.radiansToRotations(GeneralTurret.EPSILON_RADS); + 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_MULTI); - public static final double MAX_POSITION_RADS = Units.rotationsToRadians(MAX_POSITION_MULTI); + 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_MULTI) - .withForwardSoftLimitThreshold(MAX_POSITION_MULTI); + .withReverseSoftLimitThreshold(MIN_POSITION_ROTATIONS) + .withForwardSoftLimitThreshold(MAX_POSITION_ROTATIONS); public static final FeedbackConfigs FEEDBACK_CONFIGS = GeneralTurret.GENERAL_FEEDBACK_CONFIGS @@ -452,16 +452,16 @@ public final class TurretRight { public static final int CANCODER_ID = 23; public static final double CANCODER_ABSOLUTE_OFFSET = 0.0; - public static final double MIN_POSITION_MULTI = -150.0 / 360.0 - Units.radiansToRotations(GeneralTurret.EPSILON_RADS); - public static final double MAX_POSITION_MULTI = 0.0 + Units.radiansToRotations(GeneralTurret.EPSILON_RADS); + 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_MULTI); - public static final double MAX_POSITION_RADS = Units.rotationsToRadians(MAX_POSITION_MULTI); + 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_MULTI) - .withForwardSoftLimitThreshold(MAX_POSITION_MULTI); + .withReverseSoftLimitThreshold(MIN_POSITION_ROTATIONS) + .withForwardSoftLimitThreshold(MAX_POSITION_ROTATIONS); public static final FeedbackConfigs FEEDBACK_CONFIGS = GeneralTurret.GENERAL_FEEDBACK_CONFIGS diff --git a/src/main/java/org/team5924/frc2026/RobotContainer.java b/src/main/java/org/team5924/frc2026/RobotContainer.java index ff59d52..51519fc 100644 --- a/src/main/java/org/team5924/frc2026/RobotContainer.java +++ b/src/main/java/org/team5924/frc2026/RobotContainer.java @@ -41,10 +41,11 @@ 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; import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivotIOSim; +import org.team5924.frc2026.subsystems.pivots.intakePivot.IntakePivotIOTalonFX; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHood; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHoodIO; import org.team5924.frc2026.subsystems.pivots.shooterHood.ShooterHoodIOSim; @@ -54,8 +55,8 @@ 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.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.turret.Turret; @@ -103,16 +104,16 @@ public RobotContainer() { (pose) -> {}); // intake = new Intake(new IntakeIOTalonFX()); - // intakePivot = new IntakePivot(new IntakePivotIOTalonFX()); + intakePivot = new IntakePivot(new IntakePivotIOTalonFX()); // hopper = new Hopper(new HopperIOTalonFX()); - indexer = new Indexer(new IndexerIOTalonFX()); + // 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); + // flywheelRight = new Flywheel(new FlywheelIOTalonFX(false), false); // turretRight = new Turret(new TurretIOTalonFX(false), false); // ---------------------------- IO ---------------------------- @@ -126,16 +127,16 @@ public RobotContainer() { // (pose) -> {}); intake = new Intake(new IntakeIO() {}); - intakePivot = new IntakePivot(new IntakePivotIO() {}); + // intakePivot = new IntakePivot(new IntakePivotIO() {}); hopper = new Hopper(new HopperIO() {}); - // indexer = new Indexer(new IndexerIO() {}); + 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); + flywheelRight = new Flywheel(new FlywheelIO() {}, false); turretRight = new Turret(new TurretIO() {}, false); break; @@ -325,52 +326,57 @@ private void configureButtonBindings() { // flywheelRight)); // // ### intake pivot down/stow - // 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*/)); + + 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 - // // driveController - // // .leftTrigger() - // // .onTrue( - // // Commands.runOnce( - // // () -> { - // // intakePivot.setGoalState(IntakePivotState.DOWN); - // // hopper.setGoalState(Hopper.HopperState.SPIT); - // // intake.setGoalState(IntakeState.SPITOUT); - // // }, - // // intakePivot, - // // hopper, - // // intake)); - // // driveController - // // .leftTrigger() - // // .onFalse( - // // Commands.runOnce( - // // () -> { - // // intakePivot.setGoalState(IntakePivotState.STOW); - // // hopper.setGoalState(Hopper.HopperState.ON); - // // intake.setGoalState(IntakeState.OFF); - // // }, - // // intakePivot, - // // hopper, - // // intake)); + driveController + .leftTrigger() + .onTrue( + Commands.runOnce( + () -> { + intakePivot.setGoalState(IntakePivotState.DOWN); + intake.setGoalState(IntakeState.SPITOUT); + }, + intakePivot, + intake)); + driveController + .leftTrigger() + .onFalse( + Commands.runOnce( + () -> { + intakePivot.setGoalState(IntakePivotState.STOW); + intake.setGoalState(IntakeState.OFF); + }, + intakePivot, + intake)); // // ### indexing operatorController 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 44d4feb..526a87d 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 @@ -44,7 +44,8 @@ public enum IntakePivotState { MOVING(() -> 0.0), DOWN(new LoggedTunableNumber("IntakePivot/DownRads", 0)), - STOW(new LoggedTunableNumber("IntakePivot/StowRads", 2.45)), + STOW(new LoggedTunableNumber("IntakePivot/StowRads", 2.05)), + PHYSICAL_STOW(() -> 2.1), SHOOTING(new LoggedTunableNumber("IntakePivot/ShootingRads", 1.5)), // current at which the example subsystem motor moves when controlled by the operator diff --git a/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivotIOTalonFX.java b/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivotIOTalonFX.java index b86013c..66dda21 100644 --- a/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivotIOTalonFX.java +++ b/src/main/java/org/team5924/frc2026/subsystems/pivots/intakePivot/IntakePivotIOTalonFX.java @@ -57,10 +57,10 @@ public class IntakePivotIOTalonFX implements IntakePivotIO { private double setpointRads; /* Gains */ - private final LoggedTunableNumber kP = new LoggedTunableNumber("IntakePivot/kP", 70.0); + private final LoggedTunableNumber kP = new LoggedTunableNumber("IntakePivot/kP", 40.0); private final LoggedTunableNumber kI = new LoggedTunableNumber("IntakePivot/kI", 0.0); - private final LoggedTunableNumber kD = new LoggedTunableNumber("IntakePivot/kD", 0.0); - private final LoggedTunableNumber kS = new LoggedTunableNumber("IntakePivot/kS", 1.5); + private final LoggedTunableNumber kD = new LoggedTunableNumber("IntakePivot/kD", 0.5); + private final LoggedTunableNumber kS = new LoggedTunableNumber("IntakePivot/kS", .2); private final LoggedTunableNumber kV = new LoggedTunableNumber("IntakePivot/kV", 0.0); private final LoggedTunableNumber kG = new LoggedTunableNumber("IntakePivot/kG", 2.8); private final LoggedTunableNumber kA = new LoggedTunableNumber("IntakePivot/kA", 0.0); @@ -154,7 +154,8 @@ public IntakePivotIOTalonFX() { motionMagicCurrent = new MotionMagicTorqueCurrentFOC(0.0).withSlot(0).withUpdateFreqHz(100); // assuming intake pivot starts stowed - talon.setPosition(Units.radiansToRotations(IntakePivotState.STOW.getRads().getAsDouble())); + talon.setPosition( + Units.radiansToRotations(IntakePivotState.PHYSICAL_STOW.getRads().getAsDouble())); } @Override