Skip to content

shotBlocker in code #258

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

Draft
cwkpete7383 wants to merge 3 commits into
base: Issue250
Choose a base branch
from
Draft

shotBlocker in code #258

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8a48cef
shotBlocker in code
SnappyRiffs May 2, 2026
f155639
seperate shotblock pos
SnappyRiffs May 2, 2026
08f078a
Merge branch 'Issue250' into drumBlock
koolpoolo Jun 2, 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
20 changes: 17 additions & 3 deletions src/main/java/frc/robot/Controls.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -356,7 +356,10 @@ private void configureLauncherBindings() {
}

private void updateIntakeMode() {
if (driverController.leftTrigger().getAsBoolean()) {
if (connected(indexingTestController).getAsBoolean()
&& indexingTestController.rightTrigger().getAsBoolean()) {
intakeMode = IntakeMode.BLOCK;
} else if (driverController.leftTrigger().getAsBoolean()) {
intakeMode = IntakeMode.INTAKE;
ledsMode = LEDMode.INTAKE;
} else if (driverController.leftBumper().getAsBoolean()) {
Expand Down Expand Up @@ -389,6 +392,7 @@ private void configureIntakeBindings() {
case INTAKE ->
s.intakeSubsystem.smartIntake(() -> s.drivebaseSubsystem.getState().Speeds);
case EXTAKE -> s.intakeSubsystem.extakeIntake();
case BLOCK -> s.intakeSubsystem.holdAtExtake();
}
},
s.intakeSubsystem)
Expand Down Expand Up @@ -424,6 +428,16 @@ private void configureIntakeBindings() {
connected(intakeTestController)
.and(intakeTestController.y())
.onTrue(Commands.runOnce(() -> intakeMode = IntakeMode.RETRACTED));

connected(indexingTestController)
.and(indexingTestController.rightTrigger())
.onTrue(
Commands.parallel(
Commands.runOnce(() -> intakeMode = IntakeMode.BLOCK),
s.blocker.blockerOutCommand()))
.onFalse(
Commands.parallel(
Commands.runOnce(() -> updateIntakeMode()), s.blocker.blockerInCommand()));
Comment on lines +431 to +439

@gemini-code-assist gemini-code-assist Bot May 2, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

high

This trigger binding should be guarded with a null check for s.blocker. If the blocker subsystem is disabled in Subsystems.java, s.blocker will be null, leading to a NullPointerException when blockerOutCommand() or blockerInCommand() is called.

    if (s.blocker != null) {
      connected(indexingTestController)
          .and(indexingTestController.rightTrigger())
          .onTrue(
              Commands.parallel(
                  Commands.runOnce(() -> intakeMode = IntakeMode.BLOCK),
                  s.blocker.blockerOutCommand()))
          .onFalse(
              Commands.parallel(
                  Commands.runOnce(() -> updateIntakeMode()), s.blocker.blockerInCommand()));
    }

}

/**
Expand Down Expand Up @@ -477,8 +491,8 @@ private void configureTurretBindings() {
s.turretSubsystem.setDefaultCommand(
s.turretSubsystem.rotateToTargetWithCalc().withName("Turret Default Command"));

(turretAtZero
.and(new Trigger(()-> s.turretSubsystem.getTurretPosition() < 0.5))).or(driverController.povLeft())
(turretAtZero.and(new Trigger(() -> s.turretSubsystem.getTurretPosition() < 0.5)))
.or(driverController.povLeft())
.onTrue(
Commands.runOnce(() -> s.turretSubsystem.zeroTurretPosistion())
.withName("Zero Turret on Limit Switch"));
Expand Down
1 change: 1 addition & 0 deletions src/main/java/frc/robot/Hardware.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@ public class Hardware {
public static final int INTAKE_PIVOT_MOTOR_ID = 15;
public static final int INTAKE_MOTOR_ONE_ID = 16;
public static final int INTAKE_MOTOR_TWO_ID = 17;
public static final int BLOCKER_MOTOR_ID = 18;

// Launcher Motors (20-29)
public static final int FLYWHEEL_ONE_ID = 20;
Expand Down
10 changes: 10 additions & 0 deletions src/main/java/frc/robot/Subsystems.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
package frc.robot;

import static frc.robot.Subsystems.SubsystemConstants.BLOCKER_ENABLED;
import static frc.robot.Subsystems.SubsystemConstants.DRIVEBASE_ENABLED;
import static frc.robot.Subsystems.SubsystemConstants.FEEDER_ENABLED;
import static frc.robot.Subsystems.SubsystemConstants.FLYWHEELS_ENABLED;
Expand All @@ -19,6 +20,7 @@
import frc.robot.generated.AlphaTunerConstants;
import frc.robot.generated.CompTunerConstants;
import frc.robot.sensors.LEDSubsystem;
import frc.robot.subsystems.Blocker;
import frc.robot.subsystems.VisionSubsystem;
import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain;
import frc.robot.subsystems.index.Feeder;
Expand Down Expand Up @@ -52,6 +54,7 @@ public static class SubsystemConstants {
HOOD_ENABLED && FLYWHEELS_ENABLED && TURRET_ENABLED;
public static final boolean INDEXER_ENABLED = SPINDEXER_ENABLED && FEEDER_ENABLED;
public static final boolean LEDS_ENABLED = true;
public static final boolean BLOCKER_ENABLED = true;
}

// Subsystems go here
Expand All @@ -68,6 +71,7 @@ public static class SubsystemConstants {
public final TurretSubsystem turretSubsystem;
public final IndexerSubsystem indexerSubsystem;
public final LEDSubsystem ledSubsystem;
public final Blocker blocker;

public Subsystems(Mechanism2d mechanism2d) {
// Initialize subsystems here (don't forget to check if they're enabled!)
Expand Down Expand Up @@ -162,5 +166,11 @@ public Subsystems(Mechanism2d mechanism2d) {
} else {
visionSubsystem = null;
}

if (BLOCKER_ENABLED) {
blocker = new Blocker();
} else {
blocker = null;
}
}
}
114 changes: 114 additions & 0 deletions src/main/java/frc/robot/subsystems/Blocker.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,114 @@
package frc.robot.subsystems;

import com.ctre.phoenix6.CANBus;
import com.ctre.phoenix6.configs.TalonFXConfiguration;
import com.ctre.phoenix6.controls.MotionMagicVoltage;
import com.ctre.phoenix6.hardware.TalonFX;
import com.ctre.phoenix6.signals.InvertedValue;
import com.ctre.phoenix6.signals.NeutralModeValue;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Hardware;

public class Blocker extends SubsystemBase {
private final TalonFX blockerMotor;
private final MotionMagicVoltage request = new MotionMagicVoltage(0);

// Positions
private double targetPos;
public static final double DEPLOYED_POS = 0;
public static final double RETRACTED_POS = 0;
Comment thread
Fatblabs marked this conversation as resolved.
private static final double degreeTolerance = 5;

// PID variables
private static final double kP = 40;
private static final double kI = 0;
private static final double kD = 1;
private static final double kG = 0.4;
private static final double kS = 0.2;
private static final double kV = 0;
private static final double kA = 0;

// Current limits
private static final int STATOR_CURRENT_LIMIT = 60; // amps
private static final int SUPPLY_CURRENT_LIMIT = 30; // amps

// Motion Magic Config
private static final double CRUISE_VELOCITY = 100;
private static final double ACCELERATION = 400;
private static final double JERK = 0;

// Gear Ratio
private static final double GEAR_RATIO = 35;

// Soft Limits
public static final double PIVOT_MIN = -0.45; // rotations
public static final double PIVOT_MAX = 0.0;

public Blocker() {
blockerMotor = new TalonFX(Hardware.BLOCKER_MOTOR_ID, CANBus.roboRIO());
blockerConfig();
blockerMotor.clearStickyFaults();
}

public void blockerConfig() {
TalonFXConfiguration config = new TalonFXConfiguration();

config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
config.Feedback.SensorToMechanismRatio = GEAR_RATIO;

config.CurrentLimits.StatorCurrentLimit = STATOR_CURRENT_LIMIT;
config.CurrentLimits.StatorCurrentLimitEnable = true;
config.CurrentLimits.SupplyCurrentLimit = SUPPLY_CURRENT_LIMIT;
config.CurrentLimits.SupplyCurrentLimitEnable = true;

config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = PIVOT_MAX;
config.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = PIVOT_MIN;
config.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;

config.MotionMagic.MotionMagicCruiseVelocity = CRUISE_VELOCITY;
config.MotionMagic.MotionMagicAcceleration = ACCELERATION;
config.MotionMagic.MotionMagicJerk = JERK;

config.Slot0.kP = kP;
config.Slot0.kI = kI;
config.Slot0.kD = kD;
config.Slot0.kG = kG;
config.Slot0.kS = kS;
config.Slot0.kV = kV;
config.Slot0.kA = kA;

blockerMotor.getConfigurator().apply(config);
}

public void setPivotPosition(double pos) {
targetPos = pos;
blockerMotor.setControl(request.withPosition(pos));
}

public Command blockerInCommand() {
return Commands.runOnce(() -> setPivotPosition(RETRACTED_POS));
}

public Command blockerOutCommand() {
return Commands.runOnce(() -> setPivotPosition(DEPLOYED_POS));
}
Comment thread
Fatblabs marked this conversation as resolved.

public Command zeroBlocker() {
return runOnce(
() -> {
blockerMotor.setPosition(RETRACTED_POS);
targetPos = RETRACTED_POS;
})
.withName("Zero Blocker");
}

public boolean isAtTarget(double pose) {
return Math.abs(blockerMotor.getPosition().getValueAsDouble() - pose)
< Units.degreesToRotations(degreeTolerance);
}
}
8 changes: 7 additions & 1 deletion src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,8 @@ public enum IntakeMode {
SPIN,
LAUNCH,
INTAKE,
EXTAKE
EXTAKE,
BLOCK
}

protected IntakePivot intakePivot;
Expand Down Expand Up @@ -67,4 +68,9 @@ public void extakeIntake() {
intakePivot.setPivotPosition(IntakePivot.EXTAKE_POS);
intakeRollers.runRollers(-intakeRollers.TARGET_RPS);
}

public void holdAtExtake() {
intakeRollers.stopMotor();
intakePivot.setPivotPosition(IntakePivot.EXTAKE_POS);
}
}
6 changes: 4 additions & 2 deletions src/main/java/frc/robot/subsystems/launcher/LaunchCalculator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,8 +107,10 @@ public static LaunchCalculator getInstance() {

// Filtered slip state (instance, persists across cycles)
private ChassisSpeeds filteredSlip = new ChassisSpeeds(0, 0, 0);
private DoublePublisher filteredSlipXPub = NetworkTableInstance.getDefault().getDoubleTopic("/LaunchCalculator/filteredX").publish();
private DoublePublisher filteredSlipYPub = NetworkTableInstance.getDefault().getDoubleTopic("/LaunchCalculator/filteredY").publish();
private DoublePublisher filteredSlipXPub =
NetworkTableInstance.getDefault().getDoubleTopic("/LaunchCalculator/filteredX").publish();
private DoublePublisher filteredSlipYPub =
NetworkTableInstance.getDefault().getDoubleTopic("/LaunchCalculator/filteredY").publish();

// Trench stuff
private static final AprilTagFieldLayout field = AllianceUtils.FIELD_LAYOUT;
Expand Down