From 3b8f02f363485143217315db59cfc3adbc1bf68c Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 22 Jan 2026 14:35:49 -0800 Subject: [PATCH 1/8] jesus christ this is terrible --- src/main/java/frc/robot/Robot.java | 140 +++++++++++------- src/main/java/frc/robot/Superstructure.java | 20 +-- .../frc/robot/subsystems/indexer/Indexer.java | 27 ++++ ...rSubsystem.java => LindexerSubsystem.java} | 8 +- .../indexer/SpindexerSubsystem.java | 67 +++++++++ ...keSubsystem.java => FintakeSubsystem.java} | 7 +- .../frc/robot/subsystems/intake/Intake.java | 16 ++ .../subsystems/intake/LintakeSubsystem.java | 37 +++++ .../frc/robot/subsystems/shooter/Shooter.java | 29 ++++ .../subsystems/shooter/ShooterSubsystem.java | 9 +- .../subsystems/shooter/TurretSubsystem.java | 68 +++++++++ .../subsystems/swerve/SwerveSubsystem.java | 7 +- 12 files changed, 362 insertions(+), 73 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/indexer/Indexer.java rename src/main/java/frc/robot/subsystems/indexer/{IndexerSubsystem.java => LindexerSubsystem.java} (95%) create mode 100644 src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java rename src/main/java/frc/robot/subsystems/intake/{IntakeSubsystem.java => FintakeSubsystem.java} (93%) create mode 100644 src/main/java/frc/robot/subsystems/intake/Intake.java create mode 100644 src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/Shooter.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 10928ec..35683b3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -30,15 +30,21 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOSim; -import frc.robot.subsystems.indexer.IndexerSubsystem; -import frc.robot.subsystems.intake.IntakeSubsystem; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.indexer.LindexerSubsystem; +import frc.robot.subsystems.indexer.SpindexerSubsystem; +import frc.robot.subsystems.intake.FintakeSubsystem; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.LintakeSubsystem; import frc.robot.subsystems.led.LEDIOReal; import frc.robot.subsystems.led.LEDSubsystem; import frc.robot.subsystems.shooter.FlywheelIO; import frc.robot.subsystems.shooter.FlywheelIOSim; import frc.robot.subsystems.shooter.HoodIO; import frc.robot.subsystems.shooter.HoodIOSim; +import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.shooter.TurretSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread; import frc.robot.utils.CommandXboxControllerSubsystem; @@ -56,6 +62,7 @@ public class Robot extends LoggedRobot { public static final RobotType ROBOT_TYPE = Robot.isReal() ? RobotType.REAL : RobotType.SIM; + public static final RobotEdition ROBOT_EDITION = RobotEdition.COMP; public static final boolean TUNING_MODE = true; public boolean hasZeroedSinceStartup = false; @@ -65,6 +72,11 @@ public enum RobotType { REPLAY } + public enum RobotEdition { + ALPHA, + COMP + } + private final Alert driverJoystickDisconnectedAlert = new Alert("Driver controller disconnected!", AlertType.kError); private final Alert operatorJoystickDisconnectedAlert = @@ -96,55 +108,9 @@ public enum RobotType { // Subsystem initialization private final SwerveSubsystem swerve = new SwerveSubsystem(canivore); - private final IndexerSubsystem indexer = - new IndexerSubsystem( - canivore, - (ROBOT_TYPE == RobotType.REAL) - ? new RollerIO(9, IndexerSubsystem.getIndexerConfigs(), canivore) - : new RollerIOSim( - 9, - IndexerSubsystem.getIndexerConfigs(), - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX44Foc(1), 0.003, IndexerSubsystem.GEAR_RATIO), - DCMotor.getKrakenX44Foc(1)), - MotorType.KrakenX44, - canivore), - (ROBOT_TYPE == RobotType.REAL) - ? new RollerIO(10, IndexerSubsystem.getIndexerConfigs(), canivore) - : new RollerIOSim( - 10, - IndexerSubsystem.getKickerConfigs(), - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX44Foc(1), 0.00001, IndexerSubsystem.KICKER_GEAR_RATIO), - DCMotor.getKrakenX44Foc(1)), - MotorType.KrakenX44, - canivore)); // canivore, new RollerIOReal(0, IndexerSubsystem.getIndexerConfigs())); private final LEDSubsystem leds; - private final ShooterSubsystem shooter = - new ShooterSubsystem( - ROBOT_TYPE == RobotType.REAL - ? new HoodIO(HoodIO.getHoodConfiguration(), canivore) - : new HoodIOSim(canivore), - ROBOT_TYPE == RobotType.REAL - ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore) - : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(), canivore)); - private final IntakeSubsystem intake = - new IntakeSubsystem( - (ROBOT_TYPE == RobotType.REAL) - ? new RollerIO(8, IntakeSubsystem.getIntakeConfig(), canivore) - : new RollerIOSim( - 8, - IntakeSubsystem.getIntakeConfig(), - new DCMotorSim( - LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX44Foc(1), 0.001, IntakeSubsystem.GEAR_RATIO), - DCMotor.getKrakenX44Foc(1)), - MotorType.KrakenX44, - canivore)); private final CommandXboxControllerSubsystem driver = new CommandXboxControllerSubsystem(0); private final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1); @@ -158,8 +124,7 @@ public enum RobotType { @AutoLogOutput(key = "Robot/Zeroing Request") private Trigger zeroingReq = driver.b(); - private final Superstructure superstructure = - new Superstructure(swerve, indexer, intake, shooter, driver, operator); + private final Superstructure superstructure; private final Autos autos; private Optional lastAlliance = Optional.empty(); @@ -188,6 +153,69 @@ public void placeGamePiecesOnField() {} @SuppressWarnings("resource") public Robot() { + + Indexer indexer = null; + Intake intake = null; + Shooter shooter = null; + switch (ROBOT_EDITION) { + case ALPHA: + indexer = + new LindexerSubsystem( + canivore, + (ROBOT_TYPE == RobotType.REAL) + ? new RollerIO(9, LindexerSubsystem.getIndexerConfigs(), canivore) + : new RollerIOSim( + 9, + LindexerSubsystem.getIndexerConfigs(), + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), 0.003, LindexerSubsystem.GEAR_RATIO), + DCMotor.getKrakenX44Foc(1)), + MotorType.KrakenX44, + canivore), + (ROBOT_TYPE == RobotType.REAL) + ? new RollerIO(10, LindexerSubsystem.getIndexerConfigs(), canivore) + : new RollerIOSim( + 10, + LindexerSubsystem.getKickerConfigs(), + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), + 0.00001, + LindexerSubsystem.KICKER_GEAR_RATIO), + DCMotor.getKrakenX44Foc(1)), + MotorType.KrakenX44, + canivore)); + shooter = + new ShooterSubsystem( + ROBOT_TYPE == RobotType.REAL + ? new HoodIO(HoodIO.getHoodConfiguration(), canivore) + : new HoodIOSim(canivore), + ROBOT_TYPE == RobotType.REAL + ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore) + : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(), canivore)); + intake = + new FintakeSubsystem( + (ROBOT_TYPE == RobotType.REAL) + ? new RollerIO(8, FintakeSubsystem.getIntakeConfig(), canivore) + : new RollerIOSim( + 8, + FintakeSubsystem.getIntakeConfig(), + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), 0.001, FintakeSubsystem.GEAR_RATIO), + DCMotor.getKrakenX44Foc(1)), + MotorType.KrakenX44, + canivore)); + break; + case COMP: + indexer = new SpindexerSubsystem(); + intake = new LintakeSubsystem(); + shooter = new TurretSubsystem(); + break; + } + superstructure = new Superstructure(swerve, indexer, intake, shooter, driver, operator); + DriverStation.silenceJoystickConnectionWarning(true); SignalLogger.enableAutoLogging(false); RobotController.setBrownoutVoltage(6.0); @@ -316,8 +344,8 @@ public Robot() { .alongWith(leds.blinkCmd(Color.kWhite, Color.kBlack, 20.0).withTimeout(1.0)) .ignoringDisable(true)); SmartDashboard.putData("Add autos", Commands.runOnce(this::addAutos).ignoringDisable(true)); - SmartDashboard.putData("Zero hood", shooter.zeroHood().ignoringDisable(true)); - SmartDashboard.putData("Test Shot", shooter.testShoot()); + // SmartDashboard.putData("Zero hood", shooter.zeroHood().ignoringDisable(true)); + // SmartDashboard.putData("Test Shot", shooter.testShoot()); // Reset alert timers canInitialErrorTimer.restart(); @@ -378,10 +406,10 @@ private void addAutos() { "Regenerating Autos on " + DriverStation.getAlliance().map((a) -> a.toString())); // Sysid Autos - autoChooser.addOption("Hood Sysid", shooter.runHoodSysid()); - autoChooser.addOption("Index Roller Sysid", indexer.runRollerSysId()); - autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid()); - autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid()); + // autoChooser.addOption("Hood Sysid", shooter.runHoodSysid()); + // autoChooser.addOption("Index Roller Sysid", indexer.runRollerSysId()); + // autoChooser.addOption("Intake Roller Sysid", intake.runRollerSysid()); + // autoChooser.addOption("Flywheel Sysid", shooter.runFlywheelSysid()); haveAutosGenerated = true; System.out.println("Done generating autos"); } diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index f4e1895..5f26b30 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -9,9 +9,9 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.subsystems.indexer.IndexerSubsystem; -import frc.robot.subsystems.intake.IntakeSubsystem; -import frc.robot.subsystems.shooter.ShooterSubsystem; +import frc.robot.subsystems.indexer.Indexer; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.utils.CommandXboxControllerSubsystem; import frc.robot.utils.FieldUtils.FeedTargets; @@ -51,9 +51,9 @@ public Trigger getTrigger() { private Timer stateTimer = new Timer(); private final SwerveSubsystem swerve; - private final IndexerSubsystem indexer; - private final IntakeSubsystem intake; - private final ShooterSubsystem shooter; + private final Indexer indexer; + private final Intake intake; + private final Shooter shooter; private final CommandXboxControllerSubsystem driver; private final CommandXboxControllerSubsystem operator; @@ -87,9 +87,9 @@ public Trigger getTrigger() { /** Creates a new Superstructure. */ public Superstructure( SwerveSubsystem swerve, - IndexerSubsystem indexer, - IntakeSubsystem intake, - ShooterSubsystem shooter, + Indexer indexer, + Intake intake, + Shooter shooter, CommandXboxControllerSubsystem driver, CommandXboxControllerSubsystem operator) { this.swerve = swerve; @@ -260,7 +260,7 @@ private void addCommands() { indexer.index(), shooter.feed(swerve::getPose, () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); - bindCommands(SuperState.SPIT, intake.outake(), indexer.spit(), shooter.spit()); + bindCommands(SuperState.SPIT, intake.outtake(), indexer.spit(), shooter.spit()); } public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java new file mode 100644 index 0000000..cb45632 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -0,0 +1,27 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.indexer; + +import edu.wpi.first.wpilibj2.command.Command; + +/** Add your docs here. */ +public interface Indexer { + + public boolean isFull(); + + public boolean isEmpty(); + + public boolean isPartiallyFull(); + + public Command index(); + + public Command shoot(); + + public Command spit(); + + public Command kick(); + + public Command rest(); +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java similarity index 95% rename from src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java rename to src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java index 1e8eebf..1dcb00c 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java @@ -17,7 +17,8 @@ import frc.robot.components.rollers.RollerIOInputsAutoLogged; import org.littletonrobotics.junction.Logger; -public class IndexerSubsystem extends SubsystemBase { +/** Lindexer = Linear Indexer. !! ALPHA !! */ +public class LindexerSubsystem extends SubsystemBase implements Indexer { public static final double GEAR_RATIO = 2.0; private CANrangeIOReal firstCANRangeIO; @@ -46,13 +47,14 @@ public class IndexerSubsystem extends SubsystemBase { public static final double MAX_VELOCITY = 10.0; public static final double KICKER_GEAR_RATIO = 2.0; - public IndexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kickerIO) { + public LindexerSubsystem(CANBus canbus, RollerIO indexRollerIO, RollerIO kickerIO) { this.kickerIO = kickerIO; firstCANRangeIO = new CANrangeIOReal(0, canbus, 10); secondCANRangeIO = new CANrangeIOReal(1, canbus, 10); this.indexRollerIO = indexRollerIO; } + @Override public boolean isFull() { return firstCANRangeInputs.isDetected && secondCANRangeInputs.isDetected; } @@ -66,10 +68,12 @@ public Command shoot() { return this.run(() -> kickerIO.setRollerVoltage(0)); } + @Override public boolean isEmpty() { return !firstCANRangeInputs.isDetected && !secondCANRangeInputs.isDetected; } + @Override public boolean isPartiallyFull() { return !firstCANRangeInputs.isDetected && secondCANRangeInputs.isDetected; } diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java new file mode 100644 index 0000000..70fc209 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -0,0 +1,67 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.indexer; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** Spindexer = SPINning Indexer. !! COMP !! */ +public class SpindexerSubsystem extends SubsystemBase implements Indexer { + /** Creates a new SpindexerSubsystem. */ + public SpindexerSubsystem() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public boolean isFull() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'isFull'"); + } + + @Override + public boolean isEmpty() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'isEmpty'"); + } + + @Override + public boolean isPartiallyFull() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'isPartiallyFull'"); + } + + @Override + public Command index() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'index'"); + } + + @Override + public Command shoot() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'shoot'"); + } + + @Override + public Command spit() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'spit'"); + } + + @Override + public Command kick() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'kick'"); + } + + @Override + public Command rest() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'rest'"); + } +} diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java similarity index 93% rename from src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java rename to src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java index e159e95..01d7a69 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java @@ -14,7 +14,8 @@ import frc.robot.components.rollers.RollerIOInputsAutoLogged; import org.littletonrobotics.junction.Logger; -public class IntakeSubsystem extends SubsystemBase { +/**Fintake = Fixed Intake. !! ALPHA !! */ +public class FintakeSubsystem extends SubsystemBase implements Intake { public static final double GEAR_RATIO = 2.0; private RollerIO io; @@ -29,7 +30,7 @@ public class IntakeSubsystem extends SubsystemBase { (state) -> Logger.recordOutput("Intake/SysID State", state.toString())), new Mechanism((volts) -> io.setRollerVoltage(volts.in(Volts)), null, this)); - public IntakeSubsystem(RollerIO io) { + public FintakeSubsystem(RollerIO io) { this.io = io; } @@ -43,7 +44,7 @@ public Command intake() { return this.run(() -> io.setRollerVoltage(10)); } - public Command outake() { + public Command outtake() { return this.run(() -> io.setRollerVoltage(-5)); } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java new file mode 100644 index 0000000..099eb7d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -0,0 +1,16 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj2.command.Command; + +/** Add your docs here. */ +public interface Intake { + public Command intake(); + + public Command outtake(); + + public Command rest(); +} diff --git a/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java new file mode 100644 index 0000000..a400db1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/intake/LintakeSubsystem.java @@ -0,0 +1,37 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.intake; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +/** Lintake = Linear Intake. !! COMP !! */ +public class LintakeSubsystem extends SubsystemBase implements Intake { + /** Creates a new LintakeSubsystem. */ + public LintakeSubsystem() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public Command intake() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'intake'"); + } + + @Override + public Command outtake() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'outtake'"); + } + + @Override + public Command rest() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'rest'"); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java new file mode 100644 index 0000000..52f30a8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.shooter; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import java.util.function.Supplier; + +/** Add your docs here. */ +public interface Shooter { + + public Command shoot(Supplier robotPoseSupplier); + + public Command feed(Supplier robotPoseSupplier, Supplier feedTarget); + + public Command rest(); + + public Command spit(); + + public boolean atFlywheelVelocitySetpoint(); + + public boolean atHoodSetpoint(); + + public Command zeroHood(); + + public Command testShoot(); +} diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index bd8ff9c..d189d52 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -23,7 +23,7 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; -public class ShooterSubsystem extends SubsystemBase { +public class ShooterSubsystem extends SubsystemBase implements Shooter { public static double HOOD_GEAR_RATIO = 24.230769; public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); public static Rotation2d HOOD_MIN_ROTATION = Rotation2d.fromDegrees(2); @@ -73,6 +73,7 @@ public Command testShoot() { }); } + @Override public Command shoot(Supplier robotPoseSupplier) { return this.run( () -> { @@ -83,6 +84,7 @@ public Command shoot(Supplier robotPoseSupplier) { }); } + @Override public Command feed(Supplier robotPoseSupplier, Supplier feedTarget) { return this.run( () -> { @@ -97,6 +99,7 @@ public Command feed(Supplier robotPoseSupplier, Supplier feedTar }); } + @Override public Command rest() { return this.run( () -> { @@ -105,6 +108,7 @@ public Command rest() { }); } + @Override public Command spit() { return this.run( () -> { @@ -162,6 +166,7 @@ public Command runFlywheelSysid() { flywheelSysid.dynamic(Direction.kReverse)); } + @Override @AutoLogOutput(key = "Shooter/At Vel Setpoint") public boolean atFlywheelVelocitySetpoint() { return MathUtil.isNear( @@ -170,12 +175,14 @@ public boolean atFlywheelVelocitySetpoint() { FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND); } + @Override @AutoLogOutput(key = "Shooter/Hood/At Setpoint") public boolean atHoodSetpoint() { return MathUtil.isNear( hoodInputs.hoodPositionRotations.getDegrees(), hoodIO.getHoodSetpoint().getDegrees(), 1); } + @Override public Command zeroHood() { return this.runOnce(() -> hoodIO.resetEncoder(HOOD_MIN_ROTATION)); } diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java new file mode 100644 index 0000000..55974d9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -0,0 +1,68 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.shooter; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import java.util.function.Supplier; + +public class TurretSubsystem extends SubsystemBase implements Shooter { + /** Creates a new TurretSubsystem. */ + public TurretSubsystem() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public Command shoot(Supplier robotPoseSupplier) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'shoot'"); + } + + @Override + public Command feed(Supplier robotPoseSupplier, Supplier feedTarget) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'feed'"); + } + + @Override + public Command rest() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'rest'"); + } + + @Override + public Command spit() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'spit'"); + } + + @Override + public boolean atFlywheelVelocitySetpoint() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'atFlywheelVelocitySetpoint'"); + } + + @Override + public boolean atHoodSetpoint() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'atHoodSetpoint'"); + } + + @Override + public Command zeroHood() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'zeroHood'"); + } + + @Override + public Command testShoot() { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'testShoot'"); + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 424a206..33176bf 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -27,11 +27,13 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; +import frc.robot.Robot.RobotEdition; import frc.robot.Robot.RobotType; import frc.robot.components.camera.Camera; import frc.robot.components.camera.CameraIOReal; import frc.robot.components.camera.CameraIOSim; import frc.robot.subsystems.swerve.constants.AlphaSwerveConstants; +import frc.robot.subsystems.swerve.constants.CompBotSwerveConstants; import frc.robot.subsystems.swerve.constants.SwerveConstants; import frc.robot.subsystems.swerve.gyro.GyroIO; import frc.robot.subsystems.swerve.gyro.GyroIOInputsAutoLogged; @@ -63,7 +65,10 @@ import org.littletonrobotics.junction.Logger; public class SwerveSubsystem extends SubsystemBase { - public static final SwerveConstants SWERVE_CONSTANTS = new AlphaSwerveConstants(); + public static final SwerveConstants SWERVE_CONSTANTS = + Robot.ROBOT_EDITION == RobotEdition.ALPHA + ? new AlphaSwerveConstants() + : new CompBotSwerveConstants(); private final Module[] modules; // Front Left, Front Right, Back Left, Back Right private final GyroIO gyroIO; From 4c23556861b900937e049c93e0b89357c10020d9 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 22 Jan 2026 14:42:28 -0800 Subject: [PATCH 2/8] add overrides and remove some unused methods --- .../java/frc/robot/subsystems/indexer/Indexer.java | 2 -- .../robot/subsystems/indexer/LindexerSubsystem.java | 13 ++++--------- .../subsystems/indexer/SpindexerSubsystem.java | 6 ------ .../robot/subsystems/intake/FintakeSubsystem.java | 5 ++++- .../robot/subsystems/shooter/ShooterSubsystem.java | 5 +---- 5 files changed, 9 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index cb45632..fae5b83 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -17,8 +17,6 @@ public interface Indexer { public Command index(); - public Command shoot(); - public Command spit(); public Command kick(); diff --git a/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java index 1dcb00c..44cceff 100644 --- a/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/LindexerSubsystem.java @@ -59,15 +59,6 @@ public boolean isFull() { return firstCANRangeInputs.isDetected && secondCANRangeInputs.isDetected; } - public Command stopKicker() { - return this.run(() -> kickerIO.setRollerVoltage(0)); - } - ; - - public Command shoot() { - return this.run(() -> kickerIO.setRollerVoltage(0)); - } - @Override public boolean isEmpty() { return !firstCANRangeInputs.isDetected && !secondCANRangeInputs.isDetected; @@ -78,6 +69,7 @@ public boolean isPartiallyFull() { return !firstCANRangeInputs.isDetected && secondCANRangeInputs.isDetected; } + @Override public Command index() { return this.run( () -> { @@ -86,6 +78,7 @@ public Command index() { }); } + @Override public Command kick() { return this.run( () -> { @@ -94,6 +87,7 @@ public Command kick() { }); } + @Override public Command spit() { return this.run( () -> { @@ -102,6 +96,7 @@ public Command spit() { }); } + @Override public Command rest() { return this.run( () -> { diff --git a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java index 70fc209..4346925 100644 --- a/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/SpindexerSubsystem.java @@ -41,12 +41,6 @@ public Command index() { throw new UnsupportedOperationException("Unimplemented method 'index'"); } - @Override - public Command shoot() { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'shoot'"); - } - @Override public Command spit() { // TODO Auto-generated method stub diff --git a/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java index 01d7a69..a363f78 100644 --- a/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java @@ -34,20 +34,23 @@ public FintakeSubsystem(RollerIO io) { this.io = io; } + @Override public void periodic() { io.updateInputs(inputs); Logger.processInputs("Intake", inputs); } - // TODO get actual values + @Override public Command intake() { return this.run(() -> io.setRollerVoltage(10)); } + @Override public Command outtake() { return this.run(() -> io.setRollerVoltage(-5)); } + @Override public Command rest() { return this.run(() -> io.setRollerVoltage(0)); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index d189d52..9f39583 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -65,6 +65,7 @@ public ShooterSubsystem(HoodIO hoodIO, FlywheelIO flywheelIO) { this.flywheelIO = flywheelIO; } + @Override public Command testShoot() { return this.run( () -> { @@ -117,10 +118,6 @@ public Command spit() { }); // TODO: TUNE HOOD POS AND FLYWHEEL VELOCITY } - public Command setHoodPositionCommand(Supplier hoodPosition) { - return this.run(() -> hoodIO.setHoodPosition(hoodPosition.get())); - } - @Override public void periodic() { hoodIO.updateInputs(hoodInputs); From 914a858dadb2a1a72f3b11eb3e8adddf96957cd0 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 22 Jan 2026 14:48:36 -0800 Subject: [PATCH 3/8] add climber???? --- src/main/java/frc/robot/Robot.java | 9 +++++++-- .../subsystems/climber/ClimberSubsystem.java | 17 +++++++++++++++++ 2 files changed, 24 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 35683b3..791e7d7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.components.rollers.RollerIO; import frc.robot.components.rollers.RollerIOSim; +import frc.robot.subsystems.climber.ClimberSubsystem; import frc.robot.subsystems.indexer.Indexer; import frc.robot.subsystems.indexer.LindexerSubsystem; import frc.robot.subsystems.indexer.SpindexerSubsystem; @@ -111,6 +112,7 @@ public enum RobotEdition { // canivore, new RollerIOReal(0, IndexerSubsystem.getIndexerConfigs())); private final LEDSubsystem leds; + private ClimberSubsystem climber; private final CommandXboxControllerSubsystem driver = new CommandXboxControllerSubsystem(0); private final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1); @@ -153,7 +155,6 @@ public void placeGamePiecesOnField() {} @SuppressWarnings("resource") public Robot() { - Indexer indexer = null; Intake intake = null; Shooter shooter = null; @@ -212,17 +213,20 @@ public Robot() { indexer = new SpindexerSubsystem(); intake = new LintakeSubsystem(); shooter = new TurretSubsystem(); + climber = new ClimberSubsystem(); //TODO climber break; } superstructure = new Superstructure(swerve, indexer, intake, shooter, driver, operator); + if (climber == null) climber = new ClimberSubsystem();//TODO new ClimberSubsystem(new ClimberIO() {}) and such DriverStation.silenceJoystickConnectionWarning(true); SignalLogger.enableAutoLogging(false); RobotController.setBrownoutVoltage(6.0); // Metadata about the current code running on the robot - Logger.recordMetadata("Codebase", "2026 Template"); + Logger.recordMetadata("Codebase", "Rebuilt"); Logger.recordMetadata("RuntimeType", getRuntimeType().toString()); Logger.recordMetadata("Robot Mode", ROBOT_TYPE.toString()); + Logger.recordMetadata("Robot Edition", ROBOT_EDITION.toString()); Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); @@ -386,6 +390,7 @@ private void addControllerBindings() { () -> modifyJoystick(driver.getLeftX()) * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed())); + //TODO add binding for climb // ---zeroing stuff--- diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java new file mode 100644 index 0000000..2b5d698 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -0,0 +1,17 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems.climber; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ClimberSubsystem extends SubsystemBase { + /** Creates a new ClimberSubsystem. */ + public ClimberSubsystem() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From 1d2e2387616520c15e9150fcc79c00b912de2845 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 22 Jan 2026 15:19:35 -0800 Subject: [PATCH 4/8] add comments --- src/main/java/frc/robot/Autos.java | 4 +- src/main/java/frc/robot/Robot.java | 162 ++++++++++-------- .../frc/robot/components/camera/Camera.java | 4 +- .../subsystems/intake/FintakeSubsystem.java | 4 +- .../subsystems/swerve/SwerveSubsystem.java | 16 +- 5 files changed, 110 insertions(+), 80 deletions(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index ebad639..97a4364 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -12,7 +12,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.Robot.RobotType; +import frc.robot.Robot.RobotMode; import frc.robot.subsystems.swerve.SwerveSubsystem; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -78,7 +78,7 @@ public Autos(SwerveSubsystem swerve) { true, swerve, (traj, edge) -> { - if (Robot.ROBOT_TYPE != RobotType.REAL) + if (Robot.ROBOT_MODE != RobotMode.REAL) Logger.recordOutput( "Choreo/Active Traj", DriverStation.getAlliance().isPresent() diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 791e7d7..cab6428 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -62,22 +62,31 @@ import org.littletonrobotics.junction.wpilog.WPILOGWriter; public class Robot extends LoggedRobot { - public static final RobotType ROBOT_TYPE = Robot.isReal() ? RobotType.REAL : RobotType.SIM; - public static final RobotEdition ROBOT_EDITION = RobotEdition.COMP; - public static final boolean TUNING_MODE = true; - public boolean hasZeroedSinceStartup = false; - - public enum RobotType { + /** This is whether or not the robot is real, sim, or replay */ + public enum RobotMode { REAL, SIM, REPLAY } + /** This is whether or not the robot is Alpha or Comp */ public enum RobotEdition { ALPHA, COMP } + public static final RobotMode ROBOT_MODE = Robot.isReal() ? RobotMode.REAL : RobotMode.SIM; + public static final RobotEdition ROBOT_EDITION = RobotEdition.COMP; + + /** + * This is for when we're testing shot and extension numbers and should be FALSE once bring up is + * complete + */ + public static final boolean TUNING_MODE = true; + + public boolean hasZeroedSinceStartup = false; + + // Add stuff related to dashboard alerts private final Alert driverJoystickDisconnectedAlert = new Alert("Driver controller disconnected!", AlertType.kError); private final Alert operatorJoystickDisconnectedAlert = @@ -91,8 +100,6 @@ public enum RobotEdition { "Battery voltage is very low, consider turning off the robot or replacing the battery.", AlertType.kWarning); - private static CANBus canivore = new CANBus("*"); - private final Timer canInitialErrorTimer = new Timer(); private final Timer canErrorTimer = new Timer(); private final Timer canivoreErrorTimer = new Timer(); @@ -105,39 +112,40 @@ public enum RobotEdition { private static final double lowBatteryDisabledTime = 1.5; private static final double lowBatteryMinCycleCount = 10; - // Instantiate subsystems + /** + * As per the 2026+ ctre api we should be passing in the actual canbus object to any ctre device + * constructors, NOT the name as a string + */ + public static CANBus canivore = new CANBus("*"); - // Subsystem initialization - private final SwerveSubsystem swerve = new SwerveSubsystem(canivore); + // Declare subsystems that aren't part of the superstructure + // This means they can do stuff regardless of what state the robot's in + // since none of them are dependent on the state - f.e. climber can do what it wants + // Subsystems that *are* part of the superstructure are initialized later - // canivore, new RollerIOReal(0, IndexerSubsystem.getIndexerConfigs())); + // swervesubsystem decides on its own whether or not to use alpha or comp swerve constants + private final SwerveSubsystem swerve = new SwerveSubsystem(canivore); private final LEDSubsystem leds; + + // climber only exists for the comp bot - this is accounted for later private ClimberSubsystem climber; + private final Superstructure superstructure; + private final CommandXboxControllerSubsystem driver = new CommandXboxControllerSubsystem(0); private final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1); + // Assign non-superstructure triggers @AutoLogOutput(key = "Superstructure/Autoaim Request") private Trigger autoAimReq = driver.rightBumper().or(driver.leftBumper()); - @AutoLogOutput(key = "Robot/Pre Zeroing Request") - private Trigger preZeroingReq = driver.a(); - - @AutoLogOutput(key = "Robot/Zeroing Request") - private Trigger zeroingReq = driver.b(); - - private final Superstructure superstructure; - + // Auto stuff private final Autos autos; private Optional lastAlliance = Optional.empty(); - @AutoLogOutput boolean haveAutosGenerated = false; - private final LoggedDashboardChooser autoChooser = new LoggedDashboardChooser<>("Autos"); - // Logged mechanisms - - // temporarily override map with empty map to avoid collisions swith reefscape elements + // temporarily override map with empty map to avoid collisions with reefscape elements // unfortunately this also turns off collisions with walls but that's fine // TODO update once rebuilt is added to maplesim private static class EvergreenArena extends SimulatedArena { @@ -153,17 +161,30 @@ public void placeGamePiecesOnField() {} SimulatedArena.overrideInstance(new EvergreenArena()); } + // this is here because it doesn't like that the power distribution logger is never closed @SuppressWarnings("resource") public Robot() { + // these have to be instantiated as null to ensure that there's always *something* in it + // although it always gets assigned to the robot-specific version in the switch-case statement + // below, the compiler doesn't technically know that for sure + // that would be bad because if for some reason we had some case of ROBOT_EDITION that wasn't + // accounted for, it wouldn't be able to pass anything to the superstructure below and it would + // break + // granted this would never actually happen but Indexer indexer = null; Intake intake = null; Shooter shooter = null; + + // this looks at the ROBOT_EDITION variable and decides which version of each subsystem to + // create based on that + // alpha: lindexer, fintake, shooter + // comp: spindexer, lintake, turret switch (ROBOT_EDITION) { case ALPHA: indexer = new LindexerSubsystem( canivore, - (ROBOT_TYPE == RobotType.REAL) + (ROBOT_MODE == RobotMode.REAL) ? new RollerIO(9, LindexerSubsystem.getIndexerConfigs(), canivore) : new RollerIOSim( 9, @@ -174,7 +195,7 @@ public Robot() { DCMotor.getKrakenX44Foc(1)), MotorType.KrakenX44, canivore), - (ROBOT_TYPE == RobotType.REAL) + (ROBOT_MODE == RobotMode.REAL) ? new RollerIO(10, LindexerSubsystem.getIndexerConfigs(), canivore) : new RollerIOSim( 10, @@ -187,17 +208,19 @@ public Robot() { DCMotor.getKrakenX44Foc(1)), MotorType.KrakenX44, canivore)); + shooter = new ShooterSubsystem( - ROBOT_TYPE == RobotType.REAL + ROBOT_MODE == RobotMode.REAL ? new HoodIO(HoodIO.getHoodConfiguration(), canivore) : new HoodIOSim(canivore), - ROBOT_TYPE == RobotType.REAL + ROBOT_MODE == RobotMode.REAL ? new FlywheelIO(FlywheelIO.getFlywheelConfiguration(), canivore) : new FlywheelIOSim(FlywheelIO.getFlywheelConfiguration(), canivore)); + intake = new FintakeSubsystem( - (ROBOT_TYPE == RobotType.REAL) + (ROBOT_MODE == RobotMode.REAL) ? new RollerIO(8, FintakeSubsystem.getIntakeConfig(), canivore) : new RollerIOSim( 8, @@ -208,28 +231,38 @@ public Robot() { DCMotor.getKrakenX44Foc(1)), MotorType.KrakenX44, canivore)); + // note that the climber is not instantiated here break; case COMP: indexer = new SpindexerSubsystem(); intake = new LintakeSubsystem(); shooter = new TurretSubsystem(); - climber = new ClimberSubsystem(); //TODO climber + climber = new ClimberSubsystem(); // TODO climber break; } + // now that we've assigned the correct subsystems based on robot edition, we can pass them into + // the superstructure superstructure = new Superstructure(swerve, indexer, intake, shooter, driver, operator); - if (climber == null) climber = new ClimberSubsystem();//TODO new ClimberSubsystem(new ClimberIO() {}) and such + // if this is alpha, we won't have assigned a climber yet + // this creates a placeholder "no-operation" climber that will just not do anything, but is not + // null (and we need it to be not null) + if (climber == null) + climber = new ClimberSubsystem(); // TODO new ClimberSubsystem(new ClimberIO() {}) and such DriverStation.silenceJoystickConnectionWarning(true); SignalLogger.enableAutoLogging(false); RobotController.setBrownoutVoltage(6.0); + // Metadata about the current code running on the robot Logger.recordMetadata("Codebase", "Rebuilt"); Logger.recordMetadata("RuntimeType", getRuntimeType().toString()); - Logger.recordMetadata("Robot Mode", ROBOT_TYPE.toString()); + Logger.recordMetadata("Robot Mode", ROBOT_MODE.toString()); Logger.recordMetadata("Robot Edition", ROBOT_EDITION.toString()); Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + + // log if we have uncommitted changes switch (BuildConstants.DIRTY) { case 0: Logger.recordMetadata("GitDirty", "All changes committed"); @@ -242,11 +275,16 @@ public Robot() { break; } - switch (ROBOT_TYPE) { + // set up logging stuff depending on robot mode + switch (ROBOT_MODE) { case REAL: Logger.addDataReceiver(new WPILOGWriter("/U")); // Log to a USB stick Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables - new PowerDistribution(1, ModuleType.kCTRE); // Enables power distribution logging + // TODO confirm pdp vs pdh + // apparently LoggedPowerDistribution doesn't work with the pdp 2.0 + // LoggedPowerDistribution.getInstance(1, ModuleType.kCTRE); // Enables power distribution + // logging + new PowerDistribution(1, ModuleType.kCTRE); break; case REPLAY: setUseTiming(false); // Run as fast as possible @@ -266,16 +304,15 @@ public Robot() { // be added. Logger.recordOutput("Canivore Status", canivore.getStatus().Status); + Logger.recordOutput("Robot Edition", ROBOT_EDITION); PhoenixOdometryThread.getInstance().start(); - leds = new LEDSubsystem(new LEDIOReal()); + leds = new LEDSubsystem(new LEDIOReal()); // TODO sim // Set default commands - driver.setDefaultCommand(driver.rumbleCmd(0.0, 0.0)); operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0)); - swerve.setDefaultCommand( swerve.driveOpenLoopFieldRelative( () -> @@ -290,39 +327,14 @@ public Robot() { addControllerBindings(); + // Auto things autos = new Autos(swerve); autoChooser.addDefaultOption("None", Commands.none()); - // Generates autos on connected - new Trigger( - () -> - DriverStation.isDSAttached() - && DriverStation.getAlliance().isPresent() - && !haveAutosGenerated) - .onTrue(Commands.print("Connected")) - .onTrue(Commands.runOnce(this::addAutos).ignoringDisable(true)); - - new Trigger( - () -> { - boolean allianceChanged = !DriverStation.getAlliance().equals(lastAlliance); - lastAlliance = DriverStation.getAlliance(); - return allianceChanged && DriverStation.getAlliance().isPresent(); - }) - .onTrue(Commands.runOnce(this::addAutos).ignoringDisable(true)); - // Run auto when auto starts. Matches Choreolib's defer impl RobotModeTriggers.autonomous() .whileTrue(Commands.defer(() -> autoChooser.get().asProxy(), Set.of())); - CommandScheduler.getInstance() - .onCommandInterrupt( - (interrupted, interrupting) -> { - System.out.println("Interrupted: " + interrupted); - System.out.println( - "Interrputing: " - + (interrupting.isPresent() ? interrupting.get().getName() : "none")); - }); - // Add autos on alliance change new Trigger( () -> { @@ -347,6 +359,7 @@ public Robot() { Commands.runOnce(() -> addAutos()) .alongWith(leds.blinkCmd(Color.kWhite, Color.kBlack, 20.0).withTimeout(1.0)) .ignoringDisable(true)); + SmartDashboard.putData("Add autos", Commands.runOnce(this::addAutos).ignoringDisable(true)); // SmartDashboard.putData("Zero hood", shooter.zeroHood().ignoringDisable(true)); // SmartDashboard.putData("Test Shot", shooter.testShoot()); @@ -356,6 +369,16 @@ public Robot() { canErrorTimer.restart(); canivoreErrorTimer.restart(); disabledTimer.restart(); + + // log when commands get interrupted + CommandScheduler.getInstance() + .onCommandInterrupt( + (interrupted, interrupting) -> { + System.out.println("Interrupted: " + interrupted); + System.out.println( + "Interrputing: " + + (interrupting.isPresent() ? interrupting.get().getName() : "none")); + }); } /** Scales a joystick value for teleop driving */ @@ -363,7 +386,6 @@ private static double modifyJoystick(double val) { return MathUtil.applyDeadband(Math.abs(Math.pow(val, 2)) * Math.signum(val), 0.02); } - @SuppressWarnings("unlikely-arg-type") private void addControllerBindings() { // heading reset driver @@ -390,10 +412,11 @@ private void addControllerBindings() { () -> modifyJoystick(driver.getLeftX()) * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed())); - //TODO add binding for climb + // TODO add binding for climb // ---zeroing stuff--- + // create triggers for joystick disconnect alerts new Trigger(() -> DriverStation.isJoystickConnected(0)) .negate() .onTrue(Commands.runOnce(() -> driverJoystickDisconnectedAlert.set(true))) @@ -422,10 +445,15 @@ private void addAutos() { @Override public void robotPeriodic() { CommandScheduler.getInstance().run(); + superstructure.periodic(); - // Log mechanism poses + // TODO Log mechanism poses + + updateAlerts(); + } + public void updateAlerts() { // Check CAN status var canStatus = RobotController.getCANStatus(); if (canStatus.transmitErrorCount > 0 || canStatus.receiveErrorCount > 0) { diff --git a/src/main/java/frc/robot/components/camera/Camera.java b/src/main/java/frc/robot/components/camera/Camera.java index c26cc50..2ff9563 100644 --- a/src/main/java/frc/robot/components/camera/Camera.java +++ b/src/main/java/frc/robot/components/camera/Camera.java @@ -17,7 +17,7 @@ import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotController; import frc.robot.Robot; -import frc.robot.Robot.RobotType; +import frc.robot.Robot.RobotMode; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.utils.Tracer; import java.util.NoSuchElementException; @@ -106,7 +106,7 @@ public Optional update(PhotonPipelineResult result) { if (result.getTargets().size() < 1) { return Optional.empty(); } - if (Robot.ROBOT_TYPE != RobotType.REAL) + if (Robot.ROBOT_MODE != RobotMode.REAL) Logger.recordOutput( "Vision/" + io.getName() + "/Best Distance", result.getBestTarget().getBestCameraToTarget().getTranslation().getNorm()); diff --git a/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java index a363f78..b4dcfd1 100644 --- a/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/FintakeSubsystem.java @@ -14,7 +14,7 @@ import frc.robot.components.rollers.RollerIOInputsAutoLogged; import org.littletonrobotics.junction.Logger; -/**Fintake = Fixed Intake. !! ALPHA !! */ +/** Fintake = Fixed Intake. !! ALPHA !! */ public class FintakeSubsystem extends SubsystemBase implements Intake { public static final double GEAR_RATIO = 2.0; @@ -40,7 +40,7 @@ public void periodic() { Logger.processInputs("Intake", inputs); } - @Override + @Override public Command intake() { return this.run(() -> io.setRollerVoltage(10)); } diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 33176bf..e422313 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -28,7 +28,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Robot; import frc.robot.Robot.RobotEdition; -import frc.robot.Robot.RobotType; +import frc.robot.Robot.RobotMode; import frc.robot.components.camera.Camera; import frc.robot.components.camera.CameraIOReal; import frc.robot.components.camera.CameraIOSim; @@ -65,6 +65,8 @@ import org.littletonrobotics.junction.Logger; public class SwerveSubsystem extends SubsystemBase { + // decide which set of swerve constants to use based on robot edition + // defaulting to comp is probably safer? public static final SwerveConstants SWERVE_CONSTANTS = Robot.ROBOT_EDITION == RobotEdition.ALPHA ? new AlphaSwerveConstants() @@ -134,7 +136,7 @@ public class SwerveSubsystem extends SubsystemBase { new SwerveDriveSimulation(driveTrainSimConfig, new Pose2d(3, 3, Rotation2d.kZero)); public SwerveSubsystem(CANBus canbus) { - if (Robot.ROBOT_TYPE == RobotType.SIM) { + if (Robot.ROBOT_MODE == RobotMode.SIM) { // Add simulated modules modules = new Module[] { @@ -216,7 +218,7 @@ public SwerveSubsystem(CANBus canbus) { } this.gyroIO = - Robot.ROBOT_TYPE != RobotType.SIM + Robot.ROBOT_MODE != RobotMode.SIM ? new GyroIOReal(SWERVE_CONSTANTS.getGyroID(), SWERVE_CONSTANTS.getGyroConfig(), canbus) : new GyroIOSim(swerveSimulation.getGyroSimulation()); @@ -233,7 +235,7 @@ public SwerveSubsystem(CANBus canbus) { this.odometryThread = PhoenixOdometryThread.getInstance(); - if (Robot.ROBOT_TYPE == RobotType.SIM) { + if (Robot.ROBOT_MODE == RobotMode.SIM) { SimulatedArena.getInstance().addDriveTrainSimulation(swerveSimulation); } } @@ -355,12 +357,12 @@ private void updateVision() { cameraPoses[i] = cameras[i].getPose(); } // only do all this logging stuff if we're not irl for performance - if (Robot.ROBOT_TYPE != RobotType.REAL) { + if (Robot.ROBOT_MODE != RobotMode.REAL) { Logger.recordOutput("Vision/Camera Poses", cameraPoses); Pose3d[] arr = new Pose3d[cameras.length]; for (int k = 0; k < cameras.length; k++) { // honetsly not sure if this distinction is the way to go but - if (Robot.ROBOT_TYPE == RobotType.SIM) + if (Robot.ROBOT_MODE == RobotMode.SIM) // If we're in sim, use the maplesim pose to calculate vision arr[k] = new Pose3d(swerveSimulation.getSimulatedDriveTrainPose()) @@ -651,7 +653,7 @@ public Rotation2d getRotation() { public void resetPose(Pose2d newPose) { estimator.resetPose(newPose); - if (Robot.ROBOT_TYPE == RobotType.SIM) { + if (Robot.ROBOT_MODE == RobotMode.SIM) { swerveSimulation.setSimulationWorldPose(newPose); swerveSimulation.setRobotSpeeds(new ChassisSpeeds()); } From b4cbeac078b923697f21026051b678709b45f20d Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 22 Jan 2026 21:02:04 -0800 Subject: [PATCH 5/8] add javadocs for interface methods, add check for rio serial number --- src/main/java/frc/robot/Robot.java | 31 ++++++++++++++++++- .../frc/robot/subsystems/indexer/Indexer.java | 4 +++ .../frc/robot/subsystems/intake/Intake.java | 3 ++ .../frc/robot/subsystems/shooter/Shooter.java | 14 +++++++++ .../subsystems/shooter/ShooterSubsystem.java | 1 + .../subsystems/shooter/TurretSubsystem.java | 1 + 6 files changed, 53 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index cab6428..d6d290e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -76,7 +76,23 @@ public enum RobotEdition { } public static final RobotMode ROBOT_MODE = Robot.isReal() ? RobotMode.REAL : RobotMode.SIM; - public static final RobotEdition ROBOT_EDITION = RobotEdition.COMP; + // public static final RobotEdition ROBOT_EDITION = RobotEdition.COMP; + public static final RobotEdition ROBOT_EDITION; + + // TODO get rio serial numbers + static { + switch (RobotController.getSerialNumber()) { + case "1": + ROBOT_EDITION = RobotEdition.ALPHA; + break; + case "2": + ROBOT_EDITION = RobotEdition.COMP; + break; + default: + // defaulting to comp is probably safer? + ROBOT_EDITION = RobotEdition.COMP; + } + } /** * This is for when we're testing shot and extension numbers and should be FALSE once bring up is @@ -99,6 +115,12 @@ public enum RobotEdition { new Alert( "Battery voltage is very low, consider turning off the robot or replacing the battery.", AlertType.kWarning); + private final Alert wrongRobotAlert = + new Alert( + "!! ALPHA CODE HAS BEEN DEPLOYED TO COMP BOT THIS IS VERY BAD !!", AlertType.kError); + private final Alert tuningModeAlert = + new Alert( + "! Tuning mode is enabled while FMS attached, this is Quite bad !", AlertType.kWarning); private final Timer canInitialErrorTimer = new Timer(); private final Timer canErrorTimer = new Timer(); @@ -164,6 +186,7 @@ public void placeGamePiecesOnField() {} // this is here because it doesn't like that the power distribution logger is never closed @SuppressWarnings("resource") public Robot() { + // these have to be instantiated as null to ensure that there's always *something* in it // although it always gets assigned to the robot-specific version in the switch-case statement // below, the compiler doesn't technically know that for sure @@ -454,6 +477,12 @@ public void robotPeriodic() { } public void updateAlerts() { + + if (ROBOT_EDITION == RobotEdition.ALPHA && DriverStation.isFMSAttached()) { + wrongRobotAlert.set(true); + // TODO make leds strobe red or something + } + if (TUNING_MODE && DriverStation.isFMSAttached()) tuningModeAlert.set(true); // Check CAN status var canStatus = RobotController.getCANStatus(); if (canStatus.transmitErrorCount > 0 || canStatus.receiveErrorCount > 0) { diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java index fae5b83..6bd3118 100644 --- a/src/main/java/frc/robot/subsystems/indexer/Indexer.java +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -15,11 +15,15 @@ public interface Indexer { public boolean isPartiallyFull(); + /** Run indexer towards shooter and kicker away from shooter */ public Command index(); + /** Run everything backwards. This is for antijamming the robot */ public Command spit(); + /** Run both indexer and kicker towards the shooter */ public Command kick(); + /** Not running (set to 0) */ public Command rest(); } diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index 099eb7d..9a6196f 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -8,9 +8,12 @@ /** Add your docs here. */ public interface Intake { + /** Run balls towards the shooter */ public Command intake(); + /** Run balls away from the shooter. This is for antijamming the robot */ public Command outtake(); + /** Not running (set to 0) */ public Command rest(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 52f30a8..cacef67 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -11,19 +11,33 @@ /** Add your docs here. */ public interface Shooter { + /** + * Sets hood angle and flywheel velocity based on distance from hub from the shot map + current + * pose + */ public Command shoot(Supplier robotPoseSupplier); + /** + * Sets hood angle and flywheel velocity based on distance from hub from the feed map + current + * pose + feed target + */ public Command feed(Supplier robotPoseSupplier, Supplier feedTarget); + /** Not running (set to 0) */ public Command rest(); + /** Run balls out from the shooter. This is for antijamming the robot */ public Command spit(); + /** Check if flywheel has spun up to desired speed */ public boolean atFlywheelVelocitySetpoint(); + /** Check if hood is at its desired position */ public boolean atHoodSetpoint(); + /** Reset hood encoder to its minimum position */ public Command zeroHood(); + /** Shoots based on dashboard numbers. For testing only */ public Command testShoot(); } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 9f39583..69fcb45 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -23,6 +23,7 @@ import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; +/** Fixed shooter. !! ALPHA !! */ public class ShooterSubsystem extends SubsystemBase implements Shooter { public static double HOOD_GEAR_RATIO = 24.230769; public static Rotation2d HOOD_MAX_ROTATION = Rotation2d.fromDegrees(40); diff --git a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java index 55974d9..685b95a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/TurretSubsystem.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.Supplier; +/** Pivoting hooded shooter (turret). !! COMP !! */ public class TurretSubsystem extends SubsystemBase implements Shooter { /** Creates a new TurretSubsystem. */ public TurretSubsystem() {} From 048d5f24a24689cd1aba8534a96996b3e9c78ef1 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 22 Jan 2026 21:41:43 -0800 Subject: [PATCH 6/8] remove autos for now, not sure how that got in but that will be its own pr --- src/main/java/frc/robot/Autos.java | 311 ----------------------------- 1 file changed, 311 deletions(-) delete mode 100644 src/main/java/frc/robot/Autos.java diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java deleted file mode 100644 index 539dd20..0000000 --- a/src/main/java/frc/robot/Autos.java +++ /dev/null @@ -1,311 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot; - -import choreo.auto.AutoFactory; -import choreo.auto.AutoRoutine; -import choreo.auto.AutoTrajectory; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.Trigger; -// import frc.robot.Autos.PathEndType; -import frc.robot.Robot.RobotMode; -import frc.robot.subsystems.swerve.SwerveSubsystem; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; - -/** Add your docs here. */ -public class Autos { - private final AutoFactory factory; - - // Declare triggers - // mehhhhhhh - private static boolean autoFeed; - private static boolean autoIntake; - private static boolean autoScore; - private static boolean autoClimb; - - // private static boolean autoIntakeAlgae; - - @AutoLogOutput(key = "Superstructure/Auto Feed Request") - public static Trigger autoFeedReq = new Trigger(() -> autoFeed).and(DriverStation::isAutonomous); - - @AutoLogOutput(key = "Superstructure/Auto Intake Request") - public static Trigger autoIntakeReq = - new Trigger(() -> autoIntake).and(DriverStation::isAutonomous); - - @AutoLogOutput(key = "Superstructure/Auto Score Request") - public static Trigger autoScoreReq = - new Trigger(() -> autoScore).and(DriverStation::isAutonomous); - - @AutoLogOutput(key = "Superstructure/Auto Climb Request") - public static Trigger autoClimbReq = - new Trigger(() -> autoClimb).and(DriverStation::isAutonomous); - - public enum Action { - FEED, - INTAKE, - SCORE, - CLIMB; - } - - public enum Path { - // R for right - // L for left - // M for middle - // P for park (starting pose but often used for scoring pose ig) - // D for depot - // O for outpost - // C for climb - // S was going to be for scoreing pos but i think we will just score - // F for feeding poses - // I for intake??? - - // may have to rethink naming to some extent and add more poses - - DtoFL("D", "FL", Action.FEED), - FLMtoPL("FLM", "PL", Action.SCORE), - FLtoFLM("FL", "FLM", Action.FEED), - FLtoPL("FL", "PL", Action.SCORE), - FRMtoPR("FRM", "PR", Action.SCORE), - FRtoFRM("FR", "FRM", Action.FEED), - FRtoPR("FR", "PR", Action.SCORE), - OtoFR("O", "FR", Action.FEED), - PLtoCL("PL", "CL", Action.CLIMB), - PLtoCM("PL", "CM", Action.CLIMB), - PLtoD("PL", "D", Action.INTAKE), - PLtoFL("PL", "FL", Action.FEED), - PRtoCM("PR", "CM", Action.CLIMB), - PRtoCR("PR", "CR", Action.CLIMB), - PRtoFR("PR", "FR", Action.FEED), - PRtoO("PR", "O", Action.INTAKE), - // idk seperate intake and feed so action is included makes it easier for me but they use the - // same - // trajectories so i dont have to make new paths - DtoIL("D", "FL", Action.INTAKE), - ILMtoPL("FLM", "PL", Action.SCORE), - ILtoILM("FL", "FLM", Action.INTAKE), - ILtoPL("FL", "PL", Action.SCORE), - IRMtoPR("FRM", "PR", Action.SCORE), - IRtoIRM("FR", "FRM", Action.INTAKE), - IRtoPR("FR", "PR", Action.SCORE), - OtoIR("O", "FR", Action.INTAKE), - PLtoIL("PL", "FL", Action.INTAKE), - PRtoIR("PR", "FR", Action.INTAKE); - - private final String start; - private final String end; - private final Action action; - - private Path(String start, String end, Action action) { - this.start = start; - this.end = end; - this.action = action; - } - - public AutoTrajectory getTrajectory(AutoRoutine routine) { - // AutoRoutine docs say that this "creates" a new trajectory, but the factory does check if - // it's already present - return routine.trajectory(start + "to" + end); - } - } - - public Autos(SwerveSubsystem swerve) { - factory = - new AutoFactory( - swerve::getPose, - swerve::resetPose, - swerve.choreoDriveController(), - true, - swerve, - (traj, edge) -> { - if (Robot.ROBOT_MODE != RobotMode.REAL) - Logger.recordOutput( - "Choreo/Active Traj", - DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get().equals(Alliance.Blue) - ? traj.getPoses() - : traj.flipped().getPoses()); - }); - } - - public Command leaveAuto() { - final AutoRoutine routine = factory.newRoutine("Leave Auto"); - Path[] paths = {}; - - Command autoCommand = Commands.none(); - - for (Path path : paths) { - autoCommand = - autoCommand.andThen( - Commands.print("Running path: " + path.toString()).andThen(runPath(path, routine))); - } - return routine.cmd(); - } - - public Command runPath(Path path, AutoRoutine routine) { - Action action = path.action; - switch (action) { - case INTAKE: - return intakePath(path, routine); - case FEED: - return feedPath(path, routine); - case SCORE: - return scorePath(path, routine); - case CLIMB: - return climbPath(path, routine); - default: // this should never happen - return Commands.none(); - } - } - - // TODO aligning to climb pos correctly - public Command climbPath(Path path, AutoRoutine routine) { - // path align and climb - return Commands.sequence( - path.getTrajectory(routine) - .cmd() - .until( - routine.observe( - path.getTrajectory(routine) - .atTime( - path.getTrajectory(routine).getRawTrajectory().getTotalTime() - - (0.3)))), - setAutoClimbReqTrue()); - } - - public Command feedPath(Path path, AutoRoutine routine) { - return Commands.sequence( - setAutoFeedReqTrue(), - path.getTrajectory(routine).cmd(), - path.getTrajectory(routine).cmd().until(path.getTrajectory(routine).done()), - setAutoFeedReqFalse()); - } - - public Command scorePath(Path path, AutoRoutine routine) { - // path align and score - return Commands.sequence( - path.getTrajectory(routine) - .cmd() - .until( - routine.observe( - path.getTrajectory(routine) - .atTime( - path.getTrajectory(routine).getRawTrajectory().getTotalTime() - - (0.3)))), - setAutoScoreReqTrue(), - waitUntilEmpty(), - setAutoScoreReqFalse()); - } - - // feeding and intake could prob be improved - public Command intakePath(Path path, AutoRoutine routine) { - return Commands.sequence( - setAutoIntakeReqTrue(), - path.getTrajectory(routine).cmd(), - path.getTrajectory(routine).cmd().until(path.getTrajectory(routine).done()), - setAutoIntakeReqFalse()); - } - - public Command shootPreload() { - return Commands.sequence(setAutoScoreReqTrue(), waitUntilEmpty(), setAutoScoreReqFalse()); - } - - public Command setAutoIntakeReqTrue() { - return Commands.runOnce(() -> autoIntake = true); - } - - public Command setAutoIntakeReqFalse() { - return Commands.runOnce(() -> autoIntake = false); - } - - public Command setAutoScoreReqTrue() { - return Commands.runOnce(() -> autoScore = true); - } - - public Command setAutoScoreReqFalse() { - return Commands.runOnce(() -> autoScore = false); - } - - public Command setAutoFeedReqTrue() { - return Commands.runOnce(() -> autoFeed = true); - } - - public Command setAutoFeedReqFalse() { - return Commands.runOnce(() -> autoFeed = false); - } - - public Command setAutoClimbReqTrue() { - return Commands.runOnce(() -> autoClimb = true); - } - - public Command setAutoClimbReqFalse() { - return Commands.runOnce(() -> autoClimb = false); - } - - // TODO: score at the start of each auto - // specific paths: - // no idea what to name them - public Command getDepotScoreClimbAuto() { - final AutoRoutine routine = factory.newRoutine("Depot Score Clim Auto"); - Path[] paths = {Path.PLtoD, Path.DtoIL, Path.ILtoILM, Path.ILMtoPL, Path.PLtoCL}; - // Will always need to reset odo at the start of a routine - Command autoCommand = - paths[0] - .getTrajectory(routine) - .resetOdometry() - .andThen(shootPreload()); // shoot preload then do the paths - - for (Path p : paths) { - autoCommand = autoCommand.andThen(runPath(p, routine)); - } - - return routine.cmd(); - } - - public Command getOutpostScoreClimbAuto() { - final AutoRoutine routine = factory.newRoutine("Outpost Score Climb Auto"); - Path[] paths = {Path.PRtoO, Path.OtoIR, Path.IRtoIRM, Path.IRMtoPR, Path.PRtoCR}; - Command autoCommand = paths[0].getTrajectory(routine).resetOdometry().andThen(shootPreload()); - - for (Path p : paths) { - autoCommand = autoCommand.andThen(runPath(p, routine)); - } - - return routine.cmd(); - } - - public Command getDepotFeedClimbAuto() { - final AutoRoutine routine = factory.newRoutine("Depot Feed Climb Auto"); - Path[] paths = {Path.PLtoD, Path.DtoFL, Path.FLtoFLM, Path.FLMtoPL, Path.PLtoCL}; - Command autoCommand = paths[0].getTrajectory(routine).resetOdometry().andThen(shootPreload()); - - for (Path p : paths) { - autoCommand = autoCommand.andThen(runPath(p, routine)); - } - - return routine.cmd(); - } - - public Command getOutpostFeedClimbAuto() { - final AutoRoutine routine = factory.newRoutine("Outpost Feed Climb Auto"); - Path[] paths = {Path.PLtoD, Path.DtoFL, Path.FLtoFLM, Path.FLMtoPL, Path.PLtoCL}; - Command autoCommand = paths[0].getTrajectory(routine).resetOdometry().andThen(shootPreload()); - - for (Path p : paths) { - autoCommand = autoCommand.andThen(runPath(p, routine)); - } - - return routine.cmd(); - } - - public Command waitUntilEmpty() { - // TODO wait till robot empty / done scoring - // return null; - return Commands.waitSeconds(0.5); - } -} From 9434b289f1a6a039e487ebbaae7bf7a5f8c7f1dc Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 22 Jan 2026 21:46:17 -0800 Subject: [PATCH 7/8] wait wait i messed up --- src/main/java/frc/robot/Autos.java | 132 +++++++++++++++++++++++++++++ 1 file changed, 132 insertions(+) create mode 100644 src/main/java/frc/robot/Autos.java diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java new file mode 100644 index 0000000..3514700 --- /dev/null +++ b/src/main/java/frc/robot/Autos.java @@ -0,0 +1,132 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot; + +import choreo.auto.AutoFactory; +import choreo.auto.AutoRoutine; +import choreo.auto.AutoTrajectory; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Robot.RobotMode; +import frc.robot.subsystems.swerve.SwerveSubsystem; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +/** Add your docs here. */ +public class Autos { + private final AutoFactory factory; + + // Declare triggers + // mehhhhhhh + private static boolean autoPreScore; + private static boolean autoScore; + private static boolean autoIntake; + private static boolean autoFeed; + + // private static boolean autoIntakeAlgae; + + @AutoLogOutput(key = "Superstructure/Auto Pre Score Request") + public static Trigger autoPreScoreReq = + new Trigger(() -> autoPreScore).and(DriverStation::isAutonomous); + + @AutoLogOutput(key = "Superstructure/Auto Score Request") + public static Trigger autoScoreReq = + new Trigger(() -> autoScore).and(DriverStation::isAutonomous); + + @AutoLogOutput(key = "Superstructure/Auto Intake Request") + public static Trigger autoIntakeReq = + new Trigger(() -> autoIntake).and(DriverStation::isAutonomous); + + @AutoLogOutput(key = "Superstructure/Auto Feed Request") + public static Trigger autoFeedReq = new Trigger(() -> autoFeed).and(DriverStation::isAutonomous); + + public enum PathEndType { + PLACEHOLDER; + } + + public enum Path { + PLACEHOLDER("placeholder", "placeholder", PathEndType.PLACEHOLDER); + + private final String start; + private final String end; + private final PathEndType type; + + private Path(String start, String end, PathEndType type) { + this.start = start; + this.end = end; + this.type = type; + } + + public AutoTrajectory getTrajectory(AutoRoutine routine) { + // AutoRoutine docs say that this "creates" a new trajectory, but the factory does check if + // it's already present + return routine.trajectory(start + "to" + end); + } + } + + public Autos(SwerveSubsystem swerve) { + factory = + new AutoFactory( + swerve::getPose, + swerve::resetPose, + swerve.choreoDriveController(), + true, + swerve, + (traj, edge) -> { + if (Robot.ROBOT_MODE != RobotMode.REAL) + Logger.recordOutput( + "Choreo/Active Traj", + DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get().equals(Alliance.Blue) + ? traj.getPoses() + : traj.flipped().getPoses()); + }); + } + + // TODO write leave auto + public Command leaveAuto() { + final AutoRoutine routine = factory.newRoutine("Leave Auto"); + Path[] paths = {}; + + Command autoCommand = Commands.none(); + + for (Path path : paths) { + autoCommand = + autoCommand.andThen( + Commands.print("Running path: " + path.toString()).andThen(runPath(path, routine))); + } + return routine.cmd(); + } + + public Command runPath(Path path, AutoRoutine routine) { + PathEndType type = path.type; + switch (type) { + default: // this should never happen + return Commands.none(); + } + } + + public Command setAutoScoreReqTrue() { + return Commands.runOnce( + () -> { + autoScore = true; + }); + } + + public Command setAutoPreScoreReqTrue() { + return Commands.runOnce(() -> autoPreScore = true); + } + + public Command setAutoScoreReqFalse() { + return Commands.runOnce( + () -> { + autoScore = false; + autoPreScore = false; + }); + } +} \ No newline at end of file From e8b0d4ada35b664113582d0479759b6d2429bac9 Mon Sep 17 00:00:00 2001 From: spellingcat <70864274+spellingcat@users.noreply.github.com> Date: Thu, 22 Jan 2026 21:50:04 -0800 Subject: [PATCH 8/8] oh my god bruh are we serious. formatter wanted one (1) empty line --- src/main/java/frc/robot/Autos.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index 3514700..97a4364 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -129,4 +129,4 @@ public Command setAutoScoreReqFalse() { autoPreScore = false; }); } -} \ No newline at end of file +}