Skip to content

Commit 0526a58

Browse files
CrolineCroisCrolineCrois
committed
practice day changes
1 parent e7b3714 commit 0526a58

File tree

5 files changed

+105
-33
lines changed

5 files changed

+105
-33
lines changed

‎src/main/java/frc/robot/Constants.java

+1-1
Original file line numberDiff line numberDiff line change
@@ -98,7 +98,7 @@ public static class IntakeConstants {
9898
public static final int BACK_MOTOR_ID = 18;
9999
public static final int PIVOT_MOTOR_ID = 16;
100100

101-
public static final double PIVOT_P = 1;
101+
public static final double PIVOT_P = 2.5;
102102
public static final double PIVOT_I = 0;
103103
public static final double PIVOT_D = 0;
104104
public static final double PIVOT_CONVERSION_FACTOR = 0.2142;

‎src/main/java/frc/robot/RobotContainer.java

+60-24
Original file line numberDiff line numberDiff line change
@@ -463,7 +463,9 @@ private void configureBindings() {
463463
/* ElEVATOR TEST */
464464

465465
dPadUp.onTrue(new ElevatorToTrapCommand(elevatorSubsystem));
466+
dPadUp.onTrue(new InstantCommand(() -> intakePivotSubsystem.enablePowerLimit(false)));
466467
dPadDown.onTrue(new ElevatorToLimitSwitchCommand(elevatorSubsystem));
468+
dPadDown.onTrue(new InstantCommand(() -> intakePivotSubsystem.enablePowerLimit(true)));
467469

468470
// elevatorSubsystem.setManual();
469471

@@ -510,31 +512,40 @@ private void configureBindings() {
510512
// if the elevator is down, run the amp sequence
511513
rightBumper.onTrue(
512514
new ConditionalCommand(
513-
// if elevator is up
514-
new ElevatorToZeroCommand(elevatorSubsystem).alongWith(new InstantCommand(// lower the elevator
515-
() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem)), // stow the pivot
516-
517-
// if elevator is down
518-
new PrepareAmpSequence(elevatorSubsystem, intakePivotSubsystem, intakeRollerSubsystem)
519-
.until(() -> mechController.getLeftTriggerAxis() > .05
520-
|| mechController.getRightTriggerAxis() > .05),
521-
522-
// check if the elevator is currently targeting one of the upper positions to choose what to do
523-
() -> elevatorSubsystem.getTargetState() == ElevatorState.AMP
524-
|| elevatorSubsystem.getTargetState() == ElevatorState.TRAP));
515+
// if elevator is up
516+
new ElevatorToZeroCommand(elevatorSubsystem).alongWith(
517+
Commands.runOnce(() -> {
518+
intakePivotSubsystem.enablePowerLimit(true);
519+
intakePivotSubsystem.setPosition(0);
520+
}, intakePivotSubsystem)
521+
), // stow the pivot
522+
523+
// if elevator is down
524+
new PrepareAmpSequence(elevatorSubsystem, intakePivotSubsystem, intakeRollerSubsystem)
525+
.until(() -> mechController.getLeftTriggerAxis() > .05
526+
|| mechController.getRightTriggerAxis() > .05),
527+
528+
// check if the elevator is currently targeting one of the upper positions to choose what to do
529+
() -> elevatorSubsystem.getTargetState() == ElevatorState.AMP
530+
|| elevatorSubsystem.getTargetState() == ElevatorState.TRAP
531+
)
532+
);
525533

526534
// leftBumper toggles the trap position for the elevator
527535
leftBumper.onTrue(
528536
new ConditionalCommand(
529537
new ElevatorToZeroCommand(elevatorSubsystem).alongWith(new InstantCommand(// lower the elevator
530-
() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem)), // stow intake
538+
() -> {
539+
intakePivotSubsystem.enablePowerLimit(true);
540+
intakePivotSubsystem.setPosition(0);
541+
}, intakePivotSubsystem)), // stow intake
531542
new ConditionalCommand(
532543
new ElevatorToTrapCommand(elevatorSubsystem).andThen(
533-
new IntakePivotSetPositionCommand(intakePivotSubsystem, .45).withTimeout(.1)
544+
new InstantCommand(() -> intakePivotSubsystem.enablePowerLimit(false)),
545+
new IntakePivotSetPositionCommand(intakePivotSubsystem, .45)
534546
),
535547
new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen(// extend pivot
536-
new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor
537-
.until(() -> intakeRollerSubsystem.getFrontSensorReached()),
548+
new ConditionalWaitCommand(intakeRollerSubsystem::getFrontSensorReached),
538549
new IntakePivotSetPositionCommand(intakePivotSubsystem, 0)
539550
),
540551
intakeRollerSubsystem::getAmpSensor
@@ -544,18 +555,36 @@ private void configureBindings() {
544555
);
545556

546557

558+
// roll behaviors
559+
leftBumper.onTrue(
560+
new ConditionalCommand(
561+
null,
562+
new ConditionalCommand(
563+
null,
564+
new WaitCommand(.05).andThen(
565+
new ConditionalWaitCommand(intakePivotSubsystem::atPosition), // extend pivot
566+
new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor
567+
.until(intakeRollerSubsystem::getFrontSensorReached)
568+
),
569+
intakeRollerSubsystem::getAmpSensor
570+
), // raise the elevator
571+
() -> elevatorSubsystem.getTargetState() == ElevatorState.AMP // check if targeting a high pos
572+
|| elevatorSubsystem.getTargetState() == ElevatorState.TRAP)
573+
);
574+
575+
547576

548577
// aButton runs the intake sequence
549578
aButton.onTrue(
550-
new InstantCommand(() -> {intakePivotSubsystem.setPosition(1);}, intakePivotSubsystem).alongWith(// then extend the intake
579+
Commands.runOnce(() -> intakePivotSubsystem.setPosition(1), intakePivotSubsystem).alongWith(
551580
new IntakeRollerAmpIntakeCommand(intakeRollerSubsystem)).andThen(
552581
new ConditionalCommand(
553582
new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem).andThen(
554-
new InstantCommand(() -> {intakePivotSubsystem.setPosition(0);}, intakePivotSubsystem)
583+
new InstantCommand(() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem)
555584
),
556585

557586
new InstantCommand(
558-
() -> {intakePivotSubsystem.setPosition(0);}, intakePivotSubsystem
587+
() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem
559588
),
560589
() -> aButton.getAsBoolean()
561590
)
@@ -564,7 +593,7 @@ private void configureBindings() {
564593
);
565594

566595
// bButton stops the rollers
567-
bButton.onTrue(new InstantCommand(() -> {}, intakeRollerSubsystem));
596+
bButton.onTrue(Commands.idle(intakeRollerSubsystem));
568597

569598
// xButton toggles the intake being stowed
570599
xButton.onTrue(new InstantCommand(() -> {
@@ -592,14 +621,18 @@ private void configureBindings() {
592621
new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen(
593622
new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem),
594623
new IntakePivotSetPositionCommand(intakePivotSubsystem, intakePosition)
595-
).unless(intakeRollerSubsystem::getRockwellSensorValue)
624+
).unless(intakeRollerSubsystem::getRockwellSensorValue).andThen(
625+
new IntakePivotSetPositionCommand(intakePivotSubsystem, 0)
626+
)
596627
);
597628

598629
shooterFlywheelSubsystem.setDefaultCommand(new InstantCommand(() -> {
599630
if (yButton.getAsBoolean()) {
600631
lightBarSubsystem.setLightBarStatus(LightBarStatus.SHOOTER_SPIN_UP, 2);
601632
// shooterFlywheelSubsystem.setShooterMotorSpeed(shooterSpeed); // for tuning
602-
shooterFlywheelSubsystem.setShooterMotorSpeed();
633+
if (intakePivotSubsystem.atPosition()) {
634+
shooterFlywheelSubsystem.setShooterMotorSpeed();
635+
}
603636
shooterPivotSubsystem.setAutoAimBoolean(true);
604637
if (shooterFlywheelSubsystem.atSpeed()) {
605638
mechController.setRumble(RumbleType.kBothRumble, .4);
@@ -658,8 +691,11 @@ private void configureBindings() {
658691
new IntakePivotSetPositionCommand(intakePivotSubsystem, intakePosition)
659692
).unless(intakeRollerSubsystem::getRockwellSensorValue));
660693

661-
dPadRight.onTrue(new ShooterFlywheelShuttleCommand(swerveSubsystem, shooterFlywheelSubsystem, fmsSubsystem, shooterPivotSubsystem, .65, mechController).onlyWhile(dPadRight));
662-
694+
dPadRight.whileTrue(
695+
new ConditionalWaitCommand(() -> intakePivotSubsystem.atPosition())
696+
.andThen(new ShooterFlywheelShuttleCommand(swerveSubsystem, shooterFlywheelSubsystem, fmsSubsystem,
697+
shooterPivotSubsystem, .7, mechController))
698+
);
663699
// intakePivotSubsystem.setDefaultCommand(new InstantCommand(() -> {
664700
// intakePosition = MathUtil.clamp(intakePosition, 0, 1);
665701
// intakePivotSubsystem.setPosition(intakePosition);

‎src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java

+7-4
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,8 @@
99
public class IntakeRollerIntakeCommand extends Command {
1010
private final IntakeRollerSubsystem intakeSubsystem;
1111
private final LightBarSubsystem lightBarSubsystem;
12-
private double speed = .7;
12+
private double frontSpeed = .7;
13+
private double integrationSpeed = .4;
1314

1415
/**
1516
* Runs the rollers on the intake until a note is detected.
@@ -23,11 +24,12 @@ public IntakeRollerIntakeCommand(IntakeRollerSubsystem intakeSubsystem, LightBar
2324
addRequirements(intakeSubsystem);
2425
}
2526

26-
public IntakeRollerIntakeCommand(IntakeRollerSubsystem intakeSubsystem, LightBarSubsystem lightBarSubsystem, double speed) {
27+
public IntakeRollerIntakeCommand(IntakeRollerSubsystem intakeSubsystem, LightBarSubsystem lightBarSubsystem, double speed, double integrationSpeed) {
2728
this.intakeSubsystem = intakeSubsystem;
2829
this.lightBarSubsystem = lightBarSubsystem;
2930
addRequirements(intakeSubsystem);
30-
this.speed = speed;
31+
this.frontSpeed = speed;
32+
this.integrationSpeed = integrationSpeed;
3133
}
3234

3335
@Override
@@ -37,7 +39,8 @@ public void initialize() {
3739

3840
@Override
3941
public void execute() {
40-
intakeSubsystem.setRollSpeeds(speed, speed);
42+
intakeSubsystem.setRollSpeeds(frontSpeed, integrationSpeed);
43+
System.out.println(intakeSubsystem.getRockwellSensorValue());
4144
}
4245

4346
@Override

‎src/main/java/frc/robot/subsystems/intake/IntakePivotSubsystem.java

+36-3
Original file line numberDiff line numberDiff line change
@@ -3,13 +3,14 @@
33
import static frc.robot.Constants.IntakeConstants;
44

55
import com.ctre.phoenix6.configs.Slot0Configs;
6+
import com.ctre.phoenix6.configs.VoltageConfigs;
67
import com.ctre.phoenix6.controls.PositionVoltage;
78
import com.ctre.phoenix6.hardware.TalonFX;
89
import com.ctre.phoenix6.signals.NeutralModeValue;
9-
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1010
import edu.wpi.first.networktables.NetworkTable;
11-
import edu.wpi.first.networktables.NetworkTableInstance;
1211
import edu.wpi.first.networktables.NetworkTableEntry;
12+
import edu.wpi.first.networktables.NetworkTableInstance;
13+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
1314

1415

1516
/** The subsystem for the pivot on the intake. */
@@ -18,7 +19,9 @@ public class IntakePivotSubsystem extends SubsystemBase {
1819
private final TalonFX pivotMotor;
1920

2021
private PositionVoltage request = new PositionVoltage(0).withSlot(0);
22+
private VoltageConfigs voltageConfig = new VoltageConfigs();
2123
private double setPos = 0;
24+
private static final double FORWARD_VOLTAGE = 1.75;
2225

2326
private NetworkTableInstance ntInstance;
2427
private NetworkTable motorsNTTable;
@@ -32,6 +35,7 @@ public class IntakePivotSubsystem extends SubsystemBase {
3235
public IntakePivotSubsystem() {
3336
pivotMotor = new TalonFX(IntakeConstants.PIVOT_MOTOR_ID);
3437
pivotMotor.setNeutralMode(NeutralModeValue.Brake);
38+
3539
// intakeencoder = new Encoder(1, 2);
3640
// extendedlimitswitch = new DigitalInput(extendedlimitswitchID);
3741
// retractedlimitswitch = new DigitalInput(retractedlimitswitchID);
@@ -42,8 +46,16 @@ public IntakePivotSubsystem() {
4246
slot0Configs.kI = IntakeConstants.PIVOT_I;
4347
slot0Configs.kD = IntakeConstants.PIVOT_D;
4448

49+
voltageConfig.PeakForwardVoltage = FORWARD_VOLTAGE;
50+
voltageConfig.PeakReverseVoltage = -16;
51+
52+
4553
pivotMotor.getConfigurator().apply(slot0Configs);
4654
pivotMotor.setPosition(0);
55+
pivotMotor.getConfigurator().apply(voltageConfig);
56+
57+
58+
request.withLimitForwardMotion(false);
4759

4860
ntInstance = NetworkTableInstance.getDefault();
4961
motorsNTTable = ntInstance.getTable("Motors");
@@ -77,6 +89,11 @@ public double getEncoderPosition() {
7789
return pivotMotor.getPosition().getValueAsDouble() * IntakeConstants.PIVOT_CONVERSION_FACTOR;
7890
}
7991

92+
/** Returns whether or not the intake is at (or within tolerance of) its setpoint. */
93+
public boolean atPosition() {
94+
return Math.abs(getEncoderPosition() - setPos) < .05;
95+
}
96+
8097
/**
8198
* Sets the pivot motor to a speed.
8299
*
@@ -86,10 +103,26 @@ public void movePivot(double speed) {
86103
pivotMotor.set(speed);
87104
}
88105

106+
/**
107+
* Enables/disables the pivot motor's forward voltage limit for deploying the intake.
108+
* When enabled, the linkage will not slam down when deploying.
109+
*
110+
* @param enabled True enables the limit, false disables it.
111+
*/
112+
public void enablePowerLimit(boolean enabled) {
113+
if (enabled) {
114+
voltageConfig.PeakForwardVoltage = FORWARD_VOLTAGE;
115+
} else {
116+
voltageConfig.PeakForwardVoltage = 16;
117+
}
118+
119+
pivotMotor.getConfigurator().apply(voltageConfig);
120+
}
121+
89122
@Override
90123
public void periodic() {
91124
pivotMotor.setControl(request.withPosition(setPos / IntakeConstants.PIVOT_CONVERSION_FACTOR));
92-
125+
93126
intake16CurrentEntry.setDouble(pivotMotor.getSupplyCurrent().getValueAsDouble());
94127
intake16VoltageEntry.setDouble(pivotMotor.getMotorVoltage().getValueAsDouble());
95128
intake16TemperatureEntry.setDouble(pivotMotor.getDeviceTemp().getValueAsDouble());

‎src/main/java/frc/robot/subsystems/shooter/ShooterPivotSubsystem.java

+1-1
Original file line numberDiff line numberDiff line change
@@ -109,7 +109,7 @@ public ShooterPivotSubsystem(DoubleSupplier distanceSupplier, BooleanSupplier re
109109
5,
110110
6,
111111
ShooterConstants.MAX_SHOOTER_DISTANCE};
112-
double[] angles = {Units.degreesToRadians(60),
112+
double[] angles = {Units.degreesToRadians(59),
113113
Units.degreesToRadians(52),
114114
Units.degreesToRadians(36),
115115
Units.degreesToRadians(31), //4

0 commit comments

Comments
 (0)