import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
import frc.robot.commands.DoNothing;
import frc.robot.commands.drive_comm.DefaultDriveCommand;
import frc.robot.commands.vision.ShutdownAllPis;
import frc.robot.controls.BaseDriverConfig;
import frc.robot.controls.Operator;
import frc.robot.controls.PS5ControllerDriverConfig;
+import frc.robot.subsystems.Climb.LinearClimb;
import frc.robot.subsystems.Intake.Intake;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.drivetrain.GyroIOPigeon2;
private Command auto = new DoNothing();
-
// Controllers are defined here
private BaseDriverConfig driver = null;
private Operator operator = null;
+ private LinearClimb linearClimb = null;
+
+ // Auto Command selection
+ private final SendableChooser<Command> autoChooser = new SendableChooser<>();
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
// fall-through
case Vivace:
+ linearClimb = new LinearClimb();
case Phil: // AKA "IHOP"
case Vertigo: // AKA "French Toast"
drive = new Drivetrain(vision, new GyroIOPigeon2());
- driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer);
+ driver = new PS5ControllerDriverConfig(drive, linearClimb);
+ ((PS5ControllerDriverConfig)driver).setShooter(shooter);
+ ((PS5ControllerDriverConfig)driver).setTurret(turret);
+ ((PS5ControllerDriverConfig)driver).setHood(hood);
+ ((PS5ControllerDriverConfig)driver).setIntake(intake);
+ ((PS5ControllerDriverConfig)driver).setSpindexer(spindexer);
operator = new Operator(drive);
// added indexer here for now
-
// Detected objects need access to the drivetrain
DetectedObject.setDrive(drive);
-
+
// SignalLogger.start();
driver.configureControls();
operator.configureControls();
-
+
initializeAutoBuilder();
registerCommands();
PathGroupLoader.loadPathGroups();
}
drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
break;
- }
+ }
// This is really annoying so it's disabled
DriverStation.silenceJoystickConnectionWarning(true);
LiveWindow.setEnabled(false);
SmartDashboard.putData("Shutdown Orange Pis", new ShutdownAllPis());
+ autoChooserInit();
}
/**
drive.setVisionEnabled(enabled);
}
-
public void initializeAutoBuilder() {
AutoBuilder.configure(
() -> drive.getPose(),
public void registerCommands() {
}
+ /**
+ * Initialize the SendableChooser on the SmartDashboard.
+ * Fill the SendableChooser with available Commands.
+ */
+ public void autoChooserInit() {
+ // add the options to the Chooser
+ autoChooser.setDefaultOption("Do nothing", new DoNothing());
+ autoChooser.addOption("Do nada", new DoNothing());
+ autoChooser.addOption("Spin my wheels", new DoNothing());
+ autoChooser.addOption("Hello world", new InstantCommand(() -> System.out.println("Hello world")));
+
+ // put the Chooser on the SmartDashboard
+ SmartDashboard.putData("Auto chooser", autoChooser);
+ }
+
public static BooleanSupplier getAllianceColorBooleanSupplier() {
return () -> {
// Boolean supplier that controls when the path will be mirrored for the red
}
public boolean brownout() {
- if(RobotController.getBatteryVoltage() < 6.0) {
+ if (RobotController.getBatteryVoltage() < 6.0) {
return true;
- }
- else {
+ } else {
return false;
}
}
- public Command getAutoCommand(){
- return auto;
+ public Command getAutoCommand() {
+ // get the selected Command
+ Command autoSelected = autoChooser.getSelected();
+
+ return autoSelected;
}
- public void logComponents(){
- if(!Constants.LOG_MECHANISMS) return;
-
+ public void logComponents() {
+ if (!Constants.LOG_MECHANISMS)
+ return;
+
Logger.recordOutput(
- "ComponentPoses",
- new Pose3d[] {
+ "ComponentPoses",
+ new Pose3d[] {
// Subsystem Pose3ds
- }
- );
+ });
}
}
-
-
import frc.robot.Robot;
import frc.robot.commands.gpm.AutoShootCommand;
import frc.robot.constants.Constants;
+import frc.robot.subsystems.Climb.LinearClimb;
import frc.robot.subsystems.drivetrain.Drivetrain;
import frc.robot.subsystems.shooter.Shooter;
import frc.robot.subsystems.spindexer.Spindexer;
*/
public class PS5ControllerDriverConfig extends BaseDriverConfig {
private final PS5Controller driver = new PS5Controller(Constants.DRIVER_JOY);
- private final BooleanSupplier slowModeSupplier = ()->false;
+ private final BooleanSupplier slowModeSupplier = () -> false;
+ private boolean intakeBoolean = true;
+ private Command autoShoot = null;
private Shooter shooter;
private Turret turret;
private Hood hood;
private Intake intake;
private Spindexer spindexer;
+ private LinearClimb climb;
- private Command autoShoot;
+ public PS5ControllerDriverConfig(Drivetrain drive, LinearClimb climb) {
+ super(drive);
+ this.climb = climb;
+ }
- private boolean intakeBoolean = true;
+ public void setIntake(Intake intake) {
+ this.intake = intake;
+ }
- public PS5ControllerDriverConfig(Drivetrain drive, Shooter shooter, Turret turret, Hood hood, Intake intake, Spindexer spindexer) {
- super(drive);
- this.shooter = shooter;
+ public void setTurret(Turret turret) {
this.turret = turret;
+ }
+
+ public void setHood(Hood hood) {
this.hood = hood;
- this.intake = intake;
+ }
+
+ public void setShooter(Shooter shooter) {
+ this.shooter = shooter;
+ }
+
+ public void setSpindexer(Spindexer spindexer) {
this.spindexer = spindexer;
}
- public void configureControls() {
+ public void configureControls() {
// Reset the yaw. Mainly useful for testing/driver practice
driver.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw(
- new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)
- )));
+ new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI))));
// Cancel commands
- driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(()->{
+ driver.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> {
getDrivetrain().setIsAlign(false);
- getDrivetrain().setDesiredPose(()->null);
+ getDrivetrain().setDesiredPose(() -> null);
CommandScheduler.getInstance().cancelAll();
}));
// Align wheels
driver.get(PS5Button.MUTE).onTrue(new FunctionalCommand(
- ()->getDrivetrain().setStateDeadband(false),
- getDrivetrain()::alignWheels,
- interrupted->getDrivetrain().setStateDeadband(true),
- ()->false, getDrivetrain()).withTimeout(2));
+ () -> getDrivetrain().setStateDeadband(false),
+ getDrivetrain()::alignWheels,
+ interrupted -> getDrivetrain().setStateDeadband(true),
+ () -> false, getDrivetrain()).withTimeout(2));
// Intake
- if(intake != null){
- driver.get(PS5Button.CROSS).onTrue(new InstantCommand(()->{
- if(intakeBoolean){
+ if (intake != null) {
+ driver.get(PS5Button.CROSS).onTrue(new InstantCommand(() -> {
+ if (intakeBoolean) {
intake.extend();
intake.spinStart();
intakeBoolean = false;
- }
- else{
+ } else {
intake.retract();
intake.spinStop();
}
}
// Auto shoot
- if(turret != null){
+ if (turret != null) {
driver.get(PS5Button.SQUARE).onTrue(
- new InstantCommand(()->{
- if (autoShoot != null && autoShoot.isScheduled()){
+ new InstantCommand(() -> {
+ if (autoShoot != null && autoShoot.isScheduled()) {
autoShoot.cancel();
- } else{
+ } else {
autoShoot = new AutoShootCommand(turret, getDrivetrain(), hood, shooter, spindexer);
CommandScheduler.getInstance().schedule(autoShoot);
}
- })
- );
+ }));
}
-
+ if (climb != null) {
+ driver.get(PS5Button.CIRCLE).onTrue(new InstantCommand(() -> {
+ climb.hardstopCalibration();
+ })).onFalse(new InstantCommand(() -> {
+ climb.stopCalibrating();
+ }));
+ }
}
@Override
return false;
}
- public void startRumble(){
+ public void startRumble() {
driver.rumbleOn();
}
- public void endRumble(){
+ public void endRumble() {
driver.rumbleOff();
}
}