Skip to content

Gear driven intake #153

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
scotch-tape merged 16 commits into from
Jul 17, 2026
Merged

Gear driven intake #153

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
16 commits
Select commit Hold shift + click to select a range
f6fbf3c
Changed Intake extension and Intake roller constants
neelabhb-sudo Jun 23, 2026
c7d22c4
Applied Spotless
neelabhb-sudo Jun 23, 2026
7669938
Finalized can id for 2nd motor
scotch-tape Jul 10, 2026
bba2fda
Updated moving mass and moment of inertia
scotch-tape Jul 10, 2026
ff93cb6
Added Intake Agitation
The-REAL-Bol Jul 14, 2026
e1bce93
deleted comment "change" for agitated position
The-REAL-Bol Jul 14, 2026
985290e
Made improvements
scotch-tape Jul 15, 2026
401ff61
Fixed bugs from testing
scotch-tape Jul 16, 2026
1ff6834
style: Apply Spotless fixes
scotch-tape Jul 16, 2026
71a9c1b
Implemented agitation in teleop
The-REAL-Bol Jul 16, 2026
5dfa2c6
recalculated feedforward constants
neelabhb-sudo Jul 16, 2026
9cd56a0
applied spotless
The-REAL-Bol Jul 16, 2026
db83438
style: Apply Spotless fixes
The-REAL-Bol Jul 16, 2026
3e003fb
Tested intake agitation
scotch-tape Jul 17, 2026
f4b6841
fixed supply current
scotch-tape Jul 17, 2026
4dfc94d
style: Apply Spotless fixes
scotch-tape Jul 17, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
40 changes: 17 additions & 23 deletions src/main/java/frc/robot/auto/Autonomous.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,13 +75,11 @@ private Command singleNeutralCycle(boolean rightSide) {
robot
.getSwerveDrive()
.followPath("left_neutral.0", rightSide)
.deadlineFor(
robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()),
.deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()),
robot
.getSwerveDrive()
.followPath("left_neutral.1", rightSide)
.deadlineFor(
robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()),
.deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()),
robot.getSwerveDrive().followPath("left_neutral.2", rightSide),
shootFuel.shoot());
}
Expand All @@ -104,30 +102,25 @@ private Command doubleNeutralCycle(boolean rightSide) {
robot
.getSwerveDrive()
.followPath("left_neutral.0", rightSide)
.deadlineFor(
robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()),
.deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()),
robot
.getSwerveDrive()
.followPath("left_neutral.1", rightSide)
.deadlineFor(
robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()),
.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().intakeFast()),
.deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()),
robot
.getSwerveDrive()
.followPath("left_neutral.4", rightSide)
.deadlineFor(
robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()),
.deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()),
robot
.getSwerveDrive()
.followPath("left_neutral.5", rightSide)
.deadlineFor(
robot.getIntakeExtension().extend(), robot.getIntakeRollers().intakeFast()),
.deadlineFor(robot.getIntakeExtension().extend(), robot.getIntakeRollers().intake()),
shootFuel.shoot());
}

Expand All @@ -150,24 +143,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);
Expand Down Expand Up @@ -218,18 +211,19 @@ 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")
.deadlineFor(
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")
Expand Down
10 changes: 6 additions & 4 deletions src/main/java/frc/robot/auto/ShootFuel.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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());
}

Expand All @@ -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());
}

Expand Down Expand Up @@ -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());
}
Expand Down
31 changes: 22 additions & 9 deletions src/main/java/frc/robot/controls/TeleopControls.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 =
Expand Down Expand Up @@ -379,7 +371,16 @@ public void configureBindings() {
RotationsPerSecondPerSecond.of(hubMaxAngularAcceleration))),
Set.of()))
.and(autoShoot.isReadyToShoot())
.whileTrue(robot.getHopper().feed());
.whileTrue(
robot
.getHopper()
.feed()
.alongWith(
robot
.getIntakeExtension()
.agitate()
.alongWith(robot.getIntakeRollers().intake())
.onlyWhile(driver.rightStick().negate())));

shootButtonsTrigger
.and(inAllianceZone.negate())
Expand Down Expand Up @@ -434,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));
}

Expand Down
28 changes: 28 additions & 0 deletions src/main/java/frc/robot/subsystems/intakeextension/IntakeExtension.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -261,6 +262,33 @@ public Command retract() {
IntakeExtensionConstants.POSITION_TOLERANCE));
}

public Command agitate() {
return Commands.repeatingSequence(
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))
.withTimeout(0.25),
Commands.waitTime(Seconds.of(0.5)),
extend().withTimeout(0.25));
}

public Command extendSlow() {
return startEnd(
() -> {
Expand Down
21 changes: 11 additions & 10 deletions src/main/java/frc/robot/subsystems/intakeextension/IntakeExtensionConstants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,17 +26,18 @@
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 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);
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 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;
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 = 17.5;

public static final TalonFXConfiguration MOTOR_CONFIGURATION =
new TalonFXConfiguration()
Expand All @@ -52,12 +53,12 @@ public final class IntakeExtensionConstants {
.withMotionMagicJerk(0))
.withSlot0(
new Slot0Configs()
.withKA(0.035)
.withKA(0.0393)
.withKD(0.0)
.withKP(100.0)
.withKG(-0.045)
.withKV(5.878)
.withKS(0.315)
.withKG(-0.1196)
.withKV(6.737)
.withKS(0)
.withStaticFeedforwardSign(StaticFeedforwardSignValue.UseVelocitySign))
.withCurrentLimits(
new CurrentLimitsConfigs()
Expand All @@ -67,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();
Expand Down
Loading
Loading