From f6fbf3cd54fb74a52059733cc2fa7bf88b75d7c0 Mon Sep 17 00:00:00 2001 From: neelabhb-sudo Date: Tue, 23 Jun 2026 16:19:58 -0700 Subject: [PATCH 01/16] Changed Intake extension and Intake roller constants Changed intake extension constants and Intake roller constants to fit the new intake. Some things need to be changed. Removed intake fast and made all intake roller speeds 12 volts --- src/main/java/frc/robot/auto/Autonomous.java | 22 +++---- .../IntakeExtensionConstants.java | 8 +-- .../intakerollers/IntakeRollers.java | 62 +++++++++++-------- .../intakerollers/IntakeRollersConstants.java | 7 ++- 4 files changed, 56 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/robot/auto/Autonomous.java b/src/main/java/frc/robot/auto/Autonomous.java index 0c425275..4dc3e20a 100644 --- a/src/main/java/frc/robot/auto/Autonomous.java +++ b/src/main/java/frc/robot/auto/Autonomous.java @@ -76,12 +76,12 @@ private Command singleNeutralCycle(boolean rightSide) { .getSwerveDrive() .followPath("left_neutral.0", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral.1", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot.getSwerveDrive().followPath("left_neutral.2", rightSide), shootFuel.shoot()); } @@ -105,29 +105,29 @@ private Command doubleNeutralCycle(boolean rightSide) { .getSwerveDrive() .followPath("left_neutral.0", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral.1", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot.getSwerveDrive().followPath("left_neutral.2", rightSide), shootFuel.shootAllFuelStationary().withTimeout(20.0 - 13.2), robot .getSwerveDrive() .followPath("left_neutral.3", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral.4", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral.5", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), shootFuel.shoot()); } @@ -150,24 +150,24 @@ public Command bump(boolean rightSide) { .getSwerveDrive() .followPath("left_neutral_bump.0", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral_bump.1", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot.getSwerveDrive().followPath("left_neutral_bump.2", rightSide), shootFuel.shootAllFuelStationary().withTimeout(5), robot .getSwerveDrive() .followPath("left_neutral_bump.3", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral_bump.4", rightSide) .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()), + robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot.getSwerveDrive().followPath("left_neutral_bump.5", rightSide), shootFuel.shoot()) .withTimeout(20); diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java index 1092cc12..9a76344e 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java @@ -27,16 +27,16 @@ public final class IntakeExtensionConstants { public static final int MOTOR_CAN_ID = 40; public static final DCMotor MOTOR_PHYSICS = DCMotor.getKrakenX44Foc(1); - public static final Mass MOVING_MASS = Pounds.of(12); + public static final Mass MOVING_MASS = Pounds.of(14.5); //Subject to Change public static final Distance MAX_POSITION = Inches.of(10.1); public static final Distance MIN_POSITION = Inches.of(0); public static final Distance RETRACT_POSITION = Inches.of(3); - public static final Angle ANGLE = Degrees.of(-18.5); - public static final Distance PINION_RADIUS = Inches.of(0.75); + public static final Angle ANGLE = Degrees.of(-18.6196233); + public static final Distance PINION_RADIUS = Inches.of(1.5); public static final int CANDI_DEVICE_ID = 20; public static final Distance POSITION_TOLERANCE = Inches.of(0.125); public static final Voltage FINE_CONTROL_VOLTAGE = Volts.of(1.0); - public static final double GEAR_RATIO = 7.2; + public static final double GEAR_RATIO = 7; public static final TalonFXConfiguration MOTOR_CONFIGURATION = new TalonFXConfiguration() diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java index 0a43bf0a..ec0718cd 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java @@ -4,14 +4,17 @@ import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.CoastOut; import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.team6962.lib.logging.CurrentDrawLogger; import com.team6962.lib.phoenix.StatusUtil; + import dev.doglog.DogLog; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; @@ -23,27 +26,44 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeRollers extends SubsystemBase { + private TalonFX IntakeRollerMotor1; + private TalonFX IntakeRollerMotor2; private TalonFX intakeMotor; private StatusSignal velocitySignal; private StatusSignal statorCurrentSignal; private StatusSignal supplyCurrentSignal; private StatusSignal appliedVoltageSignal; private IntakeRollerSim simulation; - private double intakeVoltage = 4.0; + private double intakeVoltage = 12.0; private double intakeStallVoltage = 12.0; private Debouncer stallDebouncer = new Debouncer(0.25, DebounceType.kRising); private boolean stalling = false; /** Intializes motor and status signals Class for Intake Rollers */ public IntakeRollers() { - this.intakeMotor = - new TalonFX(IntakeRollersConstants.DEVICE_ID, new CANBus("subsystems")); // temporary - - intakeMotor.getConfigurator().apply(IntakeRollersConstants.MOTOR_CONFIGURATION); - this.velocitySignal = intakeMotor.getVelocity(); - this.statorCurrentSignal = intakeMotor.getStatorCurrent(); - this.supplyCurrentSignal = intakeMotor.getSupplyCurrent(); - this.appliedVoltageSignal = intakeMotor.getMotorVoltage(); + IntakeRollerMotor1 = + new TalonFX( + IntakeRollersConstants.DEVICE_ID_1, + IntakeRollersConstants.CANBUS); + + IntakeRollerMotor1.getConfigurator().apply(IntakeRollersConstants.MOTOR_CONFIGURATION); + + IntakeRollerMotor2 = + new TalonFX( + IntakeRollersConstants.DEVICE_ID_2, + IntakeRollersConstants.CANBUS); + + IntakeRollersConstants.MOTOR_CONFIGURATION.MotorOutput.Inverted = + IntakeRollersConstants.MOTOR_CONFIGURATION.MotorOutput.Inverted + == InvertedValue.Clockwise_Positive + ? InvertedValue.CounterClockwise_Positive + : InvertedValue.Clockwise_Positive; + + + this.velocitySignal = IntakeRollerMotor1.getVelocity(); + this.statorCurrentSignal = IntakeRollerMotor1.getStatorCurrent(); + this.supplyCurrentSignal = IntakeRollerMotor2.getSupplyCurrent(); + this.appliedVoltageSignal = IntakeRollerMotor1.getMotorVoltage(); if (RobotBase.isSimulation()) { simulation = new IntakeRollerSim(intakeMotor); } @@ -63,6 +83,12 @@ public IntakeRollers() { }); CurrentDrawLogger.add("Intake Rollers", this::getSupplyCurrent); + + IntakeRollerMotor2.setControl( + new Follower(IntakeRollerMotor1.getDeviceID(), MotorAlignmentValue.Opposed)); + if (RobotBase.isSimulation()) { + simulation = new IntakeRollerSim(IntakeRollerMotor1); + } } /** Returns command to make the motor move and stop */ @@ -93,22 +119,6 @@ public Command intake() { }); } - /** - * Returns command where motor intakes fuel at full speed. This should only be used when shooting - * fuel out of the robot with the intake is not a concern. - * - * @return The command to intake fuel at full speed. - */ - public Command intakeFast() { - return startEnd( - () -> { - intakeMotor.setControl(new VoltageOut(7).withEnableFOC(false)); - }, - () -> { - intakeMotor.setControl(new CoastOut()); - }); - } - /** * Returns command where motor outtakes fuel * diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java index 625f1155..c8b3fd01 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.Amps; +import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; @@ -10,9 +11,11 @@ import com.ctre.phoenix6.signals.InvertedValue; public class IntakeRollersConstants { - public static final double GEAR_RATIO = 32 / 18; + public static final double GEAR_RATIO = 15 / 27; public static final double MOMENT_OF_INERTIA = 0.00074271944; - public static final int DEVICE_ID = 41; + public static final int DEVICE_ID_1 = 41; + public static final int DEVICE_ID_2 = 42; // Prob not Right + public static final CANBus CANBUS = new CANBus("subsystems"); public static final TalonFXConfiguration MOTOR_CONFIGURATION = new TalonFXConfiguration() From c7d22c4b743464de88d52318e742fb544e3c6352 Mon Sep 17 00:00:00 2001 From: neelabhb-sudo Date: Tue, 23 Jun 2026 16:26:50 -0700 Subject: [PATCH 02/16] Applied Spotless --- src/main/java/frc/robot/auto/Autonomous.java | 21 +++++++------------ .../IntakeExtensionConstants.java | 2 +- .../intakerollers/IntakeRollers.java | 10 ++------- 3 files changed, 10 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/auto/Autonomous.java b/src/main/java/frc/robot/auto/Autonomous.java index 4dc3e20a..620bd420 100644 --- a/src/main/java/frc/robot/auto/Autonomous.java +++ b/src/main/java/frc/robot/auto/Autonomous.java @@ -75,13 +75,11 @@ private Command singleNeutralCycle(boolean rightSide) { robot .getSwerveDrive() .followPath("left_neutral.0", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral.1", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot.getSwerveDrive().followPath("left_neutral.2", rightSide), shootFuel.shoot()); } @@ -104,30 +102,25 @@ private Command doubleNeutralCycle(boolean rightSide) { robot .getSwerveDrive() .followPath("left_neutral.0", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral.1", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot.getSwerveDrive().followPath("left_neutral.2", rightSide), shootFuel.shootAllFuelStationary().withTimeout(20.0 - 13.2), robot .getSwerveDrive() .followPath("left_neutral.3", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral.4", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), robot .getSwerveDrive() .followPath("left_neutral.5", rightSide) - .deadlineFor( - robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), + .deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()), shootFuel.shoot()); } diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java index 9a76344e..addde9e8 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java @@ -27,7 +27,7 @@ public final class IntakeExtensionConstants { public static final int MOTOR_CAN_ID = 40; public static final DCMotor MOTOR_PHYSICS = DCMotor.getKrakenX44Foc(1); - public static final Mass MOVING_MASS = Pounds.of(14.5); //Subject to Change + public static final Mass MOVING_MASS = Pounds.of(14.5); // Subject to Change public static final Distance MAX_POSITION = Inches.of(10.1); public static final Distance MIN_POSITION = Inches.of(0); public static final Distance RETRACT_POSITION = Inches.of(3); diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java index ec0718cd..52db192c 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java @@ -14,7 +14,6 @@ import com.ctre.phoenix6.signals.MotorAlignmentValue; import com.team6962.lib.logging.CurrentDrawLogger; import com.team6962.lib.phoenix.StatusUtil; - import dev.doglog.DogLog; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.filter.Debouncer.DebounceType; @@ -42,16 +41,12 @@ public class IntakeRollers extends SubsystemBase { /** Intializes motor and status signals Class for Intake Rollers */ public IntakeRollers() { IntakeRollerMotor1 = - new TalonFX( - IntakeRollersConstants.DEVICE_ID_1, - IntakeRollersConstants.CANBUS); + new TalonFX(IntakeRollersConstants.DEVICE_ID_1, IntakeRollersConstants.CANBUS); IntakeRollerMotor1.getConfigurator().apply(IntakeRollersConstants.MOTOR_CONFIGURATION); IntakeRollerMotor2 = - new TalonFX( - IntakeRollersConstants.DEVICE_ID_2, - IntakeRollersConstants.CANBUS); + new TalonFX(IntakeRollersConstants.DEVICE_ID_2, IntakeRollersConstants.CANBUS); IntakeRollersConstants.MOTOR_CONFIGURATION.MotorOutput.Inverted = IntakeRollersConstants.MOTOR_CONFIGURATION.MotorOutput.Inverted @@ -59,7 +54,6 @@ public IntakeRollers() { ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive; - this.velocitySignal = IntakeRollerMotor1.getVelocity(); this.statorCurrentSignal = IntakeRollerMotor1.getStatorCurrent(); this.supplyCurrentSignal = IntakeRollerMotor2.getSupplyCurrent(); From 7669938feb800d3aeae79239c4c2bdb92b5fb13a Mon Sep 17 00:00:00 2001 From: rahul Date: Fri, 10 Jul 2026 15:15:00 -0700 Subject: [PATCH 03/16] Finalized can id for 2nd motor --- .../robot/subsystems/intakerollers/IntakeRollersConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java index c8b3fd01..5012090d 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java @@ -14,7 +14,7 @@ public class IntakeRollersConstants { public static final double GEAR_RATIO = 15 / 27; public static final double MOMENT_OF_INERTIA = 0.00074271944; public static final int DEVICE_ID_1 = 41; - public static final int DEVICE_ID_2 = 42; // Prob not Right + public static final int DEVICE_ID_2 = 42; public static final CANBus CANBUS = new CANBus("subsystems"); public static final TalonFXConfiguration MOTOR_CONFIGURATION = From bba2fda36ad77ebfcd952683a687b9a8e68b9f06 Mon Sep 17 00:00:00 2001 From: rahul Date: Fri, 10 Jul 2026 15:51:06 -0700 Subject: [PATCH 04/16] Updated moving mass and moment of inertia --- .../subsystems/intakeextension/IntakeExtensionConstants.java | 2 +- .../robot/subsystems/intakerollers/IntakeRollersConstants.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java index addde9e8..94a07ba8 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java @@ -27,7 +27,7 @@ public final class IntakeExtensionConstants { public static final int MOTOR_CAN_ID = 40; public static final DCMotor MOTOR_PHYSICS = DCMotor.getKrakenX44Foc(1); - public static final Mass MOVING_MASS = Pounds.of(14.5); // Subject to Change + public static final Mass MOVING_MASS = Pounds.of(13.25); public static final Distance MAX_POSITION = Inches.of(10.1); public static final Distance MIN_POSITION = Inches.of(0); public static final Distance RETRACT_POSITION = Inches.of(3); diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java index 5012090d..e164b1be 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java @@ -12,7 +12,7 @@ public class IntakeRollersConstants { public static final double GEAR_RATIO = 15 / 27; - public static final double MOMENT_OF_INERTIA = 0.00074271944; + public static final double MOMENT_OF_INERTIA = 0.00022826; public static final int DEVICE_ID_1 = 41; public static final int DEVICE_ID_2 = 42; public static final CANBus CANBUS = new CANBus("subsystems"); From ff93cb6d55bcd220f153c32684a1d5f8d8e5b004 Mon Sep 17 00:00:00 2001 From: The-REAL-Bol Date: Tue, 14 Jul 2026 14:50:29 -0700 Subject: [PATCH 05/16] Added Intake Agitation fixes deadzones for new intake --- src/main/java/frc/robot/auto/ShootFuel.java | 10 ++++--- .../intakeextension/IntakeExtension.java | 29 +++++++++++++++++++ .../IntakeExtensionConstants.java | 1 + 3 files changed, 36 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/auto/ShootFuel.java b/src/main/java/frc/robot/auto/ShootFuel.java index 49f8d80d..ccaf81a9 100644 --- a/src/main/java/frc/robot/auto/ShootFuel.java +++ b/src/main/java/frc/robot/auto/ShootFuel.java @@ -23,7 +23,9 @@ public Command shootAllFuel() { .feed() .onlyWhile( () -> autoShoot.isReadyToShoot().getAsBoolean() || RobotBase.isSimulation()) - .repeatedly()) + .repeatedly(), + robot.getIntakeRollers().intake(), + robot.getIntakeExtension().agitate()) .until(() -> robot.getHopper().isEmpty()); } @@ -40,7 +42,8 @@ public Command shootAllFuelStationary() { Commands.waitUntil( () -> autoShoot.isReadyToShoot().getAsBoolean() || RobotBase.isSimulation()) .andThen(robot.getHopper().feed().repeatedly()), - robot.getIntakeRollers().intake()) + robot.getIntakeRollers().intake(), + robot.getIntakeExtension().agitate()) .until(() -> robot.getHopper().isEmpty()); } @@ -91,8 +94,7 @@ public Command shootAllFuelOnTheMove() { autoShoot.isReadyToShoot().getAsBoolean() || RobotBase.isSimulation()) .repeatedly()), - robot.getIntakeExtension().retract(), - robot.getIntakeExtension().extend().repeatedly(), + robot.getIntakeExtension().agitate(), robot.getIntakeRollers().intake()) .until(() -> robot.getHopper().isEmpty()); } diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java index b5b70320..f8fe9090 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java @@ -7,6 +7,7 @@ import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volts; import com.ctre.phoenix6.BaseStatusSignal; @@ -37,12 +38,14 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.RobotContainer; /** This subsystem controls the extension of the intake out of the robot and back in. */ public class IntakeExtension extends SubsystemBase { private TalonFX motor; private CANdi candi; + private RobotContainer robot; // The status signals' rotations actually represent meters of mechanism motion private StatusSignal positionSignal; @@ -261,6 +264,32 @@ public Command retract() { IntakeExtensionConstants.POSITION_TOLERANCE)); } + public Command agitate() { + return Commands.sequence( + startEnd( + () -> { + motor.setControl( + new MotionMagicVoltage( + IntakeExtensionConstants.AGITATED_POSITION.in(Meters))); + }, + () -> { + if (!getPosition() + .isNear( + IntakeExtensionConstants.AGITATED_POSITION, + IntakeExtensionConstants.POSITION_TOLERANCE)) { + motor.setControl(new MotionMagicVoltage(getPosition().in(Meters))); + } + }) + .until( + () -> + getPosition() + .isNear( + IntakeExtensionConstants.AGITATED_POSITION, + IntakeExtensionConstants.POSITION_TOLERANCE)), + Commands.waitTime(Seconds.of(0.75)), + robot.getIntakeExtension().extend()); + } + public Command extendSlow() { return startEnd( () -> { diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java index 94a07ba8..dbe9562a 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java @@ -31,6 +31,7 @@ public final class IntakeExtensionConstants { public static final Distance MAX_POSITION = Inches.of(10.1); public static final Distance MIN_POSITION = Inches.of(0); public static final Distance RETRACT_POSITION = Inches.of(3); + public static final Distance AGITATED_POSITION = Inches.of(8); // change public static final Angle ANGLE = Degrees.of(-18.6196233); public static final Distance PINION_RADIUS = Inches.of(1.5); public static final int CANDI_DEVICE_ID = 20; From e1bce93d87eab079379a03f76adfd9db5613df13 Mon Sep 17 00:00:00 2001 From: The-REAL-Bol Date: Tue, 14 Jul 2026 14:52:01 -0700 Subject: [PATCH 06/16] deleted comment "change" for agitated position --- .../subsystems/intakeextension/IntakeExtensionConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java index dbe9562a..09bc9d5b 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java @@ -31,7 +31,7 @@ public final class IntakeExtensionConstants { public static final Distance MAX_POSITION = Inches.of(10.1); public static final Distance MIN_POSITION = Inches.of(0); public static final Distance RETRACT_POSITION = Inches.of(3); - public static final Distance AGITATED_POSITION = Inches.of(8); // change + public static final Distance AGITATED_POSITION = Inches.of(8); public static final Angle ANGLE = Degrees.of(-18.6196233); public static final Distance PINION_RADIUS = Inches.of(1.5); public static final int CANDI_DEVICE_ID = 20; From 985290e31c2e6e53a0bf44a61316e00077bd7ec2 Mon Sep 17 00:00:00 2001 From: rahul Date: Tue, 14 Jul 2026 17:41:53 -0700 Subject: [PATCH 07/16] Made improvements --- .../intakeextension/IntakeExtension.java | 3 ++- .../intakerollers/IntakeRollers.java | 24 +++++++++---------- 2 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java index f8fe9090..62777867 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java @@ -285,7 +285,8 @@ public Command agitate() { getPosition() .isNear( IntakeExtensionConstants.AGITATED_POSITION, - IntakeExtensionConstants.POSITION_TOLERANCE)), + IntakeExtensionConstants.POSITION_TOLERANCE)) + .withTimeout(1), Commands.waitTime(Seconds.of(0.75)), robot.getIntakeExtension().extend()); } diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java index 52db192c..b36b25a6 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java @@ -25,8 +25,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeRollers extends SubsystemBase { - private TalonFX IntakeRollerMotor1; - private TalonFX IntakeRollerMotor2; + private TalonFX intakeMotorLeader; + private TalonFX intakeMotorFollower; private TalonFX intakeMotor; private StatusSignal velocitySignal; private StatusSignal statorCurrentSignal; @@ -40,12 +40,12 @@ public class IntakeRollers extends SubsystemBase { /** Intializes motor and status signals Class for Intake Rollers */ public IntakeRollers() { - IntakeRollerMotor1 = + intakeMotorLeader = new TalonFX(IntakeRollersConstants.DEVICE_ID_1, IntakeRollersConstants.CANBUS); - IntakeRollerMotor1.getConfigurator().apply(IntakeRollersConstants.MOTOR_CONFIGURATION); + intakeMotorLeader.getConfigurator().apply(IntakeRollersConstants.MOTOR_CONFIGURATION); - IntakeRollerMotor2 = + intakeMotorFollower = new TalonFX(IntakeRollersConstants.DEVICE_ID_2, IntakeRollersConstants.CANBUS); IntakeRollersConstants.MOTOR_CONFIGURATION.MotorOutput.Inverted = @@ -54,10 +54,10 @@ public IntakeRollers() { ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive; - this.velocitySignal = IntakeRollerMotor1.getVelocity(); - this.statorCurrentSignal = IntakeRollerMotor1.getStatorCurrent(); - this.supplyCurrentSignal = IntakeRollerMotor2.getSupplyCurrent(); - this.appliedVoltageSignal = IntakeRollerMotor1.getMotorVoltage(); + this.velocitySignal = intakeMotorLeader.getVelocity(); + this.statorCurrentSignal = intakeMotorLeader.getStatorCurrent(); + this.supplyCurrentSignal = intakeMotorFollower.getSupplyCurrent(); + this.appliedVoltageSignal = intakeMotorLeader.getMotorVoltage(); if (RobotBase.isSimulation()) { simulation = new IntakeRollerSim(intakeMotor); } @@ -78,10 +78,10 @@ public IntakeRollers() { CurrentDrawLogger.add("Intake Rollers", this::getSupplyCurrent); - IntakeRollerMotor2.setControl( - new Follower(IntakeRollerMotor1.getDeviceID(), MotorAlignmentValue.Opposed)); + intakeMotorFollower.setControl( + new Follower(intakeMotorLeader.getDeviceID(), MotorAlignmentValue.Opposed)); if (RobotBase.isSimulation()) { - simulation = new IntakeRollerSim(IntakeRollerMotor1); + simulation = new IntakeRollerSim(intakeMotorLeader); } } From 401ff61405797f8f2fd3e18ecccd2ae68936cab0 Mon Sep 17 00:00:00 2001 From: rahul Date: Wed, 15 Jul 2026 19:25:47 -0700 Subject: [PATCH 08/16] Fixed bugs from testing --- src/main/java/frc/robot/auto/Autonomous.java | 11 +++-- .../intakeextension/IntakeExtension.java | 4 +- .../IntakeExtensionConstants.java | 12 ++--- .../intakerollers/IntakeRollers.java | 46 +++++++++---------- .../intakerollers/IntakeRollersConstants.java | 4 +- 5 files changed, 38 insertions(+), 39 deletions(-) diff --git a/src/main/java/frc/robot/auto/Autonomous.java b/src/main/java/frc/robot/auto/Autonomous.java index 620bd420..bce20a50 100644 --- a/src/main/java/frc/robot/auto/Autonomous.java +++ b/src/main/java/frc/robot/auto/Autonomous.java @@ -211,9 +211,11 @@ public Command center(boolean fromHub) { 3.616589307785034, 5.050553798675537, Rotation2d.k180deg))), Commands.parallel( Commands.sequence( - robot - .getSwerveDrive() - .followPath(pathName + ".0"), // Drive away from hub to where shooting can start + Commands.parallel( + robot.getSwerveDrive().followPath(pathName + ".0"), + robot + .getIntakeExtension() + .extend()), // Drive away from hub to where shooting can start robot .getSwerveDrive() .followPath(pathName + ".1") @@ -221,8 +223,7 @@ public Command center(boolean fromHub) { shootFuel.shootOnTheMove()), // Drive while shooting preload to depot setup // position shootFuel.shootAllFuelStationary().withTimeout(2) // Shoot any remaining fuel - ), - robot.getIntakeExtension().extend()), + )), robot .getSwerveDrive() .followPath(pathName + ".2") diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java index 62777867..e75c286c 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java @@ -38,14 +38,12 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.RobotContainer; /** This subsystem controls the extension of the intake out of the robot and back in. */ public class IntakeExtension extends SubsystemBase { private TalonFX motor; private CANdi candi; - private RobotContainer robot; // The status signals' rotations actually represent meters of mechanism motion private StatusSignal positionSignal; @@ -288,7 +286,7 @@ public Command agitate() { IntakeExtensionConstants.POSITION_TOLERANCE)) .withTimeout(1), Commands.waitTime(Seconds.of(0.75)), - robot.getIntakeExtension().extend()); + extend()); } public Command extendSlow() { diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java index 09bc9d5b..59b883d2 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java @@ -37,7 +37,7 @@ public final class IntakeExtensionConstants { public static final int CANDI_DEVICE_ID = 20; public static final Distance POSITION_TOLERANCE = Inches.of(0.125); public static final Voltage FINE_CONTROL_VOLTAGE = Volts.of(1.0); - public static final double GEAR_RATIO = 7; + public static final double GEAR_RATIO = 17.5; public static final TalonFXConfiguration MOTOR_CONFIGURATION = new TalonFXConfiguration() @@ -53,12 +53,12 @@ public final class IntakeExtensionConstants { .withMotionMagicJerk(0)) .withSlot0( new Slot0Configs() - .withKA(0.035) + .withKA(0.0377) .withKD(0.0) .withKP(100.0) - .withKG(-0.045) - .withKV(5.878) - .withKS(0.315) + .withKG(-0.118) + .withKV(6.785) + .withKS(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign)) .withCurrentLimits( new CurrentLimitsConfigs() @@ -68,7 +68,7 @@ public final class IntakeExtensionConstants { .withStatorCurrentLimitEnable(true)) .withMotorOutput( new MotorOutputConfigs() - .withInverted(InvertedValue.CounterClockwise_Positive) + .withInverted(InvertedValue.Clockwise_Positive) .withNeutralMode(NeutralModeValue.Coast)); public static final CANdiConfiguration CANDI_CONFIGURATION = new CANdiConfiguration(); diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java index b36b25a6..2289068c 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java @@ -25,9 +25,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class IntakeRollers extends SubsystemBase { - private TalonFX intakeMotorLeader; - private TalonFX intakeMotorFollower; - private TalonFX intakeMotor; + private TalonFX leaderMotor; + private TalonFX followerMotor; private StatusSignal velocitySignal; private StatusSignal statorCurrentSignal; private StatusSignal supplyCurrentSignal; @@ -40,13 +39,12 @@ public class IntakeRollers extends SubsystemBase { /** Intializes motor and status signals Class for Intake Rollers */ public IntakeRollers() { - intakeMotorLeader = - new TalonFX(IntakeRollersConstants.DEVICE_ID_1, IntakeRollersConstants.CANBUS); + leaderMotor = new TalonFX(IntakeRollersConstants.LEADER_ID_1, IntakeRollersConstants.CANBUS); - intakeMotorLeader.getConfigurator().apply(IntakeRollersConstants.MOTOR_CONFIGURATION); + leaderMotor.getConfigurator().apply(IntakeRollersConstants.MOTOR_CONFIGURATION); - intakeMotorFollower = - new TalonFX(IntakeRollersConstants.DEVICE_ID_2, IntakeRollersConstants.CANBUS); + followerMotor = + new TalonFX(IntakeRollersConstants.FOLLOWER_ID_2, IntakeRollersConstants.CANBUS); IntakeRollersConstants.MOTOR_CONFIGURATION.MotorOutput.Inverted = IntakeRollersConstants.MOTOR_CONFIGURATION.MotorOutput.Inverted @@ -54,12 +52,14 @@ public IntakeRollers() { ? InvertedValue.CounterClockwise_Positive : InvertedValue.Clockwise_Positive; - this.velocitySignal = intakeMotorLeader.getVelocity(); - this.statorCurrentSignal = intakeMotorLeader.getStatorCurrent(); - this.supplyCurrentSignal = intakeMotorFollower.getSupplyCurrent(); - this.appliedVoltageSignal = intakeMotorLeader.getMotorVoltage(); - if (RobotBase.isSimulation()) { - simulation = new IntakeRollerSim(intakeMotor); + followerMotor.getConfigurator().apply(IntakeRollersConstants.MOTOR_CONFIGURATION); + + this.velocitySignal = leaderMotor.getVelocity(); + this.statorCurrentSignal = leaderMotor.getStatorCurrent(); + this.supplyCurrentSignal = followerMotor.getSupplyCurrent(); + this.appliedVoltageSignal = leaderMotor.getMotorVoltage(); + if(RobotBase.isSimulation()) { + simulation = new IntakeRollerSim(leaderMotor); } DogLog.tunable( @@ -78,10 +78,10 @@ public IntakeRollers() { CurrentDrawLogger.add("Intake Rollers", this::getSupplyCurrent); - intakeMotorFollower.setControl( - new Follower(intakeMotorLeader.getDeviceID(), MotorAlignmentValue.Opposed)); + followerMotor.setControl( + new Follower(leaderMotor.getDeviceID(), MotorAlignmentValue.Opposed)); if (RobotBase.isSimulation()) { - simulation = new IntakeRollerSim(intakeMotorLeader); + simulation = new IntakeRollerSim(leaderMotor); } } @@ -90,10 +90,10 @@ public IntakeRollers() { private Command move(Voltage voltage) { return startEnd( () -> { - intakeMotor.setControl(new VoltageOut(voltage).withEnableFOC(false)); + leaderMotor.setControl(new VoltageOut(voltage).withEnableFOC(false)); }, () -> { - intakeMotor.setControl(new CoastOut()); + leaderMotor.setControl(new CoastOut()); }); } @@ -105,11 +105,11 @@ private Command move(Voltage voltage) { public Command intake() { return runEnd( () -> { - intakeMotor.setControl( + leaderMotor.setControl( new VoltageOut(stalling ? intakeStallVoltage : intakeVoltage).withEnableFOC(false)); }, () -> { - intakeMotor.setControl(new CoastOut()); + leaderMotor.setControl(new CoastOut()); }); } @@ -121,10 +121,10 @@ public Command intake() { public Command outtake() { return startEnd( () -> { - intakeMotor.setControl(new DutyCycleOut(-1).withEnableFOC(false)); + leaderMotor.setControl(new DutyCycleOut(-1).withEnableFOC(false)); }, () -> { - intakeMotor.setControl(new CoastOut()); + leaderMotor.setControl(new CoastOut()); }); } diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java index e164b1be..263509a3 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollersConstants.java @@ -13,8 +13,8 @@ public class IntakeRollersConstants { public static final double GEAR_RATIO = 15 / 27; public static final double MOMENT_OF_INERTIA = 0.00022826; - public static final int DEVICE_ID_1 = 41; - public static final int DEVICE_ID_2 = 42; + public static final int LEADER_ID_1 = 41; + public static final int FOLLOWER_ID_2 = 42; public static final CANBus CANBUS = new CANBus("subsystems"); public static final TalonFXConfiguration MOTOR_CONFIGURATION = From 1ff6834730f93bcfedb84708ead3a781d87f5489 Mon Sep 17 00:00:00 2001 From: scotch-tape <156549340+scotch-tape@users.noreply.github.com> Date: Thu, 16 Jul 2026 02:27:25 +0000 Subject: [PATCH 09/16] style: Apply Spotless fixes --- .../frc/robot/subsystems/intakerollers/IntakeRollers.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java index 2289068c..f1148690 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java @@ -58,7 +58,7 @@ public IntakeRollers() { this.statorCurrentSignal = leaderMotor.getStatorCurrent(); this.supplyCurrentSignal = followerMotor.getSupplyCurrent(); this.appliedVoltageSignal = leaderMotor.getMotorVoltage(); - if(RobotBase.isSimulation()) { + if (RobotBase.isSimulation()) { simulation = new IntakeRollerSim(leaderMotor); } @@ -78,8 +78,7 @@ public IntakeRollers() { CurrentDrawLogger.add("Intake Rollers", this::getSupplyCurrent); - followerMotor.setControl( - new Follower(leaderMotor.getDeviceID(), MotorAlignmentValue.Opposed)); + followerMotor.setControl(new Follower(leaderMotor.getDeviceID(), MotorAlignmentValue.Opposed)); if (RobotBase.isSimulation()) { simulation = new IntakeRollerSim(leaderMotor); } From 71a9c1bc4a058b928eb427ddc4d3a54bbfd090d9 Mon Sep 17 00:00:00 2001 From: The-REAL-Bol Date: Thu, 16 Jul 2026 14:41:15 -0700 Subject: [PATCH 10/16] Implemented agitation in teleop only agitates when not intaking when shooting to hub --- src/main/java/frc/robot/controls/TeleopControls.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/controls/TeleopControls.java b/src/main/java/frc/robot/controls/TeleopControls.java index e0be2d8e..826b3786 100644 --- a/src/main/java/frc/robot/controls/TeleopControls.java +++ b/src/main/java/frc/robot/controls/TeleopControls.java @@ -379,7 +379,12 @@ public void configureBindings() { RotationsPerSecondPerSecond.of(hubMaxAngularAcceleration))), Set.of())) .and(autoShoot.isReadyToShoot()) - .whileTrue(robot.getHopper().feed()); + .whileTrue( + robot + .getHopper() + .feed() + .alongWith( + robot.getIntakeExtension().agitate().onlyWhile(driver.rightStick().negate()))); shootButtonsTrigger .and(inAllianceZone.negate()) From 5dfa2c60bad12b624b97fd589c7ce611e3b526c1 Mon Sep 17 00:00:00 2001 From: neelabhb-sudo Date: Thu, 16 Jul 2026 15:09:36 -0700 Subject: [PATCH 11/16] recalculated feedforward constants --- .../intakeextension/IntakeExtensionConstants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java index 59b883d2..9ffceef1 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java @@ -26,7 +26,7 @@ public final class IntakeExtensionConstants { public static final int MOTOR_CAN_ID = 40; - public static final DCMotor MOTOR_PHYSICS = DCMotor.getKrakenX44Foc(1); + public static final DCMotor MOTOR_PHYSICS = DCMotor.getKrakenX44(1); public static final Mass MOVING_MASS = Pounds.of(13.25); public static final Distance MAX_POSITION = Inches.of(10.1); public static final Distance MIN_POSITION = Inches.of(0); @@ -53,11 +53,11 @@ public final class IntakeExtensionConstants { .withMotionMagicJerk(0)) .withSlot0( new Slot0Configs() - .withKA(0.0377) + .withKA(0.0393) .withKD(0.0) .withKP(100.0) - .withKG(-0.118) - .withKV(6.785) + .withKG(-0.1196) + .withKV(6.737) .withKS(0) .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign)) .withCurrentLimits( From 9cd56a0b7996c517ecda3d07de203ee3c10b5e59 Mon Sep 17 00:00:00 2001 From: The-REAL-Bol Date: Thu, 16 Jul 2026 15:36:54 -0700 Subject: [PATCH 12/16] applied spotless --- src/main/java/frc/robot/controls/TeleopControls.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/controls/TeleopControls.java b/src/main/java/frc/robot/controls/TeleopControls.java index 826b3786..e536936c 100644 --- a/src/main/java/frc/robot/controls/TeleopControls.java +++ b/src/main/java/frc/robot/controls/TeleopControls.java @@ -379,12 +379,12 @@ public void configureBindings() { RotationsPerSecondPerSecond.of(hubMaxAngularAcceleration))), Set.of())) .and(autoShoot.isReadyToShoot()) - .whileTrue( - robot - .getHopper() - .feed() - .alongWith( - robot.getIntakeExtension().agitate().onlyWhile(driver.rightStick().negate()))); + .whileTrue(robot.getHopper().feed() + .alongWith( + robot.getIntakeExtension().agitate() + .onlyWhile(driver.rightStick().negate()) + )); + shootButtonsTrigger .and(inAllianceZone.negate()) From db8343813bf5f345a18837c3434c26d6d069deda Mon Sep 17 00:00:00 2001 From: The-REAL-Bol <217482546+The-REAL-Bol@users.noreply.github.com> Date: Thu, 16 Jul 2026 22:38:22 +0000 Subject: [PATCH 13/16] style: Apply Spotless fixes --- src/main/java/frc/robot/controls/TeleopControls.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/controls/TeleopControls.java b/src/main/java/frc/robot/controls/TeleopControls.java index e536936c..826b3786 100644 --- a/src/main/java/frc/robot/controls/TeleopControls.java +++ b/src/main/java/frc/robot/controls/TeleopControls.java @@ -379,12 +379,12 @@ public void configureBindings() { RotationsPerSecondPerSecond.of(hubMaxAngularAcceleration))), Set.of())) .and(autoShoot.isReadyToShoot()) - .whileTrue(robot.getHopper().feed() - .alongWith( - robot.getIntakeExtension().agitate() - .onlyWhile(driver.rightStick().negate()) - )); - + .whileTrue( + robot + .getHopper() + .feed() + .alongWith( + robot.getIntakeExtension().agitate().onlyWhile(driver.rightStick().negate()))); shootButtonsTrigger .and(inAllianceZone.negate()) From 3e003fb96f0b402a92d114796366c8536419d16e Mon Sep 17 00:00:00 2001 From: rahul Date: Thu, 16 Jul 2026 19:49:39 -0700 Subject: [PATCH 14/16] Tested intake agitation --- .../frc/robot/controls/TeleopControls.java | 26 ++++++++++++------- .../intakeextension/IntakeExtension.java | 8 +++--- 2 files changed, 21 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/controls/TeleopControls.java b/src/main/java/frc/robot/controls/TeleopControls.java index 826b3786..3a2e7af9 100644 --- a/src/main/java/frc/robot/controls/TeleopControls.java +++ b/src/main/java/frc/robot/controls/TeleopControls.java @@ -292,14 +292,6 @@ public void configureBindings() { .getIntakeExtension() .moveAtVoltage(IntakeExtensionConstants.FINE_CONTROL_VOLTAGE.unaryMinus()))); - // Intake extension and retraction - WORKS - Trigger intakeRetract = operator.rightStick().or(driver.start()); - Trigger intakeExtend = - intakeRetract.negate().and(RobotState::isTeleop).and(RobotState::isEnabled); - - intakeRetract.and(() -> !fineControl).whileTrue(robot.getIntakeExtension().retract()); - intakeExtend.and(() -> !fineControl).whileTrue(robot.getIntakeExtension().extend()); - // Climb retraction // Command autodescend = robot.getClimb().descend(); // Trigger climbRetract = @@ -384,7 +376,11 @@ public void configureBindings() { .getHopper() .feed() .alongWith( - robot.getIntakeExtension().agitate().onlyWhile(driver.rightStick().negate()))); + robot + .getIntakeExtension() + .agitate() + .alongWith(robot.getIntakeRollers().intake()) + .onlyWhile(driver.rightStick().negate()))); shootButtonsTrigger .and(inAllianceZone.negate()) @@ -439,6 +435,18 @@ public void configureBindings() { // Degrees.of(1))), // robot.getHopper().feed()))); + // Intake extension and retraction - WORKS + Trigger intakeRetract = operator.rightStick().or(driver.start()); + Trigger intakeExtend = + intakeRetract + .negate() + .and(RobotState::isTeleop) + .and(RobotState::isEnabled) + .and(shootButtonsTrigger.and(inAllianceZone).and(autoShoot.isReadyToShoot()).negate()); + + intakeRetract.and(() -> !fineControl).whileTrue(robot.getIntakeExtension().retract()); + intakeExtend.and(() -> !fineControl).whileTrue(robot.getIntakeExtension().extend()); + new ShiftFeedback(List.of(driverRumble, operatorRumble)); } diff --git a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java index e75c286c..15d84daa 100644 --- a/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java +++ b/src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java @@ -263,7 +263,7 @@ public Command retract() { } public Command agitate() { - return Commands.sequence( + return Commands.repeatingSequence( startEnd( () -> { motor.setControl( @@ -284,9 +284,9 @@ public Command agitate() { .isNear( IntakeExtensionConstants.AGITATED_POSITION, IntakeExtensionConstants.POSITION_TOLERANCE)) - .withTimeout(1), - Commands.waitTime(Seconds.of(0.75)), - extend()); + .withTimeout(0.25), + Commands.waitTime(Seconds.of(0.5)), + extend().withTimeout(0.25)); } public Command extendSlow() { From f4b6841b128bcdbec23084b14393e040288261da Mon Sep 17 00:00:00 2001 From: rahul Date: Thu, 16 Jul 2026 20:02:44 -0700 Subject: [PATCH 15/16] fixed supply current --- .../frc/robot/subsystems/intakerollers/IntakeRollers.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java index f1148690..ea2bda5d 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java @@ -146,12 +146,13 @@ public Current getStatorCurrent() { } /** - * Takes status signal Supply Current and returns it as a Current + * Returns the total supply current draw of the mechanism. + * It is multiplied by 2 to account for the second intake roller motor. * * @return Current */ public Current getSupplyCurrent() { - return supplyCurrentSignal.getValue(); + return Amps.of((2 * supplyCurrentSignal.getValueAsDouble())); } /** From 4dfc94d3f0f8ae5cc4fb2d4dd28c01871b444df7 Mon Sep 17 00:00:00 2001 From: scotch-tape <156549340+scotch-tape@users.noreply.github.com> Date: Fri, 17 Jul 2026 03:04:05 +0000 Subject: [PATCH 16/16] style: Apply Spotless fixes --- .../frc/robot/subsystems/intakerollers/IntakeRollers.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java index ea2bda5d..8d2adbf0 100644 --- a/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java +++ b/src/main/java/frc/robot/subsystems/intakerollers/IntakeRollers.java @@ -146,8 +146,8 @@ public Current getStatorCurrent() { } /** - * Returns the total supply current draw of the mechanism. - * It is multiplied by 2 to account for the second intake roller motor. + * Returns the total supply current draw of the mechanism. It is multiplied by 2 to account for + * the second intake roller motor. * * @return Current */