Skip to content

Commit 6a88c39

Browse files
CrolineCroisCrolineCrois
committed
actual testable mode
1 parent a8ca983 commit 6a88c39

File tree

2 files changed

+185
-184
lines changed

2 files changed

+185
-184
lines changed

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

+153-152
Original file line numberDiff line numberDiff line change
@@ -12,11 +12,11 @@
1212

1313
import com.choreo.lib.ChoreoTrajectory;
1414
import com.fasterxml.jackson.databind.util.Named;
15-
import com.pathplanner.lib.auto.AutoBuilder;
16-
import com.pathplanner.lib.auto.NamedCommands;
17-
import com.pathplanner.lib.commands.PathPlannerAuto;
18-
import com.pathplanner.lib.path.PathPlannerPath;
19-
import com.pathplanner.lib.util.PathPlannerLogging;
15+
// import com.pathplanner.lib.auto.AutoBuilder;
16+
// import com.pathplanner.lib.auto.NamedCommands;
17+
// import com.pathplanner.lib.commands.PathPlannerAuto;
18+
// import com.pathplanner.lib.path.PathPlannerPath;
19+
// import com.pathplanner.lib.util.PathPlannerLogging;
2020

2121
import edu.wpi.first.cscore.MjpegServer;
2222
import edu.wpi.first.cscore.UsbCamera;
@@ -101,15 +101,15 @@ public class RobotContainer {
101101
private final BaseDriveController driveController;
102102
private final SwerveSubsystem swerveSubsystem;
103103

104-
// private final IntakePivotSubsystem intakePivotSubsystem;
105-
// private final IntakeRollerSubsystem intakeRollerSubsystem;
104+
private final IntakePivotSubsystem intakePivotSubsystem;
105+
private final IntakeRollerSubsystem intakeRollerSubsystem;
106106

107-
// private final ShooterFlywheelSubsystem shooterFlywheelSubsystem;
108-
// private final ShooterPivotSubsystem shooterPivotSubsystem;
107+
private final ShooterFlywheelSubsystem shooterFlywheelSubsystem;
108+
private final ShooterPivotSubsystem shooterPivotSubsystem;
109109

110110
// private final ClimbSubsystem climbSubsystem;
111111

112-
// private final ElevatorSubsystem elevatorSubsystem;
112+
private final ElevatorSubsystem elevatorSubsystem;
113113

114114
private final FieldManagementSubsystem fmsSubsystem;
115115
private final LightBarSubsystem lightBarSubsystem;
@@ -185,7 +185,7 @@ public RobotContainer() {
185185
listEntry = autonTable.getEntry("AutonList");
186186
selectedAutonEntry = autonTable.getEntry("Selected");
187187
// System.out.print("Available Autons: " + AutoBuilder.getAllAutoNames().toArray(new String[0]));
188-
listEntry.setStringArray(AutoBuilder.getAllAutoNames().toArray(new String[0]));
188+
// listEntry.setStringArray(AutoBuilder.getAllAutoNames().toArray(new String[0]));
189189
selectedAutonEntry.setString(autonValue);
190190
autonTable.addListener("Auton", EnumSet.of(NetworkTableEvent.Kind.kValueAll), (table, key, event) -> {
191191
this.autonValue = event.valueData.value.getString();
@@ -199,19 +199,19 @@ public RobotContainer() {
199199
swerveSubsystem = new SwerveSubsystem(fmsSubsystem::isRedAlliance);
200200
swerveSubsystem.setVerbose(false); // SET THIS TO true FOR TUNING VALUES
201201

202-
// intakePivotSubsystem = new IntakePivotSubsystem();
203-
// intakeRollerSubsystem = new IntakeRollerSubsystem(lightBarSubsystem);
202+
intakePivotSubsystem = new IntakePivotSubsystem();
203+
intakeRollerSubsystem = new IntakeRollerSubsystem(lightBarSubsystem);
204204

205-
// shooterPivotSubsystem = new ShooterPivotSubsystem(
206-
// swerveSubsystem::getShootingDistance,
207-
// fmsSubsystem::isRedAlliance
208-
// );
209-
// shooterFlywheelSubsystem = new ShooterFlywheelSubsystem(
210-
// swerveSubsystem::getShootingDistance,
211-
// fmsSubsystem::isRedAlliance
212-
// );
205+
shooterPivotSubsystem = new ShooterPivotSubsystem(
206+
swerveSubsystem::getShootingDistance,
207+
fmsSubsystem::isRedAlliance
208+
);
209+
shooterFlywheelSubsystem = new ShooterFlywheelSubsystem(
210+
swerveSubsystem::getShootingDistance,
211+
fmsSubsystem::isRedAlliance
212+
);
213213

214-
// elevatorSubsystem = new ElevatorSubsystem();
214+
elevatorSubsystem = new ElevatorSubsystem();
215215

216216
// climbSubsystem = new ClimbSubsystem();
217217

@@ -342,7 +342,7 @@ private void configureBindings() {
342342
xError.setValue(xPID.getPositionError());
343343
yError.setValue(yPID.getPositionError());
344344

345-
}, swerveSubsystem));}
345+
}, swerveSubsystem));
346346

347347
// /* Pressing the button resets the field axes to the current robot axes. */
348348
// driveController.getDriverHeadingResetButton().onTrue(new InstantCommand(() -> {
@@ -490,8 +490,8 @@ private void configureBindings() {
490490
// elevatorToZero.onTrue(new ElevatorToLimitSwitchCommand(elevatorSubsystem));
491491
/* INTAKE TEST */
492492

493-
// xButton.onTrue(new InstantCommand(() -> intakePivotSubsystem.setPosition(.3),
494-
// intakePivotSubsystem));
493+
xButton.onTrue(new InstantCommand(() -> intakePivotSubsystem.setPosition(.3),
494+
intakePivotSubsystem));
495495

496496
// rightBumper.onTrue(new InstantCommand(() ->
497497
// intakePivotSubsystem.setPosition(0), intakePivotSubsystem));
@@ -562,138 +562,139 @@ private void configureBindings() {
562562
// );
563563

564564

565-
// // roll behaviors
566-
// leftBumper.onTrue(
567-
// new ConditionalCommand(
568-
// new InstantCommand(),
569-
// new ConditionalCommand(
570-
// new InstantCommand(),
571-
// new WaitCommand(.05).andThen(
572-
// new ConditionalWaitCommand(intakePivotSubsystem::atPosition), // extend pivot
573-
// new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor
574-
// .until(intakeRollerSubsystem::getFrontSensorReached)
575-
// ),
576-
// intakeRollerSubsystem::getAmpSensor
577-
// ), // raise the elevator
578-
// () -> elevatorSubsystem.getTargetState() == ElevatorState.AMP // check if targeting a high pos
579-
// || elevatorSubsystem.getTargetState() == ElevatorState.TRAP)
580-
// );
565+
// roll behaviors
566+
leftBumper.onTrue(
567+
new ConditionalCommand(
568+
new InstantCommand(),
569+
new ConditionalCommand(
570+
new InstantCommand(),
571+
new WaitCommand(.05).andThen(
572+
new ConditionalWaitCommand(intakePivotSubsystem::atPosition), // extend pivot
573+
new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor
574+
.until(intakeRollerSubsystem::getFrontSensorReached)
575+
),
576+
intakeRollerSubsystem::getAmpSensor
577+
), // raise the elevator
578+
() -> elevatorSubsystem.getTargetState() == ElevatorState.AMP // check if targeting a high pos
579+
|| elevatorSubsystem.getTargetState() == ElevatorState.TRAP)
580+
);
581+
582+
583+
584+
// aButton runs the intake sequence
585+
aButton.onTrue(
586+
Commands.runOnce(() -> intakePivotSubsystem.setPosition(1), intakePivotSubsystem).alongWith(
587+
new IntakeRollerAmpIntakeCommand(intakeRollerSubsystem)).andThen(
588+
new ConditionalCommand(
589+
new WaitCommand(.3).andThen(
590+
new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem).andThen(
591+
new InstantCommand(() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem)
592+
)
593+
),
594+
595+
new InstantCommand(
596+
() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem
597+
),
598+
aButton
599+
)
600+
).until(() -> mechController.getLeftTriggerAxis() > .1) // cancel if try to outtake
601+
602+
);
603+
604+
// bButton stops the rollers
605+
bButton.onTrue(Commands.idle(intakeRollerSubsystem).withTimeout(0));
606+
607+
// xButton toggles the intake being stowed
608+
xButton.onTrue(new InstantCommand(() -> {
609+
double outPosition = 1;
610+
double stowPosition = 0;
611+
if (elevatorSubsystem.getExtensionPercent() > .5
612+
&& elevatorSubsystem.getTargetState() == ElevatorState.TRAP) {
613+
outPosition = .45; // push intake out
614+
} else if (elevatorSubsystem.getExtensionPercent() > .5
615+
&& elevatorSubsystem.getTargetState() == ElevatorState.AMP) {
616+
stowPosition = .2;
617+
}
581618

619+
intakePivotSubsystem.setPosition(
620+
intakePivotSubsystem.getEncoderPosition() < (outPosition + stowPosition) / 2
621+
? outPosition
622+
: stowPosition
623+
);
582624

625+
}, intakePivotSubsystem));
583626

584-
// // aButton runs the intake sequence
585-
// aButton.onTrue(
586-
// Commands.runOnce(() -> intakePivotSubsystem.setPosition(1), intakePivotSubsystem).alongWith(
587-
// new IntakeRollerAmpIntakeCommand(intakeRollerSubsystem)).andThen(
588-
// new ConditionalCommand(
589-
// new WaitCommand(.3).andThen(
590-
// new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem).andThen(
591-
// new InstantCommand(() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem)
592-
// )
593-
// ),
594-
595-
// new InstantCommand(
596-
// () -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem
597-
// ),
598-
// aButton
599-
// )
600-
// ).until(() -> mechController.getLeftTriggerAxis() > .1) // cancel if try to outtake
601-
602-
// );
627+
// yButton runs the flywheels
603628

604-
// // bButton stops the rollers
605-
// bButton.onTrue(Commands.idle(intakeRollerSubsystem).withTimeout(0));
606-
607-
// // xButton toggles the intake being stowed
608-
// xButton.onTrue(new InstantCommand(() -> {
609-
// double outPosition = 1;
610-
// double stowPosition = 0;
611-
// if (elevatorSubsystem.getExtensionPercent() > .5
612-
// && elevatorSubsystem.getTargetState() == ElevatorState.TRAP) {
613-
// outPosition = .45; // push intake out
614-
// } else if (elevatorSubsystem.getExtensionPercent() > .5
615-
// && elevatorSubsystem.getTargetState() == ElevatorState.AMP) {
616-
// stowPosition = .2;
617-
// }
618-
619-
// intakePivotSubsystem.setPosition(
620-
// intakePivotSubsystem.getEncoderPosition() < (outPosition + stowPosition) / 2
621-
// ? outPosition
622-
// : stowPosition
623-
// );
624-
625-
// }, intakePivotSubsystem));
626-
627-
// // yButton runs the flywheels
628-
629-
// yButton.onTrue(
630-
// new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen(
631-
// new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem),
632-
// new IntakePivotSetPositionCommand(intakePivotSubsystem, intakePosition)
633-
// ).unless(intakeRollerSubsystem::getRockwellSensorValue).andThen(
634-
// new IntakePivotSetPositionCommand(intakePivotSubsystem, 0)
635-
// )
636-
// );
629+
yButton.onTrue(
630+
new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen(
631+
new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem),
632+
new IntakePivotSetPositionCommand(intakePivotSubsystem, intakePosition)
633+
).unless(intakeRollerSubsystem::getRockwellSensorValue).andThen(
634+
new IntakePivotSetPositionCommand(intakePivotSubsystem, 0)
635+
)
636+
);
637+
637638

638-
// shooterFlywheelSubsystem.setDefaultCommand(new InstantCommand(() -> {
639-
// if (yButton.getAsBoolean()) {
640-
// lightBarSubsystem.setLightBarStatus(LightBarStatus.SHOOTER_SPIN_UP, 2);
641-
// // shooterFlywheelSubsystem.setShooterMotorSpeed(shooterSpeed); // for tuning
639+
shooterFlywheelSubsystem.setDefaultCommand(new InstantCommand(() -> {
640+
if (yButton.getAsBoolean()) {
641+
lightBarSubsystem.setLightBarStatus(LightBarStatus.SHOOTER_SPIN_UP, 2);
642+
// shooterFlywheelSubsystem.setShooterMotorSpeed(shooterSpeed); // for tuning
642643

643-
// shooterFlywheelSubsystem.setShooterMotorSpeed();
644+
shooterFlywheelSubsystem.setShooterMotorSpeed();
644645

645-
// shooterPivotSubsystem.setAutoAimBoolean(true);
646-
// if (shooterFlywheelSubsystem.atSpeed()) {
647-
// mechController.setRumble(RumbleType.kBothRumble, .4);
648-
// } else {
649-
// mechController.setRumble(RumbleType.kBothRumble, 0);
650-
// if (lightBarSubsystem.getLightBarMechStatus() == LightBarStatus.SHOOTER_SPIN_UP) {
651-
// double top = shooterFlywheelSubsystem.getTopSpeed()
652-
// / shooterFlywheelSubsystem.getTargetTopRPS();
653-
// double bottom = shooterFlywheelSubsystem.getBottomSpeed()
654-
// / shooterFlywheelSubsystem.getTargetBottomRPS();
655-
// double avg = (top + bottom) / 2; // in case they're different, this just shows the average.
656-
657-
// lightBarSubsystem.updateShooterSpeedPercentage(avg);
658-
// }
646+
shooterPivotSubsystem.setAutoAimBoolean(true);
647+
if (shooterFlywheelSubsystem.atSpeed()) {
648+
mechController.setRumble(RumbleType.kBothRumble, .4);
649+
} else {
650+
mechController.setRumble(RumbleType.kBothRumble, 0);
651+
if (lightBarSubsystem.getLightBarMechStatus() == LightBarStatus.SHOOTER_SPIN_UP) {
652+
double top = shooterFlywheelSubsystem.getTopSpeed()
653+
/ shooterFlywheelSubsystem.getTargetTopRPS();
654+
double bottom = shooterFlywheelSubsystem.getBottomSpeed()
655+
/ shooterFlywheelSubsystem.getTargetBottomRPS();
656+
double avg = (top + bottom) / 2; // in case they're different, this just shows the average.
657+
658+
lightBarSubsystem.updateShooterSpeedPercentage(avg);
659+
}
659660

660-
// }
661-
// } else {
662-
// // if(fmsSubsystem.getMatchStatus() != MatchStatus.AUTON){
663-
// // shooterPivotSubsystem.setAutoAimBoolean(false);
664-
// // }
665-
// if (mechController.getPOV() == 90) {
666-
667-
// if (shooterFlywheelSubsystem.atSpeed()) {
668-
// mechController.setRumble(RumbleType.kBothRumble, .4);
669-
// } else {
670-
// mechController.setRumble(RumbleType.kBothRumble, 0);
671-
// }
672-
// } else {
673-
// shooterFlywheelSubsystem.stopShooter();
674-
// mechController.setRumble(RumbleType.kBothRumble, 0);
675-
// }
676-
// }
677-
// if (shooterFlywheelSubsystem.atSpeed()) {
678-
// mechController.setRumble(RumbleType.kBothRumble, .4);
679-
// } else {
680-
// mechController.setRumble(RumbleType.kBothRumble, 0);
681-
// }
682-
683-
// if (noteInBack
684-
// && !intakeRollerSubsystem.getRockwellSensorValue()
685-
// && intakeRollerSubsystem.getIntegrationSpeed() > 0
686-
// ) {
687-
// System.out.println("Dist: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getShootingDistance())
688-
// + " Angle: " + GRTUtil.twoDecimals(Units.radiansToDegrees(shooterPivotSubsystem.getPosition()))
689-
// + " Speed: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getSplineSpeed()));
690-
// }
691-
// noteInBack = intakeRollerSubsystem.getRockwellSensorValue();
692-
693-
// // if we are at speed, rumble the mech controller
661+
}
662+
} else {
663+
// if(fmsSubsystem.getMatchStatus() != MatchStatus.AUTON){
664+
// shooterPivotSubsystem.setAutoAimBoolean(false);
665+
// }
666+
if (mechController.getPOV() == 90) {
667+
668+
if (shooterFlywheelSubsystem.atSpeed()) {
669+
mechController.setRumble(RumbleType.kBothRumble, .4);
670+
} else {
671+
mechController.setRumble(RumbleType.kBothRumble, 0);
672+
}
673+
} else {
674+
shooterFlywheelSubsystem.stopShooter();
675+
mechController.setRumble(RumbleType.kBothRumble, 0);
676+
}
677+
}
678+
if (shooterFlywheelSubsystem.atSpeed()) {
679+
mechController.setRumble(RumbleType.kBothRumble, .4);
680+
} else {
681+
mechController.setRumble(RumbleType.kBothRumble, 0);
682+
}
683+
684+
if (noteInBack
685+
&& !intakeRollerSubsystem.getRockwellSensorValue()
686+
&& intakeRollerSubsystem.getIntegrationSpeed() > 0
687+
) {
688+
System.out.println("Dist: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getShootingDistance())
689+
+ " Angle: " + GRTUtil.twoDecimals(Units.radiansToDegrees(shooterPivotSubsystem.getPosition()))
690+
+ " Speed: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getSplineSpeed()));
691+
}
692+
noteInBack = intakeRollerSubsystem.getRockwellSensorValue();
693+
694+
// if we are at speed, rumble the mech controller
694695

695-
// }, shooterFlywheelSubsystem
696-
// ));
696+
}, shooterFlywheelSubsystem
697+
));
697698

698699
// dPadRight.onTrue(new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen(
699700
// new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem),
@@ -712,12 +713,12 @@ private void configureBindings() {
712713
// // }, intakePivotSubsystem));
713714

714715
// // The triggers intake/outtake the rollers
715-
// intakeRollerSubsystem.setDefaultCommand(new InstantCommand(() -> {
716+
intakeRollerSubsystem.setDefaultCommand(new InstantCommand(() -> {
716717

717-
// double power = .7 * (mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis());
718+
double power = .7 * (mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis());
718719

719-
// intakeRollerSubsystem.setRollSpeeds(power, power);
720-
// }, intakeRollerSubsystem));
720+
intakeRollerSubsystem.setRollSpeeds(power, power);
721+
}, intakeRollerSubsystem)); }
721722

722723
// // Offset buttons to correct the shooter if needed
723724
// offsetUpButton.onTrue(new InstantCommand(

0 commit comments

Comments
 (0)