Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
39 changes: 32 additions & 7 deletions src/main/java/frc/robot/Configs.java
Original file line number Diff line number Diff line change
Expand Up @@ -126,31 +126,56 @@ public static final class Shooter {
}
}

public static final TalonFXConfiguration TRANSFER_CONFIG = new TalonFXConfiguration();
public static final TalonFXConfiguration KICKER_CONFIG = new TalonFXConfiguration();

static {

TRANSFER_CONFIG
KICKER_CONFIG
.CurrentLimits
.SupplyCurrentLimit = Constants.Transfer.CURRENT_LIMIT;

TRANSFER_CONFIG
KICKER_CONFIG
.CurrentLimits
.SupplyCurrentLowerLimit = Constants.Transfer.CURRENT_LOWER_LIMIT;

TRANSFER_CONFIG
KICKER_CONFIG
.CurrentLimits
.SupplyCurrentLowerTime = Constants.Transfer.CURRENT_LOWER_TIME;

TRANSFER_CONFIG
KICKER_CONFIG
.MotorOutput
.Inverted = Constants.Transfer.MOTOR_INVERTED ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive;
.Inverted = Constants.Transfer.KICKER_INVERTED ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive;

TRANSFER_CONFIG
KICKER_CONFIG
.MotorOutput
.NeutralMode = NeutralModeValue.Brake;
}

public static final TalonFXConfiguration FEEDER_CONFIG = new TalonFXConfiguration();

static {
FEEDER_CONFIG
.CurrentLimits
.SupplyCurrentLimit = Constants.Transfer.CURRENT_LIMIT;

FEEDER_CONFIG
.CurrentLimits
.SupplyCurrentLowerLimit = Constants.Transfer.CURRENT_LOWER_LIMIT;

FEEDER_CONFIG
.CurrentLimits
.SupplyCurrentLowerTime = Constants.Transfer.CURRENT_LOWER_TIME;

FEEDER_CONFIG
.MotorOutput
.Inverted = Constants.Transfer.FEEDER_INVERTED ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive;

FEEDER_CONFIG
.MotorOutput
.NeutralMode = NeutralModeValue.Coast;

}

public static class Intake {
public static TalonFXConfiguration INTAKE_CONFIGURATION = new TalonFXConfiguration();

Expand Down
10 changes: 7 additions & 3 deletions src/main/java/frc/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -510,15 +510,19 @@ public static final class Shooter {

}
public static class Transfer {
public static final int TRANSFER_MOTOR_ID = 58;
public static final int TRANSFER_KICKER_ID = 58;
public static final int TRANSFER_FEEDER_ID = 57;

public static final int CURRENT_LIMIT = 75; //Amps
public static final int CURRENT_LOWER_LIMIT = 25;
public static final double CURRENT_LOWER_TIME = 0.5;

public static final boolean MOTOR_INVERTED = true;
public static final boolean KICKER_INVERTED = true;
public static final boolean FEEDER_INVERTED = true;


public static final double LOAD_POWER = 0.25;
public static final double KICKER_LOAD_POWER = 0.25;
public static final double FEEDER_LOAD_POWER = 0.5;
}

public static class VisionOdometryConstants {
Expand Down
41 changes: 18 additions & 23 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -198,14 +198,12 @@ public void setupSmartDashboard() {
.setSpeedMultiplierSupplier(() -> shooterPercent.get() / 100.0);

Command floatCommand = new ConditionalCommand(
Commands.parallel(
turretSubsystem.getFloatCommand(),
intakePivot.getFloatCommand()
)
.withTimeout(Constants.Operator.Misc.FLOAT_TIME),
Commands.none(),
() -> !FieldUtil.isEnabled()
).ignoringDisable(true);
Commands.parallel(
turretSubsystem.getFloatCommand(),
intakePivot.getFloatCommand())
.withTimeout(Constants.Operator.Misc.FLOAT_TIME),
Commands.none(),
() -> !FieldUtil.isEnabled()).ignoringDisable(true);

SmartDashboard.putData("FloatCommand", floatCommand);
SmartDashboard.putNumber("Auto/StartDelay", Constants.Operator.Auto.DEFAULT_START_DELAY);
Expand Down Expand Up @@ -369,7 +367,7 @@ public void configureBindings() {
Trigger intakeTrigger = new Trigger(() -> driverController.getRightBumperButton());

Trigger reverseTransferTrigger = new Trigger(() -> driverController.getXButton());

// Driver X button: hold to lock robot pose (X-lock)
Trigger xLockTrigger = new Trigger(() -> driverController.getXButton());
xLockTrigger.whileTrue(driveSubsystem.getLockPoseCommand());
Expand All @@ -380,13 +378,10 @@ public void configureBindings() {
targetingSupplier,
driveSubsystem::getPose,
driveSubsystem::getVelocity));



/* Transfer runs ONLY while button AND solver valid */
shootTrigger
.whileTrue(
transferSubsystem.getLoadCommand());
shootTrigger.whileTrue(
transferSubsystem.getLoadCommand());

/* Intake pivot runs while button held */
intakeTrigger.whileTrue(
Expand All @@ -409,7 +404,7 @@ public void configureBindings() {
intakeRoller.getStopCommand()));

reverseTransferTrigger.whileTrue(
transferSubsystem.getSetPowerCommand(-1)).onFalse(transferSubsystem.getSetPowerCommand(0));
transferSubsystem.getPowerCommand(-1.0, -1.0)).onFalse(transferSubsystem.getStopCommand());

// =========================
// Turret Offset Adjustment (POV Left / Right)
Expand Down Expand Up @@ -510,10 +505,10 @@ public void configureBindings() {
new InstantCommand(() -> xHeld = false));

// new Trigger(
// () -> operatorController.getYButton()).onTrue(
// new InstantCommand(() -> yHeld = true))
// .onFalse(
// new InstantCommand(() -> yHeld = false));
// () -> operatorController.getYButton()).onTrue(
// new InstantCommand(() -> yHeld = true))
// .onFalse(
// new InstantCommand(() -> yHeld = false));

}

Expand Down Expand Up @@ -716,12 +711,12 @@ private Command getIntakeStrategyCommand() {

case B_N_F_R:
return new DriveToSequenceCommand(driveSubsystem, Constants.Operator.Auto.BACK_N_FORTH_RIGHT_SEQUENCE.stream()
.map(FieldUtil::flipIfRed)
.toList());
.map(FieldUtil::flipIfRed)
.toList());
case B_N_F_L:
return new DriveToSequenceCommand(driveSubsystem, Constants.Operator.Auto.BACK_N_FORTH_LEFT_SEQUENCE.stream()
.map(FieldUtil::flipIfRed)
.toList());
.map(FieldUtil::flipIfRed)
.toList());
default:
return Commands.none();
}
Expand Down
43 changes: 30 additions & 13 deletions src/main/java/frc/robot/subsystems/TransferSubsystem.java
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,8 @@ public class TransferSubsystem extends SubsystemBase {
// --------------------------------------------------------------------

/** Motor driving the transfer mechanism. */
private final TalonFX transferMotor;
private final TalonFX kickerMotor;
private final TalonFX feederMotor;

// --------------------------------------------------------------------
// Construction / Configuration
Expand All @@ -41,8 +42,11 @@ public class TransferSubsystem extends SubsystemBase {
* Creates and configures the transfer subsystem.
*/
public TransferSubsystem() {
transferMotor = new TalonFX(Constants.Transfer.TRANSFER_MOTOR_ID, Constants.CANIVORE_LOOP_NAME);
transferMotor.getConfigurator().apply(Configs.TRANSFER_CONFIG);
kickerMotor = new TalonFX(Constants.Transfer.TRANSFER_KICKER_ID, Constants.CANIVORE_LOOP_NAME);
kickerMotor.getConfigurator().apply(Configs.KICKER_CONFIG);

feederMotor = new TalonFX(Constants.Transfer.TRANSFER_FEEDER_ID);
feederMotor.getConfigurator().apply(Configs.FEEDER_CONFIG);
}

// --------------------------------------------------------------------
Expand All @@ -54,8 +58,12 @@ public TransferSubsystem() {
*
* @param power desired power
*/
public void setPower(double power) {
transferMotor.set(power);
public void setKickerPower(double power) {
kickerMotor.set(power);
}

public void setFeederPower(double power) {
feederMotor.set(power);
}

// --------------------------------------------------------------------
Expand All @@ -71,24 +79,33 @@ public void setPower(double power) {
* @param power desired transfer power
* @return an instant command that sets motor power
*/
public Command getSetPowerCommand(double power) {
public Command getPowerCommand(double kickerPower, double feederPower) {
return new InstantCommand(
() -> setPower(power),
this
() -> {
setKickerPower(kickerPower);
setFeederPower(feederPower);
}
);
}

/**
* @return a command that runs the transfer at the load speed
*/
public Command getLoadCommand() {
return getSetPowerCommand(Constants.Transfer.LOAD_POWER).withName("TransferLoad");
return new InstantCommand(
() -> {
setKickerPower(Constants.Transfer.KICKER_LOAD_POWER);
setFeederPower(Constants.Transfer.FEEDER_LOAD_POWER);
}
);
}

/**
* @return a command that stops the transfer motor
*/
public Command getStopCommand() {
return new InstantCommand(() -> transferMotor.stopMotor());
return new InstantCommand(
() -> {
kickerMotor.stopMotor();
feederMotor.stopMotor();
}
);
}
}
Loading