From c899e1aefad31bc914210b75b7b83292d3e5bcae Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Sun, 15 Mar 2026 15:48:22 -0400 Subject: [PATCH 1/2] Button mappings & Tuning 3/15/26 --- src/main/java/frc/robot/Constants.java | 6 +-- src/main/java/frc/robot/RobotContainer.java | 51 ++++++++++++------- .../java/frc/robot/subsystems/Hopper.java | 14 ++--- .../java/frc/robot/subsystems/Intake.java | 29 ++++++----- .../java/frc/robot/subsystems/Shooter.java | 18 +++---- 5 files changed, 64 insertions(+), 54 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index e5d0fd8..e3dc2c9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -479,7 +479,7 @@ public class IntakeFlywheelConstants { public static final Angle MIN_ANGLE = Rotations.of(0.0); public static final Angle MAX_ANGLE = Rotations.of(1); public static final Angle STARTING_ANGLE = Rotations.of(0.0); - public static final double PICKUP_SPEED = 0.0; + public static final double PICKUP_SPEED = 30.0; public static final Distance WHEEL_RADIUS = Meters.of(0.05); public static final Translation3d OFFSET = Translation3d.kZero; public static final MomentOfInertia MOI = KilogramSquareMeters.of(0.0028125); @@ -615,7 +615,7 @@ public static VisionSystemSim getSystemSim() { public class IntakePivotConstants { public static final String NAME = "Intake"; - public static final Angle PICKUP_ANGLE = Degrees.of(130.0); + public static final Angle PICKUP_ANGLE = Degrees.of(123.0); public static final Angle STOW_ANGLE = Degrees.of(0.0); public static final Angle TOLERANCE = Degrees.of(1.0); @@ -628,7 +628,7 @@ public class IntakePivotConstants { private static final double SENSOR_TO_MECHANISM = 17.35; public static final Angle MIN_ANGLE = Degrees.of(0.0); - public static final Angle MAX_ANGLE = Degrees.of(130.0); + public static final Angle MAX_ANGLE = Degrees.of(126.0); public static final Angle STARTING_ANGLE = Degrees.of(0.0); public static final Distance ARM_LENGTH = Foot.one(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f031ec6..2fdef22 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -73,7 +73,7 @@ public class RobotContainer { // Subsystems public final Drive drive; - private final Hopper hopper; + public final Hopper hopper; private final Shooter shooter; public final Intake intake; // private final BallCounter ballCounter; @@ -366,28 +366,41 @@ private void configureButtonBindings() { controller.leftBumper().whileTrue(shooter.runFlywheel(RotationsPerSecond.of(100))); controller.leftBumper().onFalse(shooter.runFlywheel(RotationsPerSecond.of(0))); - // Intake + Spindexer + Tower - controller - .rightBumper() - .whileTrue( - Commands.parallel( - // intake.runRollers(RotationsPerSecond.of(30)), - hopper.runSpindexer(15), shooter.runTower(RotationsPerSecond.of(30)))); + // Intake Rollers 11 Motor: 9 Intake + + // ALPHA KATIE REQUESTS INTAKE RIGHT TRIGGER, SHOOT LEFT TRIGGER, AUTO ALIGN "A" + + // Intake + controller.rightTrigger().whileTrue(intake.intake()); + controller.rightTrigger().onFalse(intake.stowAndStopRollers()); + // Shoot controller - .rightBumper() - .onFalse( + .leftTrigger() + .whileTrue( Commands.parallel( - intake.runRollers(RotationsPerSecond.of(0)), - hopper.runSpindexer(0), - shooter.runTower(RotationsPerSecond.of(0)))); - - // Intake Rollers 11 Motor: 9 Intake + shooter.prepareToShoot(RotationsPerSecond.of(100), RotationsPerSecond.of(30)), + hopper.runSpindexer(RotationsPerSecond.of(15)) + ) + ); - controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(22.5)))); + controller - .a() - .onFalse(new RunCommand(() -> intake._rollerIO.runVoltage(Volts.of(0.0)), intake)); + .leftTrigger() + .onFalse( + Commands.parallel( + shooter.prepareToShoot(RotationsPerSecond.of(0), RotationsPerSecond.of(0)), + hopper.runSpindexer(RotationsPerSecond.of(0)) + ) + ); + + // Align + // controller.a().onTrue(); + + // controller.a().whileTrue((intake.runRollers(RotationsPerSecond.of(22.5)))); + // controller + // .a() + // .onFalse(new RunCommand(() -> intake._rollerIO.runVoltage(Volts.of(0.0)), intake)); // Spindexer 1:1 // controller.x().whileTrue(hopper.runSpindexer(18)); @@ -399,7 +412,7 @@ private void configureButtonBindings() { // controller.y().onFalse(shooter.runTower(RotationsPerSecond.of(0))); // controller.b().onFalse(shooter.setHoodAngle(ShooterRotaryConstants.STARTING_ANGLE.magnitude())); - controller.povLeft().onTrue(shooter.calibrateHood()); + // controller.povLeft().onTrue(shooter.calibrateHood()); // controller.povLeft().onTrue(shooter.setHoodAngle(10)); // controller.povDown().onTrue(shooter.setHoodAngle(15)); // controller.povRight().onTrue(shooter.setHoodAngle(20)); diff --git a/src/main/java/frc/robot/subsystems/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper.java index 46c4b82..d23f508 100644 --- a/src/main/java/frc/robot/subsystems/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper.java @@ -30,16 +30,12 @@ public void setGoal(double position) { PIDSlot.SLOT_0); } - // Velocity of Rollers - public void setVelocity(double velocity) { - AngularVelocity angVelo = RotationsPerSecond.of(velocity); + public Command runSpindexer(AngularVelocity velocity) { + return Commands.runOnce(() -> { - _io.runVelocity(angVelo, HopperConstants.ACCELERATION, PIDSlot.SLOT_0); - targetVelocity = angVelo; - } - - public Command runSpindexer(double velocity) { - return Commands.runOnce(() -> setVelocity(velocity), this); + _io.runVelocity(velocity, HopperConstants.ACCELERATION, PIDSlot.SLOT_0); + targetVelocity = velocity; + }, this); } @Override diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 0ff58c8..5eef395 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -76,14 +76,14 @@ public void stop() { } public Command runRollers(AngularVelocity velocity) { - return Commands.run(() -> setVelocity(velocity), this); + return Commands.run(() -> setVelocity(velocity)); } public Command intake() { - return Commands.sequence( - Commands.run( - () -> setVelocity(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED))), - setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)); + return Commands.parallel( + runRollers(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED)), + setPivotAngle(IntakePivotConstants.PICKUP_ANGLE) + ); } public boolean isIntendedAngle() { @@ -98,21 +98,22 @@ public boolean canIntake() { } public Command stowAndStopRollers() { - return Commands.sequence( - Commands.run( - () -> setVelocity(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED))), - setStowAngle(IntakePivotConstants.STOW_ANGLE)); + return Commands.parallel( + runRollers(RotationsPerSecond.of(0.0)), + setStowAngle() + ); } - private Command setStowAngle(Angle stowAngle) { + private Command setStowAngle() { return this.runOnce( () -> _pivotIO.runPosition( - stowAngle, + IntakePivotConstants.STOW_ANGLE, IntakePivotConstants.CRUISE_VELOCITY, IntakePivotConstants.ACCELERATION, IntakePivotConstants.JERK, - PIDSlot.SLOT_0)); + PIDSlot.SLOT_0) + ); } public void tunePivotPosition() { @@ -132,7 +133,7 @@ public void periodic() { Logger.recordOutput("Intake/TargetSpeed", targetSpeed); _pivotIO.periodic(); - // Logger.recordOutput("Intake/TargetPivot", null); + Logger.recordOutput("Intake/TargetPivot", _pivotIO.getPosition().in(Degrees)); Logger.recordOutput( "3DField/1_Intake", new Pose3d( @@ -144,6 +145,8 @@ public void periodic() { new Translation3d(Math.sin(_pivotIO.getPosition().in(Radians) * 0.1055), 0, 0), new Rotation3d(0, 0, 0))); + + // _pivotIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*0.25)); //--- Tests the pivot } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index c7ca63c..7b84f26 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -29,6 +29,8 @@ import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.ShooterRotaryConstants; import frc.robot.Robot; +import frc.robot.RobotContainer; + import org.littletonrobotics.junction.Logger; public class Shooter extends SubsystemBase { @@ -181,17 +183,13 @@ public Command calibrateHood() { // _hood.setEncoderPosition(Angle.ofBaseUnits(0, Degrees)))); // } - public Command shoot(AngularVelocity velocity) { + public Command prepareToShoot(AngularVelocity flywheelVelocity, AngularVelocity towerVelocity) { // Prepare targets - return Commands.sequence( - // Set and wait in parallel for both hood and flywheel - Commands.parallel( - Commands.run(() -> setFlywheelVelocity(velocity)).until(this::flyAtVelocity), - Commands.run(() -> setHoodAngle(hoodAngle)).until(this::hoodAtAngle)), - // feed once ready - Commands.runOnce(() -> runFeeder(FeederConstants.FEED_SPEED)), - // stop flywheel when finished - Commands.runOnce(() -> setFlywheelVelocity(RotationsPerSecond.of(0.0)))); + return Commands.parallel( + Commands.run(() -> setFlywheelVelocity(flywheelVelocity)).until(this::flyAtVelocity), + Commands.run(() -> setHoodAngle(hoodAngle)).until(this::hoodAtAngle), + runTower(towerVelocity) + ); } public Command runFlywheel(AngularVelocity velocity) { From 08ab4a6f1cf3fb22f47783c021a4a92ec7fb57a4 Mon Sep 17 00:00:00 2001 From: Ghost <145223182+GhostR406@users.noreply.github.com> Date: Sun, 15 Mar 2026 15:52:11 -0400 Subject: [PATCH 2/2] applied spotlessly --- src/main/java/frc/robot/RobotContainer.java | 18 ++++++------------ src/main/java/frc/robot/subsystems/Hopper.java | 11 ++++++----- src/main/java/frc/robot/subsystems/Intake.java | 13 +++---------- .../java/frc/robot/subsystems/Shooter.java | 9 +++------ 4 files changed, 18 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2fdef22..b9c96a1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,6 @@ import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.BaseStatusSignal; import com.pathplanner.lib.auto.AutoBuilder; @@ -380,19 +379,14 @@ private void configureButtonBindings() { .whileTrue( Commands.parallel( shooter.prepareToShoot(RotationsPerSecond.of(100), RotationsPerSecond.of(30)), - hopper.runSpindexer(RotationsPerSecond.of(15)) - ) - ); + hopper.runSpindexer(RotationsPerSecond.of(15)))); - controller - .leftTrigger() - .onFalse( - Commands.parallel( - shooter.prepareToShoot(RotationsPerSecond.of(0), RotationsPerSecond.of(0)), - hopper.runSpindexer(RotationsPerSecond.of(0)) - ) - ); + .leftTrigger() + .onFalse( + Commands.parallel( + shooter.prepareToShoot(RotationsPerSecond.of(0), RotationsPerSecond.of(0)), + hopper.runSpindexer(RotationsPerSecond.of(0)))); // Align // controller.a().onTrue(); diff --git a/src/main/java/frc/robot/subsystems/Hopper.java b/src/main/java/frc/robot/subsystems/Hopper.java index d23f508..aaf8913 100644 --- a/src/main/java/frc/robot/subsystems/Hopper.java +++ b/src/main/java/frc/robot/subsystems/Hopper.java @@ -31,11 +31,12 @@ public void setGoal(double position) { } public Command runSpindexer(AngularVelocity velocity) { - return Commands.runOnce(() -> { - - _io.runVelocity(velocity, HopperConstants.ACCELERATION, PIDSlot.SLOT_0); - targetVelocity = velocity; - }, this); + return Commands.runOnce( + () -> { + _io.runVelocity(velocity, HopperConstants.ACCELERATION, PIDSlot.SLOT_0); + targetVelocity = velocity; + }, + this); } @Override diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 5eef395..5c60f22 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -82,8 +82,7 @@ public Command runRollers(AngularVelocity velocity) { public Command intake() { return Commands.parallel( runRollers(RotationsPerSecond.of(IntakeFlywheelConstants.PICKUP_SPEED)), - setPivotAngle(IntakePivotConstants.PICKUP_ANGLE) - ); + setPivotAngle(IntakePivotConstants.PICKUP_ANGLE)); } public boolean isIntendedAngle() { @@ -98,10 +97,7 @@ public boolean canIntake() { } public Command stowAndStopRollers() { - return Commands.parallel( - runRollers(RotationsPerSecond.of(0.0)), - setStowAngle() - ); + return Commands.parallel(runRollers(RotationsPerSecond.of(0.0)), setStowAngle()); } private Command setStowAngle() { @@ -112,8 +108,7 @@ private Command setStowAngle() { IntakePivotConstants.CRUISE_VELOCITY, IntakePivotConstants.ACCELERATION, IntakePivotConstants.JERK, - PIDSlot.SLOT_0) - ); + PIDSlot.SLOT_0)); } public void tunePivotPosition() { @@ -145,8 +140,6 @@ public void periodic() { new Translation3d(Math.sin(_pivotIO.getPosition().in(Radians) * 0.1055), 0, 0), new Rotation3d(0, 0, 0))); - - // _pivotIO.runVoltage(Volts.of(Math.sin(Timer.getFPGATimestamp())*0.25)); //--- Tests the pivot } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 7b84f26..4cb36ee 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -29,8 +29,6 @@ import frc.robot.Constants.ShooterConstants; import frc.robot.Constants.ShooterRotaryConstants; import frc.robot.Robot; -import frc.robot.RobotContainer; - import org.littletonrobotics.junction.Logger; public class Shooter extends SubsystemBase { @@ -186,10 +184,9 @@ public Command calibrateHood() { public Command prepareToShoot(AngularVelocity flywheelVelocity, AngularVelocity towerVelocity) { // Prepare targets return Commands.parallel( - Commands.run(() -> setFlywheelVelocity(flywheelVelocity)).until(this::flyAtVelocity), - Commands.run(() -> setHoodAngle(hoodAngle)).until(this::hoodAtAngle), - runTower(towerVelocity) - ); + Commands.run(() -> setFlywheelVelocity(flywheelVelocity)).until(this::flyAtVelocity), + Commands.run(() -> setHoodAngle(hoodAngle)).until(this::hoodAtAngle), + runTower(towerVelocity)); } public Command runFlywheel(AngularVelocity velocity) {