From 217cf9fbe9fa0a00aaad3bf7783439cf8a341cf9 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Wed, 14 Jan 2026 22:25:14 -0800 Subject: [PATCH 01/32] Basically blank commit --- src/main/java/frc/robot/Superstructure.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index eb716e5..d02b8b0 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -165,7 +165,7 @@ private void addTransitions() { // Maybe should be a transition from idle to flow as well? In case robot doesn't already have // a fuel } - + // Transition from any state to SPIT for anti jamming antiJamReq.onTrue(changeStateTo(SuperState.SPIT)); From d6625a769e172140e0edb522c2937aa1d905d216 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 15 Jan 2026 08:47:29 -0800 Subject: [PATCH 02/32] Untrack the changed files --- src/main/java/frc/robot/Robot.java | 432 ------------------ src/main/java/frc/robot/Superstructure.java | 286 ------------ .../frc/robot/subsystems/IntakeSubsystem.java | 23 - .../robot/subsystems/RoutingSubsystem.java | 36 -- .../robot/subsystems/ShooterSubsystem.java | 18 - 5 files changed, 795 deletions(-) delete mode 100644 src/main/java/frc/robot/Robot.java delete mode 100644 src/main/java/frc/robot/Superstructure.java delete mode 100644 src/main/java/frc/robot/subsystems/IntakeSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/RoutingSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/ShooterSubsystem.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java deleted file mode 100644 index 5358727..0000000 --- a/src/main/java/frc/robot/Robot.java +++ /dev/null @@ -1,432 +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 static edu.wpi.first.units.Units.Meter; - -import com.ctre.phoenix6.CANBus; -import com.ctre.phoenix6.SignalLogger; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.wpilibj.Alert; -import edu.wpi.first.wpilibj.Alert.AlertType; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.PowerDistribution; -import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; -import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; -import edu.wpi.first.wpilibj2.command.button.Trigger; -import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.RoutingSubsystem; -import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.led.LEDIOReal; -import frc.robot.subsystems.led.LEDSubsystem; -import frc.robot.subsystems.swerve.SwerveSubsystem; -import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread; -import frc.robot.utils.CommandXboxControllerSubsystem; -import java.util.Optional; -import java.util.Set; -import org.ironmaple.simulation.SimulatedArena; -import org.ironmaple.simulation.drivesims.COTS; -import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; -import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.LogFileUtil; -import org.littletonrobotics.junction.LoggedRobot; -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import org.littletonrobotics.junction.networktables.NT4Publisher; -import org.littletonrobotics.junction.wpilog.WPILOGReader; -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 boolean TUNING_MODE = true; - public boolean hasZeroedSinceStartup = false; - - public enum RobotType { - REAL, - SIM, - REPLAY - } - - private final Alert driverJoystickDisconnectedAlert = - new Alert("Driver controller disconnected!", AlertType.kError); - private final Alert operatorJoystickDisconnectedAlert = - new Alert("Operator controller disconnected!", AlertType.kError); - private final Alert canErrorAlert = - new Alert("CAN errors detected, robot may not be controllable.", AlertType.kError); - private final Alert canivoreErrorAlert = - new Alert("CANivore errors detected, robot may not be controllable.", AlertType.kError); - private final Alert lowBatteryAlert = - new Alert( - "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(); - private final Timer disabledTimer = new Timer(); - private static final double CAN_ERROR_TIME_THRESHOLD = 0.5; // Seconds to disable alert - private static final double CANIVORE_ERROR_TIME_THRESHOLD = 0.5; - - private static int lowBatteryCycleCount = 0; - private static final double lowBatteryVoltage = 11.8; // TODO tune - private static final double lowBatteryDisabledTime = 1.5; - private static final double lowBatteryMinCycleCount = 10; - - // Instantiate subsystems - - // Maple Sim Stuff - private final DriveTrainSimulationConfig driveTrainSimConfig = - DriveTrainSimulationConfig.Default() - .withGyro(COTS.ofPigeon2()) - .withSwerveModule( - COTS.ofMark4n( - DCMotor.getKrakenX60Foc(1), - DCMotor.getKrakenX60Foc(1), - // Still not sure where the 1.5 came from - 1.5, - // Running l2+ swerve modules - 2)) - .withTrackLengthTrackWidth( - Meter.of(SwerveSubsystem.SWERVE_CONSTANTS.getTrackWidthX()), - Meter.of(SwerveSubsystem.SWERVE_CONSTANTS.getTrackWidthY())) - .withBumperSize( - Meter.of(SwerveSubsystem.SWERVE_CONSTANTS.getBumperWidth()), - Meter.of(SwerveSubsystem.SWERVE_CONSTANTS.getBumperLength())) - .withRobotMass(SwerveSubsystem.SWERVE_CONSTANTS.getMass()); - - private final SwerveDriveSimulation swerveSimulation = - new SwerveDriveSimulation(driveTrainSimConfig, new Pose2d(3, 3, Rotation2d.kZero)); - // Subsystem initialization - private final SwerveSubsystem swerve = new SwerveSubsystem(swerveSimulation, canivore); - private final LEDSubsystem leds; - - private final RoutingSubsystem routing = new RoutingSubsystem(); - private final IntakeSubsystem intake = new IntakeSubsystem(); - private final ShooterSubsystem shooter = new ShooterSubsystem(); - - private final CommandXboxControllerSubsystem driver = new CommandXboxControllerSubsystem(0); - private final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1); - - @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 = - new Superstructure(swerve, routing, intake, shooter, driver, operator); - - private final Autos autos; - private Optional lastAlliance = Optional.empty(); - @AutoLogOutput boolean haveAutosGenerated = false; - private final LoggedDashboardChooser autoChooser = new LoggedDashboardChooser<>("Autos"); - - // Logged mechanisms - - @SuppressWarnings("resource") - public Robot() { - 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("RuntimeType", getRuntimeType().toString()); - Logger.recordMetadata("Robot Mode", ROBOT_TYPE.toString()); - Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); - Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); - Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); - switch (BuildConstants.DIRTY) { - case 0: - Logger.recordMetadata("GitDirty", "All changes committed"); - break; - case 1: - Logger.recordMetadata("GitDirty", "Uncommitted changes"); - break; - default: - Logger.recordMetadata("GitDirty", "Unknown"); - break; - } - - switch (ROBOT_TYPE) { - 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 - break; - case REPLAY: - setUseTiming(false); // Run as fast as possible - String logPath = - LogFileUtil - .findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user) - Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log - Logger.addDataReceiver( - new WPILOGWriter( - LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log - break; - case SIM: - Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables - break; - } - Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may - // be added. - - Logger.recordOutput("Canivore Status", canivore.getStatus().Status); - - PhoenixOdometryThread.getInstance().start(); - - leds = new LEDSubsystem(new LEDIOReal()); - - // Set default commands - - driver.setDefaultCommand(driver.rumbleCmd(0.0, 0.0)); - operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0)); - - if (ROBOT_TYPE == RobotType.SIM) { - SimulatedArena.getInstance().addDriveTrainSimulation(swerveSimulation); - } - - swerve.setDefaultCommand( - swerve.driveOpenLoopFieldRelative( - () -> - new ChassisSpeeds( - modifyJoystick(driver.getLeftY()) - * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - modifyJoystick(driver.getLeftX()) - * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), - modifyJoystick(driver.getRightX()) - * SwerveSubsystem.SWERVE_CONSTANTS.getMaxAngularSpeed()) - .times(-1))); - - addControllerBindings(); - - 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( - () -> { - var allianceChanged = !DriverStation.getAlliance().equals(lastAlliance); - lastAlliance = DriverStation.getAlliance(); - return allianceChanged && DriverStation.getAlliance().isPresent(); - }) - .onTrue( - Commands.runOnce(() -> addAutos()) - .alongWith(leds.blinkCmd(Color.kWhite, Color.kBlack, 20.0).withTimeout(1.0)) - .ignoringDisable(true)); - // TODO tbh idk if the leds will work here - - // Add autos when first connecting to DS - new Trigger( - () -> - DriverStation.isDSAttached() - && DriverStation.getAlliance().isPresent() - && !haveAutosGenerated) - .onTrue(Commands.print("connected")) - .onTrue( - 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)); - - // Reset alert timers - canInitialErrorTimer.restart(); - canErrorTimer.restart(); - canivoreErrorTimer.restart(); - disabledTimer.restart(); - } - - /** Scales a joystick value for teleop driving */ - 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 - .leftStick() - .and(driver.rightStick()) - .onTrue( - Commands.runOnce( - () -> - swerve.setYaw( - DriverStation.getAlliance().equals(Alliance.Blue) - // ? Rotation2d.kCW_90deg - // : Rotation2d.kCCW_90deg))); - ? Rotation2d.kZero - : Rotation2d.k180deg))); - - // ---zeroing stuff--- - - new Trigger(() -> DriverStation.isJoystickConnected(0)) - .negate() - .onTrue(Commands.runOnce(() -> driverJoystickDisconnectedAlert.set(true))) - .onFalse(Commands.runOnce(() -> driverJoystickDisconnectedAlert.set(false))); - - new Trigger(() -> DriverStation.isJoystickConnected(1)) - .negate() - .onTrue(Commands.runOnce(() -> operatorJoystickDisconnectedAlert.set(true))) - .onFalse(Commands.runOnce(() -> operatorJoystickDisconnectedAlert.set(false))); - } - - private void addAutos() { - System.out.println("------- Regenerating Autos"); - System.out.println( - "Regenerating Autos on " + DriverStation.getAlliance().map((a) -> a.toString())); - haveAutosGenerated = true; - } - - @Override - public void robotPeriodic() { - CommandScheduler.getInstance().run(); - superstructure.periodic(); - - // Log mechanism poses - - // Check CAN status - var canStatus = RobotController.getCANStatus(); - if (canStatus.transmitErrorCount > 0 || canStatus.receiveErrorCount > 0) { - canErrorTimer.restart(); - } - canErrorAlert.set( - !canErrorTimer.hasElapsed(CAN_ERROR_TIME_THRESHOLD) - && !canInitialErrorTimer.hasElapsed(CAN_ERROR_TIME_THRESHOLD)); - - // Log CANivore status - if (Robot.isReal()) { - var canivoreStatus = - Optional.of(canivore.getStatus()); // TODO i don't know if i'm doing the optionaling right - if (canivoreStatus.isPresent()) { - Logger.recordOutput("CANivoreStatus/Status", canivoreStatus.get().Status.getName()); - Logger.recordOutput("CANivoreStatus/Utilization", canivoreStatus.get().BusUtilization); - Logger.recordOutput("CANivoreStatus/OffCount", canivoreStatus.get().BusOffCount); - Logger.recordOutput("CANivoreStatus/TxFullCount", canivoreStatus.get().TxFullCount); - Logger.recordOutput("CANivoreStatus/ReceiveErrorCount", canivoreStatus.get().REC); - Logger.recordOutput("CANivoreStatus/TransmitErrorCount", canivoreStatus.get().TEC); - - if (!canivoreStatus.get().Status.isOK() - || canStatus.transmitErrorCount > 0 - || canStatus.receiveErrorCount > 0) { - canivoreErrorTimer.restart(); - } - } - // TODO i don't really like how this doesn't seem to be sticky - canivoreErrorAlert.set( - !canivoreErrorTimer.hasElapsed(CANIVORE_ERROR_TIME_THRESHOLD) - && !canInitialErrorTimer.hasElapsed(CAN_ERROR_TIME_THRESHOLD)); - } - - // Low battery alert - lowBatteryCycleCount += 1; - if (DriverStation.isEnabled()) { - disabledTimer.reset(); - } - if (RobotController.getBatteryVoltage() <= lowBatteryVoltage - && disabledTimer.hasElapsed(lowBatteryDisabledTime) - && lowBatteryCycleCount >= lowBatteryMinCycleCount) { - lowBatteryAlert.set(true); - } - } - - @Override - public void simulationInit() { - // Sets the odometry pose to start at the same place as maple sim pose - swerve.resetPose(swerveSimulation.getSimulatedDriveTrainPose()); - } - - @Override - public void simulationPeriodic() { - // Update maple simulation - SimulatedArena.getInstance().simulationPeriodic(); - // Log simulated pose - Logger.recordOutput("MapleSim/Pose", swerveSimulation.getSimulatedDriveTrainPose()); - } - - @Override - public void disabledInit() {} - - @Override - public void disabledPeriodic() {} - - @Override - public void disabledExit() {} - - @Override - public void autonomousInit() {} - - @Override - public void autonomousPeriodic() {} - - @Override - public void autonomousExit() {} - - @Override - public void teleopInit() {} - - @Override - public void teleopPeriodic() {} - - @Override - public void teleopExit() {} - - @Override - public void testInit() { - CommandScheduler.getInstance().cancelAll(); - } - - @Override - public void testPeriodic() {} - - @Override - public void testExit() {} -} diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java deleted file mode 100644 index d02b8b0..0000000 --- a/src/main/java/frc/robot/Superstructure.java +++ /dev/null @@ -1,286 +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 edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.Timer; -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.IntakeSubsystem; -import frc.robot.subsystems.RoutingSubsystem; -import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.swerve.SwerveSubsystem; -import frc.robot.utils.CommandXboxControllerSubsystem; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; - -public class Superstructure { - - /** - * We should have a state for every single action the robot will perform. - */ - public enum SuperState { - IDLE, - INTAKE, - READY, - FEED, - FEED_FLOW, - SCORE, - SCORE_FLOW, - SPIT; - public final Trigger trigger; - - private SuperState() { - trigger = new Trigger(() -> state == this); - } - - public Trigger getTrigger() { - return trigger; - } - } - - @AutoLogOutput(key = "Superstructure/State") - private static SuperState state = SuperState.IDLE; - - private SuperState prevState = SuperState.IDLE; - - private Timer stateTimer = new Timer(); - - private final SwerveSubsystem swerve; - private final RoutingSubsystem routing; - private final IntakeSubsystem intake; - private final ShooterSubsystem shooter; - private final CommandXboxControllerSubsystem driver; - private final CommandXboxControllerSubsystem operator; - - // Declare triggers - @AutoLogOutput(key = "Superstructure/Score Request") - private Trigger scoreReq; - - @AutoLogOutput(key = "Superstructure/Intake Request") - private Trigger intakeReq; - - @AutoLogOutput(key = "Superstructure/Feed Request") - private Trigger feedReq; - - @AutoLogOutput(key = "Superstructre/Flowstate Request") - private Trigger flowReq; - - @AutoLogOutput(key = "Superstructre/Anti Jam Req") - private Trigger antiJamReq; - - @AutoLogOutput(key = "Superstructure/Is Full") - private Trigger isFull; - - @AutoLogOutput(key = "Superstructure/Is Empty") - private Trigger isEmpty; - - // @AutoLogOutput(key = "Superstructure/At Extension?") - // public Trigger atExtensionTrigger = new Trigger(this::atExtension).or(Robot::isSimulation); - - /** Creates a new Superstructure. */ - public Superstructure( - SwerveSubsystem swerve, - RoutingSubsystem routing, - IntakeSubsystem intake, - ShooterSubsystem shooter, - CommandXboxControllerSubsystem driver, - CommandXboxControllerSubsystem operator) { - this.swerve = swerve; - this.routing = routing; - this.intake = intake; - this.shooter = shooter; - this.driver = driver; - this.operator = operator; - - addTriggers(); - addTransitions(); - addCommands(); - - stateTimer.start(); - } - - private void addTriggers() { - // TODO: THESE BINDINGS WILL LIKELY CHANGE. SHOULD HAVE A FULL MEETING TO DISCUSS - scoreReq = - driver - .rightTrigger() - .and(DriverStation::isTeleop) - .or(Autos.autoScoreReq); // Maybe should include if its our turn? - - intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq); - - feedReq = driver.rightBumper().and(DriverStation::isTeleop).or(Autos.autoFeedReq); - - flowReq = operator.rightTrigger(); - - antiJamReq = driver.a().or(operator.a()); - - isFull = new Trigger(routing::isFull); - - isEmpty = new Trigger(routing::isEmpty); - } - - private void addTransitions() { - bindTransition(SuperState.IDLE, SuperState.INTAKE, intakeReq); - - bindTransition(SuperState.INTAKE, SuperState.IDLE, intakeReq.negate().and(isEmpty)); - - bindTransition( - SuperState.INTAKE, SuperState.READY, (intakeReq.negate().and(isEmpty.negate())).or(isFull)); - - bindTransition(SuperState.INTAKE, SuperState.FEED, feedReq); - - bindTransition(SuperState.INTAKE, SuperState.SCORE, scoreReq); - - bindTransition(SuperState.READY, SuperState.INTAKE, intakeReq.and(isFull.negate())); - - bindTransition(SuperState.READY, SuperState.FEED, feedReq); - - bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty); - - bindTransition(SuperState.READY, SuperState.SCORE, scoreReq); - - bindTransition(SuperState.SCORE, SuperState.IDLE, isEmpty); - - // FEED_FLOW transitions - { - bindTransition(SuperState.FEED, SuperState.FEED_FLOW, flowReq); - - // No so sure about the end condition here. - bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, flowReq.negate()); - - // Maybe should be a transition from idle to flow as well? In case robot doesn't already have - // a fuel - } - - // SCORE_FLOW transitions - { - bindTransition(SuperState.SCORE, SuperState.SCORE_FLOW, flowReq); - - bindTransition(SuperState.SCORE_FLOW, SuperState.IDLE, flowReq.negate()); - // Maybe should be a transition from idle to flow as well? In case robot doesn't already have - // a fuel - } - - // Transition from any state to SPIT for anti jamming - antiJamReq.onTrue(changeStateTo(SuperState.SPIT)); - - bindTransition(SuperState.SPIT, SuperState.IDLE, antiJamReq.negate()); - } - - private void addCommands() { - bindCommands( - SuperState.IDLE, - intake.rest(), - routing.rest(), - shooter.rest()); // Maybe the routing should be indexing? - - bindCommands(SuperState.INTAKE, intake.intake(), routing.index(), shooter.rest()); - - bindCommands( - SuperState.READY, - intake.rest(), - routing.index(), - shooter.rest()); // Maybe index at slower speed? - - bindCommands(SuperState.SCORE, intake.rest(), routing.index(), shooter.shoot()); - - bindCommands(SuperState.SCORE_FLOW, intake.intake(), routing.index(), shooter.shoot()); - - bindCommands(SuperState.FEED, intake.rest(), routing.index(), shooter.feed()); - - bindCommands(SuperState.FEED_FLOW, intake.intake(), routing.index(), shooter.feed()); - - bindCommands(SuperState.SPIT, intake.spit(), routing.reverseIndex(), shooter.shoot()); - } - - public void periodic() { - Logger.recordOutput("Superstructure/Superstructure State", state); - Logger.recordOutput("Superstructure/State Timer", stateTimer.get()); - } - - /** - * @param start first state - * @param end second state - * @param trigger trigger to make it go from the first state to the second (assuming it's already - * in the first state) - */ - private void bindTransition(SuperState start, SuperState end, Trigger trigger) { - // when 1) the robot is in the start state and 2) the trigger is true, the robot changes state - // to the end state - trigger.and(start.getTrigger()).onTrue(changeStateTo(end)); - } - - /** - * @param start first state - * @param end second state - * @param trigger trigger to make it go from the first state to the second (assuming it's already - * in the first state) - * @param cmd some command to run while making the transition - */ - private void bindTransition(SuperState start, SuperState end, Trigger trigger, Command cmd) { - // when 1) the robot is in the start state and 2) the trigger is true, the robot changes state - // to the end state IN PARALLEL to running the command that got passed in - trigger.and(start.getTrigger()).onTrue(Commands.parallel(changeStateTo(end), cmd)); - } - - /** - * Runs the passed in command(s) in parallel when the superstructure is in the passed in state - * - * @param state - * @param commands - */ - private void bindCommands(SuperState state, Command... commands) { - state.getTrigger().whileTrue(Commands.parallel(commands)); - } - - // public boolean atExtension(SuperState state) { - // } - - // public boolean atExtension() { - // return atExtension(state); - // } - - private Command changeStateTo(SuperState nextState) { - return Commands.runOnce( - () -> { - System.out.println("Changing state from " + state + " to " + nextState); - stateTimer.reset(); - this.prevState = state; - state = nextState; - setSubstates(); - }) - .ignoringDisable(true) - .withName("State Change Command"); - } - - private void setSubstates() {} - - // public Command transitionAfterZeroing() { - // } - - /** - * Only for setting initial state at the beginning of auto - * - * @param state the state to set to - */ - public void resetStateForAuto(SuperState nextState) { - System.out.println("Resetting state from " + state + " to " + nextState + " for auto."); - stateTimer.reset(); - this.prevState = state; - state = nextState; - setSubstates(); - } - - public static SuperState getState() { - return state; - } - - public boolean stateIsIdle() { - return getState() == SuperState.IDLE; - } -} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java deleted file mode 100644 index 80f7af6..0000000 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -// TODO -public class IntakeSubsystem extends SubsystemBase { - public Command intake() { - // TODO - return idle(); - } - - // Can't call it idle bc idle is smthing else - public Command rest() { - // TODO - return idle(); - } - - public Command spit() { - // TODO - return idle(); - } -} diff --git a/src/main/java/frc/robot/subsystems/RoutingSubsystem.java b/src/main/java/frc/robot/subsystems/RoutingSubsystem.java deleted file mode 100644 index 8bd19e0..0000000 --- a/src/main/java/frc/robot/subsystems/RoutingSubsystem.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -// TODO!!! -public class RoutingSubsystem extends SubsystemBase { - - // TODO: BASE ON ACTUAL SENSOR READINGS - private boolean isFull; - private boolean isEmpty; - - public boolean isFull() { - return isFull; - } - - public boolean isEmpty() { - return isEmpty; - } - - // TODO!! - public Command index() { - return this.run(() -> {}); - } - - // Can't call it idle bc idle is smthing else - public Command rest() { - // TODO - return idle(); - } - - public Command reverseIndex() { - // TODO - return this.run(() -> {}); - } -} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java deleted file mode 100644 index d3fad56..0000000 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ /dev/null @@ -1,18 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ShooterSubsystem extends SubsystemBase { - public Command shoot() { - return this.idle(); - } - - public Command feed() { - return this.idle(); - } - - public Command rest() { - return this.idle(); - } -} From c4abb0b1f1dc8e6aa091b530d675747265fcdda4 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Thu, 15 Jan 2026 08:47:45 -0800 Subject: [PATCH 03/32] Re-track them --- .github/workflows/checks.yml | 73 +++ notes/superstructure.md | 39 ++ src/main/java/frc/robot/Robot.java | 432 ++++++++++++++++++ src/main/java/frc/robot/Superstructure.java | 286 ++++++++++++ .../frc/robot/subsystems/IntakeSubsystem.java | 23 + .../robot/subsystems/RoutingSubsystem.java | 36 ++ .../robot/subsystems/ShooterSubsystem.java | 18 + 7 files changed, 907 insertions(+) create mode 100644 .github/workflows/checks.yml create mode 100644 notes/superstructure.md create mode 100644 src/main/java/frc/robot/Robot.java create mode 100644 src/main/java/frc/robot/Superstructure.java create mode 100644 src/main/java/frc/robot/subsystems/IntakeSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/RoutingSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/ShooterSubsystem.java diff --git a/.github/workflows/checks.yml b/.github/workflows/checks.yml new file mode 100644 index 0000000..4c13e52 --- /dev/null +++ b/.github/workflows/checks.yml @@ -0,0 +1,73 @@ +# These are just from last year, but with new wpilib docker containers +name: checks + +on: + push: + branches: ["main"] + pull_request: + +jobs: + # Checks that the build succeded + build: + name: Build + runs-on: ubuntu-latest + + # Wpilib docker container + # Latest as of 1/11/26 + # Need to use a container without -py. The ones with -py don't have java installed + container: wpilib/roborio-cross-ubuntu:2025-22.04 + + # Steps represent a sequence of tasks that will be executed as part of the job + # Copied from onseason bot's action + steps: + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v3 + + # Declares the repository safe and not under dubious ownership. + - name: Add repository to git safe directories + run: git config --global --add safe.directory $GITHUB_WORKSPACE + + # Grant execute permission for gradlew + - name: Grant execute permission for gradlew + run: chmod +x gradlew + + # Runs a single command using the runners shell + - name: Compile and run tests on robot code + run: ./gradlew build + + format: + name: Format + + permissions: + # Gives permissions for any formatting changes to be commited + contents: write + + runs-on: ubuntu-latest + + # Wpilib docker container + # Latest as of 1/11/26 + # Need to use a container without -py. The ones with -py don't have java installed + container: wpilib/roborio-cross-ubuntu:2025-22.04 + + # Steps represent a sequence of tasks that will be executed as part of the job + # Copied from Onseason implementation + steps: + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v3 + + # Declares the repository safe and not under dubious ownership. + - name: Add repository to git safe directories + run: git config --global --add safe.directory $GITHUB_WORKSPACE + + # Grant execute permission for gradlew + - name: Grant execute permission for gradlew + run: chmod +x gradlew + + # Runs a single command using the runners shell + - name: Compile and run tests on robot code + run: ./gradlew spotlessApply + + # Commit changes + - uses: stefanzweifel/git-auto-commit-action@v5 + with: + commit_message: run spotless \ No newline at end of file diff --git a/notes/superstructure.md b/notes/superstructure.md new file mode 100644 index 0000000..b236a1a --- /dev/null +++ b/notes/superstructure.md @@ -0,0 +1,39 @@ +```mermaid +%%{init: {"flowchart": {"defaultRenderer": "elk"}} }%% +graph TD; + +IDLE + +subgraph BALL + +IDLE <--> |intakeReq + intakeEmpty| INTAKE + +READY <--> |intakeReq + empty| INTAKE + +READY --> |feedReq| FEED + +READY --> |scoreReq| SCORE + + +FEED --> |flowReq| FEED_FLOW + +SCORE --> |flowReq| SCORE_FLOW + + +FEED --> |empty| IDLE + +SCORE --> |empty| IDLE + +FEED <--> |position on field + no scoring when not our turn| SCORE + +FEED_FLOW --> |empty| IDLE + +SCORE_FLOW --> |empty| IDLE + +end + +subgraph ANTI_JAM + +SPIT + +end diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java new file mode 100644 index 0000000..5358727 --- /dev/null +++ b/src/main/java/frc/robot/Robot.java @@ -0,0 +1,432 @@ +// 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 static edu.wpi.first.units.Units.Meter; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.SignalLogger; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.Alert; +import edu.wpi.first.wpilibj.Alert.AlertType; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.PowerDistribution; +import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.RoutingSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.led.LEDIOReal; +import frc.robot.subsystems.led.LEDSubsystem; +import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.subsystems.swerve.odometry.PhoenixOdometryThread; +import frc.robot.utils.CommandXboxControllerSubsystem; +import java.util.Optional; +import java.util.Set; +import org.ironmaple.simulation.SimulatedArena; +import org.ironmaple.simulation.drivesims.COTS; +import org.ironmaple.simulation.drivesims.SwerveDriveSimulation; +import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.LogFileUtil; +import org.littletonrobotics.junction.LoggedRobot; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; +import org.littletonrobotics.junction.networktables.NT4Publisher; +import org.littletonrobotics.junction.wpilog.WPILOGReader; +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 boolean TUNING_MODE = true; + public boolean hasZeroedSinceStartup = false; + + public enum RobotType { + REAL, + SIM, + REPLAY + } + + private final Alert driverJoystickDisconnectedAlert = + new Alert("Driver controller disconnected!", AlertType.kError); + private final Alert operatorJoystickDisconnectedAlert = + new Alert("Operator controller disconnected!", AlertType.kError); + private final Alert canErrorAlert = + new Alert("CAN errors detected, robot may not be controllable.", AlertType.kError); + private final Alert canivoreErrorAlert = + new Alert("CANivore errors detected, robot may not be controllable.", AlertType.kError); + private final Alert lowBatteryAlert = + new Alert( + "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(); + private final Timer disabledTimer = new Timer(); + private static final double CAN_ERROR_TIME_THRESHOLD = 0.5; // Seconds to disable alert + private static final double CANIVORE_ERROR_TIME_THRESHOLD = 0.5; + + private static int lowBatteryCycleCount = 0; + private static final double lowBatteryVoltage = 11.8; // TODO tune + private static final double lowBatteryDisabledTime = 1.5; + private static final double lowBatteryMinCycleCount = 10; + + // Instantiate subsystems + + // Maple Sim Stuff + private final DriveTrainSimulationConfig driveTrainSimConfig = + DriveTrainSimulationConfig.Default() + .withGyro(COTS.ofPigeon2()) + .withSwerveModule( + COTS.ofMark4n( + DCMotor.getKrakenX60Foc(1), + DCMotor.getKrakenX60Foc(1), + // Still not sure where the 1.5 came from + 1.5, + // Running l2+ swerve modules + 2)) + .withTrackLengthTrackWidth( + Meter.of(SwerveSubsystem.SWERVE_CONSTANTS.getTrackWidthX()), + Meter.of(SwerveSubsystem.SWERVE_CONSTANTS.getTrackWidthY())) + .withBumperSize( + Meter.of(SwerveSubsystem.SWERVE_CONSTANTS.getBumperWidth()), + Meter.of(SwerveSubsystem.SWERVE_CONSTANTS.getBumperLength())) + .withRobotMass(SwerveSubsystem.SWERVE_CONSTANTS.getMass()); + + private final SwerveDriveSimulation swerveSimulation = + new SwerveDriveSimulation(driveTrainSimConfig, new Pose2d(3, 3, Rotation2d.kZero)); + // Subsystem initialization + private final SwerveSubsystem swerve = new SwerveSubsystem(swerveSimulation, canivore); + private final LEDSubsystem leds; + + private final RoutingSubsystem routing = new RoutingSubsystem(); + private final IntakeSubsystem intake = new IntakeSubsystem(); + private final ShooterSubsystem shooter = new ShooterSubsystem(); + + private final CommandXboxControllerSubsystem driver = new CommandXboxControllerSubsystem(0); + private final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1); + + @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 = + new Superstructure(swerve, routing, intake, shooter, driver, operator); + + private final Autos autos; + private Optional lastAlliance = Optional.empty(); + @AutoLogOutput boolean haveAutosGenerated = false; + private final LoggedDashboardChooser autoChooser = new LoggedDashboardChooser<>("Autos"); + + // Logged mechanisms + + @SuppressWarnings("resource") + public Robot() { + 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("RuntimeType", getRuntimeType().toString()); + Logger.recordMetadata("Robot Mode", ROBOT_TYPE.toString()); + Logger.recordMetadata("GitSHA", BuildConstants.GIT_SHA); + Logger.recordMetadata("GitDate", BuildConstants.GIT_DATE); + Logger.recordMetadata("GitBranch", BuildConstants.GIT_BRANCH); + switch (BuildConstants.DIRTY) { + case 0: + Logger.recordMetadata("GitDirty", "All changes committed"); + break; + case 1: + Logger.recordMetadata("GitDirty", "Uncommitted changes"); + break; + default: + Logger.recordMetadata("GitDirty", "Unknown"); + break; + } + + switch (ROBOT_TYPE) { + 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 + break; + case REPLAY: + setUseTiming(false); // Run as fast as possible + String logPath = + LogFileUtil + .findReplayLog(); // Pull the replay log from AdvantageScope (or prompt the user) + Logger.setReplaySource(new WPILOGReader(logPath)); // Read replay log + Logger.addDataReceiver( + new WPILOGWriter( + LogFileUtil.addPathSuffix(logPath, "_sim"))); // Save outputs to a new log + break; + case SIM: + Logger.addDataReceiver(new NT4Publisher()); // Publish data to NetworkTables + break; + } + Logger.start(); // Start logging! No more data receivers, replay sources, or metadata values may + // be added. + + Logger.recordOutput("Canivore Status", canivore.getStatus().Status); + + PhoenixOdometryThread.getInstance().start(); + + leds = new LEDSubsystem(new LEDIOReal()); + + // Set default commands + + driver.setDefaultCommand(driver.rumbleCmd(0.0, 0.0)); + operator.setDefaultCommand(operator.rumbleCmd(0.0, 0.0)); + + if (ROBOT_TYPE == RobotType.SIM) { + SimulatedArena.getInstance().addDriveTrainSimulation(swerveSimulation); + } + + swerve.setDefaultCommand( + swerve.driveOpenLoopFieldRelative( + () -> + new ChassisSpeeds( + modifyJoystick(driver.getLeftY()) + * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), + modifyJoystick(driver.getLeftX()) + * SwerveSubsystem.SWERVE_CONSTANTS.getMaxLinearSpeed(), + modifyJoystick(driver.getRightX()) + * SwerveSubsystem.SWERVE_CONSTANTS.getMaxAngularSpeed()) + .times(-1))); + + addControllerBindings(); + + 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( + () -> { + var allianceChanged = !DriverStation.getAlliance().equals(lastAlliance); + lastAlliance = DriverStation.getAlliance(); + return allianceChanged && DriverStation.getAlliance().isPresent(); + }) + .onTrue( + Commands.runOnce(() -> addAutos()) + .alongWith(leds.blinkCmd(Color.kWhite, Color.kBlack, 20.0).withTimeout(1.0)) + .ignoringDisable(true)); + // TODO tbh idk if the leds will work here + + // Add autos when first connecting to DS + new Trigger( + () -> + DriverStation.isDSAttached() + && DriverStation.getAlliance().isPresent() + && !haveAutosGenerated) + .onTrue(Commands.print("connected")) + .onTrue( + 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)); + + // Reset alert timers + canInitialErrorTimer.restart(); + canErrorTimer.restart(); + canivoreErrorTimer.restart(); + disabledTimer.restart(); + } + + /** Scales a joystick value for teleop driving */ + 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 + .leftStick() + .and(driver.rightStick()) + .onTrue( + Commands.runOnce( + () -> + swerve.setYaw( + DriverStation.getAlliance().equals(Alliance.Blue) + // ? Rotation2d.kCW_90deg + // : Rotation2d.kCCW_90deg))); + ? Rotation2d.kZero + : Rotation2d.k180deg))); + + // ---zeroing stuff--- + + new Trigger(() -> DriverStation.isJoystickConnected(0)) + .negate() + .onTrue(Commands.runOnce(() -> driverJoystickDisconnectedAlert.set(true))) + .onFalse(Commands.runOnce(() -> driverJoystickDisconnectedAlert.set(false))); + + new Trigger(() -> DriverStation.isJoystickConnected(1)) + .negate() + .onTrue(Commands.runOnce(() -> operatorJoystickDisconnectedAlert.set(true))) + .onFalse(Commands.runOnce(() -> operatorJoystickDisconnectedAlert.set(false))); + } + + private void addAutos() { + System.out.println("------- Regenerating Autos"); + System.out.println( + "Regenerating Autos on " + DriverStation.getAlliance().map((a) -> a.toString())); + haveAutosGenerated = true; + } + + @Override + public void robotPeriodic() { + CommandScheduler.getInstance().run(); + superstructure.periodic(); + + // Log mechanism poses + + // Check CAN status + var canStatus = RobotController.getCANStatus(); + if (canStatus.transmitErrorCount > 0 || canStatus.receiveErrorCount > 0) { + canErrorTimer.restart(); + } + canErrorAlert.set( + !canErrorTimer.hasElapsed(CAN_ERROR_TIME_THRESHOLD) + && !canInitialErrorTimer.hasElapsed(CAN_ERROR_TIME_THRESHOLD)); + + // Log CANivore status + if (Robot.isReal()) { + var canivoreStatus = + Optional.of(canivore.getStatus()); // TODO i don't know if i'm doing the optionaling right + if (canivoreStatus.isPresent()) { + Logger.recordOutput("CANivoreStatus/Status", canivoreStatus.get().Status.getName()); + Logger.recordOutput("CANivoreStatus/Utilization", canivoreStatus.get().BusUtilization); + Logger.recordOutput("CANivoreStatus/OffCount", canivoreStatus.get().BusOffCount); + Logger.recordOutput("CANivoreStatus/TxFullCount", canivoreStatus.get().TxFullCount); + Logger.recordOutput("CANivoreStatus/ReceiveErrorCount", canivoreStatus.get().REC); + Logger.recordOutput("CANivoreStatus/TransmitErrorCount", canivoreStatus.get().TEC); + + if (!canivoreStatus.get().Status.isOK() + || canStatus.transmitErrorCount > 0 + || canStatus.receiveErrorCount > 0) { + canivoreErrorTimer.restart(); + } + } + // TODO i don't really like how this doesn't seem to be sticky + canivoreErrorAlert.set( + !canivoreErrorTimer.hasElapsed(CANIVORE_ERROR_TIME_THRESHOLD) + && !canInitialErrorTimer.hasElapsed(CAN_ERROR_TIME_THRESHOLD)); + } + + // Low battery alert + lowBatteryCycleCount += 1; + if (DriverStation.isEnabled()) { + disabledTimer.reset(); + } + if (RobotController.getBatteryVoltage() <= lowBatteryVoltage + && disabledTimer.hasElapsed(lowBatteryDisabledTime) + && lowBatteryCycleCount >= lowBatteryMinCycleCount) { + lowBatteryAlert.set(true); + } + } + + @Override + public void simulationInit() { + // Sets the odometry pose to start at the same place as maple sim pose + swerve.resetPose(swerveSimulation.getSimulatedDriveTrainPose()); + } + + @Override + public void simulationPeriodic() { + // Update maple simulation + SimulatedArena.getInstance().simulationPeriodic(); + // Log simulated pose + Logger.recordOutput("MapleSim/Pose", swerveSimulation.getSimulatedDriveTrainPose()); + } + + @Override + public void disabledInit() {} + + @Override + public void disabledPeriodic() {} + + @Override + public void disabledExit() {} + + @Override + public void autonomousInit() {} + + @Override + public void autonomousPeriodic() {} + + @Override + public void autonomousExit() {} + + @Override + public void teleopInit() {} + + @Override + public void teleopPeriodic() {} + + @Override + public void teleopExit() {} + + @Override + public void testInit() { + CommandScheduler.getInstance().cancelAll(); + } + + @Override + public void testPeriodic() {} + + @Override + public void testExit() {} +} diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java new file mode 100644 index 0000000..d02b8b0 --- /dev/null +++ b/src/main/java/frc/robot/Superstructure.java @@ -0,0 +1,286 @@ +// 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 edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +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.IntakeSubsystem; +import frc.robot.subsystems.RoutingSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.swerve.SwerveSubsystem; +import frc.robot.utils.CommandXboxControllerSubsystem; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class Superstructure { + + /** + * We should have a state for every single action the robot will perform. + */ + public enum SuperState { + IDLE, + INTAKE, + READY, + FEED, + FEED_FLOW, + SCORE, + SCORE_FLOW, + SPIT; + public final Trigger trigger; + + private SuperState() { + trigger = new Trigger(() -> state == this); + } + + public Trigger getTrigger() { + return trigger; + } + } + + @AutoLogOutput(key = "Superstructure/State") + private static SuperState state = SuperState.IDLE; + + private SuperState prevState = SuperState.IDLE; + + private Timer stateTimer = new Timer(); + + private final SwerveSubsystem swerve; + private final RoutingSubsystem routing; + private final IntakeSubsystem intake; + private final ShooterSubsystem shooter; + private final CommandXboxControllerSubsystem driver; + private final CommandXboxControllerSubsystem operator; + + // Declare triggers + @AutoLogOutput(key = "Superstructure/Score Request") + private Trigger scoreReq; + + @AutoLogOutput(key = "Superstructure/Intake Request") + private Trigger intakeReq; + + @AutoLogOutput(key = "Superstructure/Feed Request") + private Trigger feedReq; + + @AutoLogOutput(key = "Superstructre/Flowstate Request") + private Trigger flowReq; + + @AutoLogOutput(key = "Superstructre/Anti Jam Req") + private Trigger antiJamReq; + + @AutoLogOutput(key = "Superstructure/Is Full") + private Trigger isFull; + + @AutoLogOutput(key = "Superstructure/Is Empty") + private Trigger isEmpty; + + // @AutoLogOutput(key = "Superstructure/At Extension?") + // public Trigger atExtensionTrigger = new Trigger(this::atExtension).or(Robot::isSimulation); + + /** Creates a new Superstructure. */ + public Superstructure( + SwerveSubsystem swerve, + RoutingSubsystem routing, + IntakeSubsystem intake, + ShooterSubsystem shooter, + CommandXboxControllerSubsystem driver, + CommandXboxControllerSubsystem operator) { + this.swerve = swerve; + this.routing = routing; + this.intake = intake; + this.shooter = shooter; + this.driver = driver; + this.operator = operator; + + addTriggers(); + addTransitions(); + addCommands(); + + stateTimer.start(); + } + + private void addTriggers() { + // TODO: THESE BINDINGS WILL LIKELY CHANGE. SHOULD HAVE A FULL MEETING TO DISCUSS + scoreReq = + driver + .rightTrigger() + .and(DriverStation::isTeleop) + .or(Autos.autoScoreReq); // Maybe should include if its our turn? + + intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq); + + feedReq = driver.rightBumper().and(DriverStation::isTeleop).or(Autos.autoFeedReq); + + flowReq = operator.rightTrigger(); + + antiJamReq = driver.a().or(operator.a()); + + isFull = new Trigger(routing::isFull); + + isEmpty = new Trigger(routing::isEmpty); + } + + private void addTransitions() { + bindTransition(SuperState.IDLE, SuperState.INTAKE, intakeReq); + + bindTransition(SuperState.INTAKE, SuperState.IDLE, intakeReq.negate().and(isEmpty)); + + bindTransition( + SuperState.INTAKE, SuperState.READY, (intakeReq.negate().and(isEmpty.negate())).or(isFull)); + + bindTransition(SuperState.INTAKE, SuperState.FEED, feedReq); + + bindTransition(SuperState.INTAKE, SuperState.SCORE, scoreReq); + + bindTransition(SuperState.READY, SuperState.INTAKE, intakeReq.and(isFull.negate())); + + bindTransition(SuperState.READY, SuperState.FEED, feedReq); + + bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty); + + bindTransition(SuperState.READY, SuperState.SCORE, scoreReq); + + bindTransition(SuperState.SCORE, SuperState.IDLE, isEmpty); + + // FEED_FLOW transitions + { + bindTransition(SuperState.FEED, SuperState.FEED_FLOW, flowReq); + + // No so sure about the end condition here. + bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, flowReq.negate()); + + // Maybe should be a transition from idle to flow as well? In case robot doesn't already have + // a fuel + } + + // SCORE_FLOW transitions + { + bindTransition(SuperState.SCORE, SuperState.SCORE_FLOW, flowReq); + + bindTransition(SuperState.SCORE_FLOW, SuperState.IDLE, flowReq.negate()); + // Maybe should be a transition from idle to flow as well? In case robot doesn't already have + // a fuel + } + + // Transition from any state to SPIT for anti jamming + antiJamReq.onTrue(changeStateTo(SuperState.SPIT)); + + bindTransition(SuperState.SPIT, SuperState.IDLE, antiJamReq.negate()); + } + + private void addCommands() { + bindCommands( + SuperState.IDLE, + intake.rest(), + routing.rest(), + shooter.rest()); // Maybe the routing should be indexing? + + bindCommands(SuperState.INTAKE, intake.intake(), routing.index(), shooter.rest()); + + bindCommands( + SuperState.READY, + intake.rest(), + routing.index(), + shooter.rest()); // Maybe index at slower speed? + + bindCommands(SuperState.SCORE, intake.rest(), routing.index(), shooter.shoot()); + + bindCommands(SuperState.SCORE_FLOW, intake.intake(), routing.index(), shooter.shoot()); + + bindCommands(SuperState.FEED, intake.rest(), routing.index(), shooter.feed()); + + bindCommands(SuperState.FEED_FLOW, intake.intake(), routing.index(), shooter.feed()); + + bindCommands(SuperState.SPIT, intake.spit(), routing.reverseIndex(), shooter.shoot()); + } + + public void periodic() { + Logger.recordOutput("Superstructure/Superstructure State", state); + Logger.recordOutput("Superstructure/State Timer", stateTimer.get()); + } + + /** + * @param start first state + * @param end second state + * @param trigger trigger to make it go from the first state to the second (assuming it's already + * in the first state) + */ + private void bindTransition(SuperState start, SuperState end, Trigger trigger) { + // when 1) the robot is in the start state and 2) the trigger is true, the robot changes state + // to the end state + trigger.and(start.getTrigger()).onTrue(changeStateTo(end)); + } + + /** + * @param start first state + * @param end second state + * @param trigger trigger to make it go from the first state to the second (assuming it's already + * in the first state) + * @param cmd some command to run while making the transition + */ + private void bindTransition(SuperState start, SuperState end, Trigger trigger, Command cmd) { + // when 1) the robot is in the start state and 2) the trigger is true, the robot changes state + // to the end state IN PARALLEL to running the command that got passed in + trigger.and(start.getTrigger()).onTrue(Commands.parallel(changeStateTo(end), cmd)); + } + + /** + * Runs the passed in command(s) in parallel when the superstructure is in the passed in state + * + * @param state + * @param commands + */ + private void bindCommands(SuperState state, Command... commands) { + state.getTrigger().whileTrue(Commands.parallel(commands)); + } + + // public boolean atExtension(SuperState state) { + // } + + // public boolean atExtension() { + // return atExtension(state); + // } + + private Command changeStateTo(SuperState nextState) { + return Commands.runOnce( + () -> { + System.out.println("Changing state from " + state + " to " + nextState); + stateTimer.reset(); + this.prevState = state; + state = nextState; + setSubstates(); + }) + .ignoringDisable(true) + .withName("State Change Command"); + } + + private void setSubstates() {} + + // public Command transitionAfterZeroing() { + // } + + /** + * Only for setting initial state at the beginning of auto + * + * @param state the state to set to + */ + public void resetStateForAuto(SuperState nextState) { + System.out.println("Resetting state from " + state + " to " + nextState + " for auto."); + stateTimer.reset(); + this.prevState = state; + state = nextState; + setSubstates(); + } + + public static SuperState getState() { + return state; + } + + public boolean stateIsIdle() { + return getState() == SuperState.IDLE; + } +} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..80f7af6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +// TODO +public class IntakeSubsystem extends SubsystemBase { + public Command intake() { + // TODO + return idle(); + } + + // Can't call it idle bc idle is smthing else + public Command rest() { + // TODO + return idle(); + } + + public Command spit() { + // TODO + return idle(); + } +} diff --git a/src/main/java/frc/robot/subsystems/RoutingSubsystem.java b/src/main/java/frc/robot/subsystems/RoutingSubsystem.java new file mode 100644 index 0000000..8bd19e0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/RoutingSubsystem.java @@ -0,0 +1,36 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +// TODO!!! +public class RoutingSubsystem extends SubsystemBase { + + // TODO: BASE ON ACTUAL SENSOR READINGS + private boolean isFull; + private boolean isEmpty; + + public boolean isFull() { + return isFull; + } + + public boolean isEmpty() { + return isEmpty; + } + + // TODO!! + public Command index() { + return this.run(() -> {}); + } + + // Can't call it idle bc idle is smthing else + public Command rest() { + // TODO + return idle(); + } + + public Command reverseIndex() { + // TODO + return this.run(() -> {}); + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..d3fad56 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ShooterSubsystem extends SubsystemBase { + public Command shoot() { + return this.idle(); + } + + public Command feed() { + return this.idle(); + } + + public Command rest() { + return this.idle(); + } +} From bfeb2a7fc311a31207535fdebb0c44adbdf84564 Mon Sep 17 00:00:00 2001 From: SCool62 <79726681+SCool62@users.noreply.github.com> Date: Wed, 14 Jan 2026 21:14:36 -0800 Subject: [PATCH 04/32] Cherry pick the merge --- src/main/java/frc/robot/Autos.java | 12 ++-- src/main/java/frc/robot/Robot.java | 10 ++- src/main/java/frc/robot/Superstructure.java | 62 ++++++++++++++++--- .../frc/robot/subsystems/IntakeSubsystem.java | 23 +++++++ .../robot/subsystems/RoutingSubsystem.java | 36 +++++++++++ .../robot/subsystems/ShooterSubsystem.java | 18 ++++++ 6 files changed, 146 insertions(+), 15 deletions(-) create mode 100644 src/main/java/frc/robot/subsystems/IntakeSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/RoutingSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/ShooterSubsystem.java diff --git a/src/main/java/frc/robot/Autos.java b/src/main/java/frc/robot/Autos.java index d7e1e8f..ebad639 100644 --- a/src/main/java/frc/robot/Autos.java +++ b/src/main/java/frc/robot/Autos.java @@ -25,7 +25,8 @@ public class Autos { // mehhhhhhh private static boolean autoPreScore; private static boolean autoScore; - private static boolean autoIntakeCoral; + private static boolean autoIntake; + private static boolean autoFeed; // private static boolean autoIntakeAlgae; @@ -37,9 +38,12 @@ public class Autos { public static Trigger autoScoreReq = new Trigger(() -> autoScore).and(DriverStation::isAutonomous); - @AutoLogOutput(key = "Superstructure/Auto Coral Intake Request") - public static Trigger autoIntakeCoralReq = - new Trigger(() -> autoIntakeCoral).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; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 42760be..890c029 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -24,6 +24,9 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.RoutingSubsystem; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.led.LEDIOReal; import frc.robot.subsystems.led.LEDSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; @@ -85,6 +88,10 @@ public enum RobotType { private final SwerveSubsystem swerve = new SwerveSubsystem(canivore); private final LEDSubsystem leds; + private final RoutingSubsystem routing = new RoutingSubsystem(); + private final IntakeSubsystem intake = new IntakeSubsystem(); + private final ShooterSubsystem shooter = new ShooterSubsystem(); + private final CommandXboxControllerSubsystem driver = new CommandXboxControllerSubsystem(0); private final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1); @@ -97,7 +104,8 @@ public enum RobotType { @AutoLogOutput(key = "Robot/Zeroing Request") private Trigger zeroingReq = driver.b(); - private final Superstructure superstructure = new Superstructure(swerve, driver, operator); + private final Superstructure superstructure = + new Superstructure(swerve, routing, intake, shooter, driver, operator); private final Autos autos; private Optional lastAlliance = Optional.empty(); diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index d4caa25..1cb479d 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -9,6 +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.IntakeSubsystem; +import frc.robot.subsystems.RoutingSubsystem; +import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.utils.CommandXboxControllerSubsystem; import org.littletonrobotics.junction.AutoLogOutput; @@ -16,12 +19,16 @@ public class Superstructure { - /** - * We should have a state for every single "pose" the robot will hit. See this document for - * screenshots of the robot in each state. There are also named positions in cad for each state. - */ + /** We should have a state for every single action the robot will perform. */ public enum SuperState { - IDLE(); + IDLE, + INTAKE, + READY, + FEED, + FEED_FLOW, + SCORE, + SCORE_FLOW, + SPIT; public final Trigger trigger; private SuperState() { @@ -41,15 +48,33 @@ public Trigger getTrigger() { private Timer stateTimer = new Timer(); private final SwerveSubsystem swerve; + private final RoutingSubsystem routing; + private final IntakeSubsystem intake; + private final ShooterSubsystem shooter; private final CommandXboxControllerSubsystem driver; private final CommandXboxControllerSubsystem operator; // Declare triggers - @AutoLogOutput(key = "Superstructure/Pre Score Request") - public Trigger preScoreReq; - @AutoLogOutput(key = "Superstructure/Score Request") - public Trigger scoreReq; + private Trigger scoreReq; + + @AutoLogOutput(key = "Superstructure/Intake Request") + private Trigger intakeReq; + + @AutoLogOutput(key = "Superstructure/Feed Request") + private Trigger feedReq; + + @AutoLogOutput(key = "Superstructre/Flowstate Request") + private Trigger flowReq; + + @AutoLogOutput(key = "Superstructre/Anti Jam Req") + private Trigger antiJamReq; + + @AutoLogOutput(key = "Superstructure/Is Full") + private Trigger isFull; + + @AutoLogOutput(key = "Superstructure/Is Empty") + private Trigger isEmpty; // @AutoLogOutput(key = "Superstructure/At Extension?") // public Trigger atExtensionTrigger = new Trigger(this::atExtension).or(Robot::isSimulation); @@ -57,14 +82,21 @@ public Trigger getTrigger() { /** Creates a new Superstructure. */ public Superstructure( SwerveSubsystem swerve, + RoutingSubsystem routing, + IntakeSubsystem intake, + ShooterSubsystem shooter, CommandXboxControllerSubsystem driver, CommandXboxControllerSubsystem operator) { this.swerve = swerve; + this.routing = routing; + this.intake = intake; + this.shooter = shooter; this.driver = driver; this.operator = operator; addTriggers(); addTransitions(); + addCommands(); stateTimer.start(); } @@ -131,7 +163,7 @@ private void addTransitions() { // Maybe should be a transition from idle to flow as well? In case robot doesn't already have // a fuel } - + // Transition from any state to SPIT for anti jamming antiJamReq.onTrue(changeStateTo(SuperState.SPIT)); @@ -194,6 +226,16 @@ private void bindTransition(SuperState start, SuperState end, Trigger trigger, C trigger.and(start.getTrigger()).onTrue(Commands.parallel(changeStateTo(end), cmd)); } + /** + * Runs the passed in command(s) in parallel when the superstructure is in the passed in state + * + * @param state + * @param commands + */ + private void bindCommands(SuperState state, Command... commands) { + state.getTrigger().whileTrue(Commands.parallel(commands)); + } + // public boolean atExtension(SuperState state) { // } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java new file mode 100644 index 0000000..80f7af6 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -0,0 +1,23 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +// TODO +public class IntakeSubsystem extends SubsystemBase { + public Command intake() { + // TODO + return idle(); + } + + // Can't call it idle bc idle is smthing else + public Command rest() { + // TODO + return idle(); + } + + public Command spit() { + // TODO + return idle(); + } +} diff --git a/src/main/java/frc/robot/subsystems/RoutingSubsystem.java b/src/main/java/frc/robot/subsystems/RoutingSubsystem.java new file mode 100644 index 0000000..8bd19e0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/RoutingSubsystem.java @@ -0,0 +1,36 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +// TODO!!! +public class RoutingSubsystem extends SubsystemBase { + + // TODO: BASE ON ACTUAL SENSOR READINGS + private boolean isFull; + private boolean isEmpty; + + public boolean isFull() { + return isFull; + } + + public boolean isEmpty() { + return isEmpty; + } + + // TODO!! + public Command index() { + return this.run(() -> {}); + } + + // Can't call it idle bc idle is smthing else + public Command rest() { + // TODO + return idle(); + } + + public Command reverseIndex() { + // TODO + return this.run(() -> {}); + } +} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java new file mode 100644 index 0000000..d3fad56 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ShooterSubsystem extends SubsystemBase { + public Command shoot() { + return this.idle(); + } + + public Command feed() { + return this.idle(); + } + + public Command rest() { + return this.idle(); + } +} From 2f90f5bcd6a920a521e2358f7edfe610d9a244d1 Mon Sep 17 00:00:00 2001 From: SCool62 <79726681+SCool62@users.noreply.github.com> Date: Thu, 15 Jan 2026 16:55:14 +0000 Subject: [PATCH 05/32] run spotless --- src/main/java/frc/robot/Superstructure.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 1cb479d..f5da6c0 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -261,7 +261,6 @@ private void setSubstates() {} // public Command transitionAfterZeroing() { // } - /** * Only for setting initial state at the beginning of auto * From f2b8d190f5aa6f27124329a5424f93199462d10c Mon Sep 17 00:00:00 2001 From: SCool62 Date: Fri, 16 Jan 2026 16:15:18 -0800 Subject: [PATCH 06/32] Delete placeholder subsystems --- .../frc/robot/subsystems/IntakeSubsystem.java | 23 ------------ .../robot/subsystems/RoutingSubsystem.java | 36 ------------------- .../robot/subsystems/ShooterSubsystem.java | 18 ---------- 3 files changed, 77 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/IntakeSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/RoutingSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/ShooterSubsystem.java diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java deleted file mode 100644 index 80f7af6..0000000 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ /dev/null @@ -1,23 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -// TODO -public class IntakeSubsystem extends SubsystemBase { - public Command intake() { - // TODO - return idle(); - } - - // Can't call it idle bc idle is smthing else - public Command rest() { - // TODO - return idle(); - } - - public Command spit() { - // TODO - return idle(); - } -} diff --git a/src/main/java/frc/robot/subsystems/RoutingSubsystem.java b/src/main/java/frc/robot/subsystems/RoutingSubsystem.java deleted file mode 100644 index 8bd19e0..0000000 --- a/src/main/java/frc/robot/subsystems/RoutingSubsystem.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -// TODO!!! -public class RoutingSubsystem extends SubsystemBase { - - // TODO: BASE ON ACTUAL SENSOR READINGS - private boolean isFull; - private boolean isEmpty; - - public boolean isFull() { - return isFull; - } - - public boolean isEmpty() { - return isEmpty; - } - - // TODO!! - public Command index() { - return this.run(() -> {}); - } - - // Can't call it idle bc idle is smthing else - public Command rest() { - // TODO - return idle(); - } - - public Command reverseIndex() { - // TODO - return this.run(() -> {}); - } -} diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java deleted file mode 100644 index d3fad56..0000000 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ /dev/null @@ -1,18 +0,0 @@ -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class ShooterSubsystem extends SubsystemBase { - public Command shoot() { - return this.idle(); - } - - public Command feed() { - return this.idle(); - } - - public Command rest() { - return this.idle(); - } -} From a1328e6c6f54f46eb99ed394cbf97f584fbc716b Mon Sep 17 00:00:00 2001 From: SCool62 Date: Fri, 16 Jan 2026 16:29:56 -0800 Subject: [PATCH 07/32] Initial integration --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/Superstructure.java | 41 ++++++++++--------- .../robot/subsystems/hood/HoodSubsystem.java | 32 ++++++++++++++- .../subsystems/indexer/IndexerSubsystem.java | 10 ++++- .../java/frc/robot/utils/autoaim/AutoAim.java | 7 ++++ .../utils/autoaim/InterpolatingShotTree.java | 2 +- 6 files changed, 71 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 8eb1fdd..4013e07 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -130,7 +130,7 @@ public enum RobotType { private Trigger zeroingReq = driver.b(); private final Superstructure superstructure = - new Superstructure(swerve, routing, intake, shooter, driver, operator); + new Superstructure(swerve, indexer, intake, hood, driver, operator); private final Autos autos; private Optional lastAlliance = Optional.empty(); diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index f5da6c0..271cdc9 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.IntakeSubsystem; -import frc.robot.subsystems.RoutingSubsystem; -import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.hood.HoodSubsystem; +import frc.robot.subsystems.indexer.IndexerSubsystem; +import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.utils.CommandXboxControllerSubsystem; import org.littletonrobotics.junction.AutoLogOutput; @@ -48,9 +48,9 @@ public Trigger getTrigger() { private Timer stateTimer = new Timer(); private final SwerveSubsystem swerve; - private final RoutingSubsystem routing; + private final IndexerSubsystem indexer; private final IntakeSubsystem intake; - private final ShooterSubsystem shooter; + private final HoodSubsystem shooter; // TODO: COMBINE WITH SHOOTER FLYWHEEL WHEN ITS MERGED private final CommandXboxControllerSubsystem driver; private final CommandXboxControllerSubsystem operator; @@ -82,13 +82,13 @@ public Trigger getTrigger() { /** Creates a new Superstructure. */ public Superstructure( SwerveSubsystem swerve, - RoutingSubsystem routing, + IndexerSubsystem indexer, IntakeSubsystem intake, - ShooterSubsystem shooter, + HoodSubsystem shooter, CommandXboxControllerSubsystem driver, CommandXboxControllerSubsystem operator) { this.swerve = swerve; - this.routing = routing; + this.indexer = indexer; this.intake = intake; this.shooter = shooter; this.driver = driver; @@ -117,9 +117,9 @@ private void addTriggers() { antiJamReq = driver.a().or(operator.a()); - isFull = new Trigger(routing::isFull); + isFull = new Trigger(indexer::isFull); - isEmpty = new Trigger(routing::isEmpty); + isEmpty = new Trigger(indexer::isEmpty); } private void addTransitions() { @@ -174,26 +174,29 @@ private void addCommands() { bindCommands( SuperState.IDLE, intake.rest(), - routing.rest(), - shooter.rest()); // Maybe the routing should be indexing? + indexer.rest(), + shooter.rest()); // Maybe the indexer should be indexing? - bindCommands(SuperState.INTAKE, intake.intake(), routing.index(), shooter.rest()); + bindCommands(SuperState.INTAKE, intake.intake(), indexer.index(), shooter.rest()); bindCommands( SuperState.READY, intake.rest(), - routing.index(), + indexer.index(), shooter.rest()); // Maybe index at slower speed? - bindCommands(SuperState.SCORE, intake.rest(), routing.index(), shooter.shoot()); + bindCommands( + SuperState.SCORE, intake.rest(), indexer.indexToShoot(), shooter.shoot(swerve::getPose)); - bindCommands(SuperState.SCORE_FLOW, intake.intake(), routing.index(), shooter.shoot()); + bindCommands( + SuperState.SCORE_FLOW, intake.intake(), indexer.index(), shooter.shoot(swerve::getPose)); - bindCommands(SuperState.FEED, intake.rest(), routing.index(), shooter.feed()); + bindCommands(SuperState.FEED, intake.rest(), indexer.index(), shooter.feed(swerve::getPose)); - bindCommands(SuperState.FEED_FLOW, intake.intake(), routing.index(), shooter.feed()); + bindCommands( + SuperState.FEED_FLOW, intake.intake(), indexer.index(), shooter.feed(swerve::getPose)); - bindCommands(SuperState.SPIT, intake.spit(), routing.reverseIndex(), shooter.shoot()); + bindCommands(SuperState.SPIT, intake.outake(), indexer.outtake(), shooter.spit()); } public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/hood/HoodSubsystem.java b/src/main/java/frc/robot/subsystems/hood/HoodSubsystem.java index 0fa5323..7dea659 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodSubsystem.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodSubsystem.java @@ -4,10 +4,12 @@ package frc.robot.subsystems.hood; -import com.google.common.base.Supplier; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.utils.autoaim.AutoAim; +import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; public class HoodSubsystem extends SubsystemBase { @@ -30,4 +32,32 @@ public void periodic() { hoodIO.updateInputs(hoodInputs); Logger.processInputs("Shooter/Hood", hoodInputs); } + + public Command shoot(Supplier robotPoseSupplier) { + return this.run( + () -> { + hoodIO.setHoodPosition( + AutoAim.HUB_SHOT_TREE + .get(AutoAim.distanceToHub(robotPoseSupplier.get())) + .hoodRotation()); + // TODO: FLYWHEEL WHEN MERGED + }); + } + + public Command feed(Supplier robotPoseSupplier) { + return shoot(robotPoseSupplier); // TODO + } + + public Command rest() { + return this.run( + () -> { + hoodIO.setHoodPosition(Rotation2d.kZero); // TODO: TUNE TUCKED POSITION IF NEEDED + // TODO: FLYWHEEL + }); + } + + public Command spit() { + return this.run( + () -> hoodIO.setHoodPosition(Rotation2d.kZero)); // TODO: FLYWHEEL AND TUNE HOOD POS + } } diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index 0304518..4053296 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -55,10 +55,11 @@ public Command index() { }); } - public Command score() { + public Command indexToShoot() { return this.run( () -> { rollerIO.setRollerVoltage(10); + // TODO: KICKER WHEEL }); } @@ -69,6 +70,13 @@ public Command outtake() { }); } + public Command rest() { + return this.run( + () -> { + rollerIO.setRollerVoltage(0.0); + }); + } + public static TalonFXConfiguration getIndexerConfigs() { TalonFXConfiguration config = new TalonFXConfiguration(); diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 732d6e4..fa5e44e 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -1,5 +1,8 @@ package frc.robot.utils.autoaim; +import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.utils.FieldUtils; + public class AutoAim { public static final InterpolatingShotTree HUB_SHOT_TREE = new InterpolatingShotTree(); @@ -11,4 +14,8 @@ public class AutoAim { } // TODO: SOTM + + public static double distanceToHub(Pose2d pose) { + return pose.getTranslation().getDistance(FieldUtils.getCurrentHubTranslation()); + } } diff --git a/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java b/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java index 00468d5..2f71925 100644 --- a/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java +++ b/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java @@ -5,7 +5,7 @@ import java.util.TreeMap; public class InterpolatingShotTree { - private record ShotData( + public record ShotData( Rotation2d hoodRotation, double flywheelVelocityRotPerSec, double flightTimeSec) {} private final TreeMap map = new TreeMap<>(); From 966d6aeb1bb5ca045e168b4e7ba46a32a2957a32 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Fri, 16 Jan 2026 16:41:46 -0800 Subject: [PATCH 08/32] More fleshed out feeding --- src/main/java/frc/robot/Superstructure.java | 6 +++-- .../robot/subsystems/hood/HoodSubsystem.java | 15 ++++++++--- src/main/java/frc/robot/utils/FieldUtils.java | 26 +++++++++++++++++++ .../java/frc/robot/utils/autoaim/AutoAim.java | 9 ++++++- 4 files changed, 49 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 271cdc9..c01e3d3 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -14,6 +14,8 @@ import frc.robot.subsystems.intake.IntakeSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.utils.CommandXboxControllerSubsystem; +import frc.robot.utils.FieldUtils.FeedTargets; + import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -191,10 +193,10 @@ private void addCommands() { bindCommands( SuperState.SCORE_FLOW, intake.intake(), indexer.index(), shooter.shoot(swerve::getPose)); - bindCommands(SuperState.FEED, intake.rest(), indexer.index(), shooter.feed(swerve::getPose)); + bindCommands(SuperState.FEED, intake.rest(), indexer.index(), shooter.feed(swerve::getPose, () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); // TODO: ADD SOME SELECTION LOGIC bindCommands( - SuperState.FEED_FLOW, intake.intake(), indexer.index(), shooter.feed(swerve::getPose)); + SuperState.FEED_FLOW, intake.intake(), indexer.index(), shooter.feed(swerve::getPose, () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); bindCommands(SuperState.SPIT, intake.outake(), indexer.outtake(), shooter.spit()); } diff --git a/src/main/java/frc/robot/subsystems/hood/HoodSubsystem.java b/src/main/java/frc/robot/subsystems/hood/HoodSubsystem.java index 7dea659..846ff8b 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodSubsystem.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodSubsystem.java @@ -9,6 +9,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.utils.autoaim.AutoAim; +import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; + import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -36,16 +38,21 @@ public void periodic() { public Command shoot(Supplier robotPoseSupplier) { return this.run( () -> { + ShotData shotData = AutoAim.HUB_SHOT_TREE + .get(AutoAim.distanceToHub(robotPoseSupplier.get())); hoodIO.setHoodPosition( - AutoAim.HUB_SHOT_TREE - .get(AutoAim.distanceToHub(robotPoseSupplier.get())) + shotData .hoodRotation()); // TODO: FLYWHEEL WHEN MERGED }); } - public Command feed(Supplier robotPoseSupplier) { - return shoot(robotPoseSupplier); // TODO + public Command feed(Supplier robotPoseSupplier, Supplier feedTarget) { + return this.run(() -> { + ShotData shotData = AutoAim.FEED_SHOT_TREE.get(robotPoseSupplier.get().getTranslation().getDistance(feedTarget.get().getTranslation())); + hoodIO.setHoodPosition(shotData.hoodRotation()); + // TODO: FLYWHEEL + }); } public Command rest() { diff --git a/src/main/java/frc/robot/utils/FieldUtils.java b/src/main/java/frc/robot/utils/FieldUtils.java index a514981..ad26c4c 100644 --- a/src/main/java/frc/robot/utils/FieldUtils.java +++ b/src/main/java/frc/robot/utils/FieldUtils.java @@ -27,4 +27,30 @@ public static Pose2d getCurrentHubPose() { ? BLUE_HUB_POS : RED_HUB_POS; } + + public enum FeedTargets { + BLUE_BACK_RIGHT(new Translation2d(0.6643, 0.75)), // Eyeballed in Choreo + BLUE_BACK_LEFT(new Translation2d(0.75, 7.367)), + RED_BACK_RIGHT(ChoreoAllianceFlipUtil.flip(BLUE_BACK_RIGHT.getPose())), + RED_BACK_LEFT(ChoreoAllianceFlipUtil.flip(BLUE_BACK_LEFT.getPose())) + ; + + private Pose2d targetPose; + + private FeedTargets(Pose2d pose) { + this.targetPose = pose; + } + + private FeedTargets(Translation2d translation) { + this.targetPose = new Pose2d(translation, Rotation2d.kZero); + } + + public Pose2d getPose() { + return targetPose; + } + + public Translation2d getTranslation() { + return targetPose.getTranslation(); + } + } } diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index fa5e44e..1cf8522 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -9,10 +9,17 @@ public class AutoAim { // If we need other shot trees (i.e. for feeding) we can put them here - static { + static { // For hub shot tree // TODO: ADD SHOTS TO HUB SHOT HERE } + // Ig we'll see if we need more than 1 feed shot tree + public static final InterpolatingShotTree FEED_SHOT_TREE = new InterpolatingShotTree(); + + static { // For feed shot tree + // TODO: POPULATE + } + // TODO: SOTM public static double distanceToHub(Pose2d pose) { From af07565c9401264695b4df54ad315d334f9a4362 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Fri, 16 Jan 2026 18:37:13 -0800 Subject: [PATCH 09/32] Make stuff somewhat simmable --- src/main/java/frc/robot/Robot.java | 19 +++++++++++++++---- .../subsystems/indexer/IndexerSubsystem.java | 4 ++-- .../subsystems/intake/IntakeSubsystem.java | 5 +++-- 3 files changed, 20 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4013e07..a7b5123 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -98,12 +98,13 @@ public enum RobotType { new IndexerSubsystem( canivore, (ROBOT_TYPE == RobotType.REAL) - ? new RollerIOReal(0, IndexerSubsystem.getIndexerConfigs()) + ? new RollerIOReal(10, IndexerSubsystem.getIndexerConfigs()) : new RollerIOCTRESim( - 0, + 11, IndexerSubsystem.getIndexerConfigs(), new DCMotorSim( - LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX44Foc(1), 1, 1), + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), 0.003, IndexerSubsystem.GEAR_RATIO), DCMotor.getKrakenX44Foc(1)), MotorType.KrakenX44)); @@ -115,7 +116,17 @@ public enum RobotType { ? new HoodIO(HoodIO.getHoodConfiguration(), canivore) : new HoodIOSim(canivore)); private final IntakeSubsystem intake = - new IntakeSubsystem(new RollerIOReal(0, IntakeSubsystem.getIntakeIOConfig())); + new IntakeSubsystem( + (ROBOT_TYPE == RobotType.REAL) + ? new RollerIOReal(13, IntakeSubsystem.getIntakeIOConfig()) + : new RollerIOCTRESim( + 13, + IntakeSubsystem.getIntakeIOConfig(), + new DCMotorSim( + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX44Foc(1), 0.001, IntakeSubsystem.GEAR_RATIO), + DCMotor.getKrakenX44Foc(1)), + MotorType.KrakenX44)); private final CommandXboxControllerSubsystem driver = new CommandXboxControllerSubsystem(0); private final CommandXboxControllerSubsystem operator = new CommandXboxControllerSubsystem(1); diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index 4053296..ccd6730 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -13,7 +13,7 @@ import org.littletonrobotics.junction.Logger; public class IndexerSubsystem extends SubsystemBase { - + public static double GEAR_RATIO = 2.0; // Add actual CanBus CANrangeIOReal firstCANRangeIO; @@ -85,7 +85,7 @@ public static TalonFXConfiguration getIndexerConfigs() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; // Converts angular motion to linear motion - config.Feedback.SensorToMechanismRatio = 1; + config.Feedback.SensorToMechanismRatio = GEAR_RATIO; config.Slot0.kS = 0; config.Slot0.kG = 0; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index 5ffc088..c99779b 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -11,10 +11,10 @@ import org.littletonrobotics.junction.Logger; public class IntakeSubsystem extends SubsystemBase { + public static double GEAR_RATIO = 2.0; private RollerIOReal io; private RollerIOInputsAutoLogged inputs = new RollerIOInputsAutoLogged(); - private static TalonFX motor = new TalonFX(10, "*"); public IntakeSubsystem(RollerIOReal io) { this.io = io; @@ -52,6 +52,8 @@ public static TalonFXConfiguration getIntakeIOConfig() { config.MotorOutput.NeutralMode = NeutralModeValue.Coast; config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + config.Feedback.SensorToMechanismRatio = GEAR_RATIO; + config.Slot0.kS = 0.24; config.Slot0.kV = 0.6; config.Slot0.kP = 110.0; @@ -62,7 +64,6 @@ public static TalonFXConfiguration getIntakeIOConfig() { config.CurrentLimits.SupplyCurrentLimit = 60.0; config.CurrentLimits.SupplyCurrentLimitEnable = true; - motor.getConfigurator().apply(config); return config; } } From 3cf7bc0a7937fa0ef9d9527871de4b5fcbf8ffe4 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Fri, 16 Jan 2026 18:39:24 -0800 Subject: [PATCH 10/32] Sim launches now --- src/main/java/frc/robot/Superstructure.java | 14 ++++++++--- .../frc/robot/subsystems/hood/HoodIO.java | 19 +++++++++----- .../robot/subsystems/hood/HoodSubsystem.java | 25 +++++++++++-------- .../subsystems/intake/IntakeSubsystem.java | 1 - src/main/java/frc/robot/utils/FieldUtils.java | 3 +-- 5 files changed, 39 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index c01e3d3..fbd3d87 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -15,7 +15,6 @@ import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.utils.CommandXboxControllerSubsystem; import frc.robot.utils.FieldUtils.FeedTargets; - import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -193,10 +192,19 @@ private void addCommands() { bindCommands( SuperState.SCORE_FLOW, intake.intake(), indexer.index(), shooter.shoot(swerve::getPose)); - bindCommands(SuperState.FEED, intake.rest(), indexer.index(), shooter.feed(swerve::getPose, () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); // TODO: ADD SOME SELECTION LOGIC + bindCommands( + SuperState.FEED, + intake.rest(), + indexer.index(), + shooter.feed( + swerve::getPose, + () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); // TODO: ADD SOME SELECTION LOGIC bindCommands( - SuperState.FEED_FLOW, intake.intake(), indexer.index(), shooter.feed(swerve::getPose, () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); + SuperState.FEED_FLOW, + intake.intake(), + indexer.index(), + shooter.feed(swerve::getPose, () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); bindCommands(SuperState.SPIT, intake.outake(), indexer.outtake(), shooter.spit()); } diff --git a/src/main/java/frc/robot/subsystems/hood/HoodIO.java b/src/main/java/frc/robot/subsystems/hood/HoodIO.java index 5302c7f..c350cf7 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodIO.java @@ -35,12 +35,12 @@ public static class HoodIOInputs { protected TalonFX hoodMotor; - private final BaseStatusSignal hoodPositionRotations = hoodMotor.getPosition(); - private final BaseStatusSignal hoodAngularVelocity = hoodMotor.getVelocity(); - private final StatusSignal hoodVoltage = hoodMotor.getMotorVoltage(); - private final StatusSignal hoodStatorCurrent = hoodMotor.getStatorCurrent(); - private final StatusSignal hoodSupplyCurrent = hoodMotor.getSupplyCurrent(); - private final StatusSignal hoodTemp = hoodMotor.getDeviceTemp(); + private final BaseStatusSignal hoodPositionRotations; + private final BaseStatusSignal hoodAngularVelocity; + private final StatusSignal hoodVoltage; + private final StatusSignal hoodStatorCurrent; + private final StatusSignal hoodSupplyCurrent; + private final StatusSignal hoodTemp; private VoltageOut voltageOut = new VoltageOut(0.0).withEnableFOC(true); private PositionVoltage positionVoltage = new PositionVoltage(0.0).withEnableFOC(true); private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); @@ -49,6 +49,13 @@ public HoodIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) { hoodMotor = new TalonFX(1, canbus); // TODO motorid hoodMotor.getConfigurator().apply(HoodIO.getHoodConfiguration()); + hoodPositionRotations = hoodMotor.getPosition(); + hoodAngularVelocity = hoodMotor.getVelocity(); + hoodVoltage = hoodMotor.getMotorVoltage(); + hoodStatorCurrent = hoodMotor.getStatorCurrent(); + hoodSupplyCurrent = hoodMotor.getSupplyCurrent(); + hoodTemp = hoodMotor.getDeviceTemp(); + BaseStatusSignal.setUpdateFrequencyForAll( 50.0, hoodPositionRotations, diff --git a/src/main/java/frc/robot/subsystems/hood/HoodSubsystem.java b/src/main/java/frc/robot/subsystems/hood/HoodSubsystem.java index 846ff8b..57cddb2 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodSubsystem.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodSubsystem.java @@ -10,7 +10,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; - import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -38,21 +37,25 @@ public void periodic() { public Command shoot(Supplier robotPoseSupplier) { return this.run( () -> { - ShotData shotData = AutoAim.HUB_SHOT_TREE - .get(AutoAim.distanceToHub(robotPoseSupplier.get())); - hoodIO.setHoodPosition( - shotData - .hoodRotation()); + ShotData shotData = + AutoAim.HUB_SHOT_TREE.get(AutoAim.distanceToHub(robotPoseSupplier.get())); + hoodIO.setHoodPosition(shotData.hoodRotation()); // TODO: FLYWHEEL WHEN MERGED }); } public Command feed(Supplier robotPoseSupplier, Supplier feedTarget) { - return this.run(() -> { - ShotData shotData = AutoAim.FEED_SHOT_TREE.get(robotPoseSupplier.get().getTranslation().getDistance(feedTarget.get().getTranslation())); - hoodIO.setHoodPosition(shotData.hoodRotation()); - // TODO: FLYWHEEL - }); + return this.run( + () -> { + ShotData shotData = + AutoAim.FEED_SHOT_TREE.get( + robotPoseSupplier + .get() + .getTranslation() + .getDistance(feedTarget.get().getTranslation())); + hoodIO.setHoodPosition(shotData.hoodRotation()); + // TODO: FLYWHEEL + }); } public Command rest() { diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java index c99779b..530291b 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java @@ -1,7 +1,6 @@ package frc.robot.subsystems.intake; import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/frc/robot/utils/FieldUtils.java b/src/main/java/frc/robot/utils/FieldUtils.java index ad26c4c..d5db691 100644 --- a/src/main/java/frc/robot/utils/FieldUtils.java +++ b/src/main/java/frc/robot/utils/FieldUtils.java @@ -32,8 +32,7 @@ public enum FeedTargets { BLUE_BACK_RIGHT(new Translation2d(0.6643, 0.75)), // Eyeballed in Choreo BLUE_BACK_LEFT(new Translation2d(0.75, 7.367)), RED_BACK_RIGHT(ChoreoAllianceFlipUtil.flip(BLUE_BACK_RIGHT.getPose())), - RED_BACK_LEFT(ChoreoAllianceFlipUtil.flip(BLUE_BACK_LEFT.getPose())) - ; + RED_BACK_LEFT(ChoreoAllianceFlipUtil.flip(BLUE_BACK_LEFT.getPose())); private Pose2d targetPose; From e331e90b6e7624a49fd0bed5cc3c9f5d98fa3662 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Fri, 16 Jan 2026 19:00:46 -0800 Subject: [PATCH 11/32] State transitions work in sim --- src/main/java/frc/robot/Superstructure.java | 4 ++-- src/main/java/frc/robot/utils/autoaim/AutoAim.java | 6 ++++++ 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index fbd3d87..66c86ff 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -65,10 +65,10 @@ public Trigger getTrigger() { @AutoLogOutput(key = "Superstructure/Feed Request") private Trigger feedReq; - @AutoLogOutput(key = "Superstructre/Flowstate Request") + @AutoLogOutput(key = "Superstructure/Flowstate Request") private Trigger flowReq; - @AutoLogOutput(key = "Superstructre/Anti Jam Req") + @AutoLogOutput(key = "Superstructure/Anti Jam Req") private Trigger antiJamReq; @AutoLogOutput(key = "Superstructure/Is Full") diff --git a/src/main/java/frc/robot/utils/autoaim/AutoAim.java b/src/main/java/frc/robot/utils/autoaim/AutoAim.java index 1cf8522..1426b50 100644 --- a/src/main/java/frc/robot/utils/autoaim/AutoAim.java +++ b/src/main/java/frc/robot/utils/autoaim/AutoAim.java @@ -1,7 +1,9 @@ package frc.robot.utils.autoaim; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import frc.robot.utils.FieldUtils; +import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; public class AutoAim { @@ -11,6 +13,8 @@ public class AutoAim { static { // For hub shot tree // TODO: ADD SHOTS TO HUB SHOT HERE + HUB_SHOT_TREE.put( + 1.0, new ShotData(Rotation2d.kCW_90deg, 10, 0.5)); // Placeholder to prevent crashes } // Ig we'll see if we need more than 1 feed shot tree @@ -18,6 +22,8 @@ public class AutoAim { static { // For feed shot tree // TODO: POPULATE + FEED_SHOT_TREE.put( + 1.0, new ShotData(Rotation2d.kCW_90deg, 10, 0.5)); // Placeholder to prevent crashes } // TODO: SOTM From b6a23debdcb6a6887c50faec3a4383e11044cd7b Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 17 Jan 2026 12:23:44 -0800 Subject: [PATCH 12/32] Talked to Cassie about button bindings --- src/main/java/frc/robot/Superstructure.java | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 66c86ff..b7c964f 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -77,6 +77,8 @@ public Trigger getTrigger() { @AutoLogOutput(key = "Superstructure/Is Empty") private Trigger isEmpty; + private boolean shouldFeed = false; + // @AutoLogOutput(key = "Superstructure/At Extension?") // public Trigger atExtensionTrigger = new Trigger(this::atExtension).or(Robot::isSimulation); @@ -103,16 +105,20 @@ public Superstructure( } private void addTriggers() { - // TODO: THESE BINDINGS WILL LIKELY CHANGE. SHOULD HAVE A FULL MEETING TO DISCUSS + // Toggles for feeding + operator.leftBumper().onTrue(Commands.runOnce(() -> shouldFeed = true)); + operator.rightBumper().onTrue(Commands.runOnce(() -> shouldFeed = false)); + scoreReq = driver .rightTrigger() .and(DriverStation::isTeleop) + .and(() -> shouldFeed == false) .or(Autos.autoScoreReq); // Maybe should include if its our turn? intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq); - feedReq = driver.rightBumper().and(DriverStation::isTeleop).or(Autos.autoFeedReq); + feedReq = driver.rightBumper().and(DriverStation::isTeleop).and(() -> shouldFeed == true).or(Autos.autoFeedReq); flowReq = operator.rightTrigger(); From 56d8006188dc2a5fa5576d733ed5de72f98332f7 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 17 Jan 2026 12:34:10 -0800 Subject: [PATCH 13/32] Add more transitions for flow state at cassies request --- src/main/java/frc/robot/Superstructure.java | 22 ++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index b7c964f..c7fb3f8 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -153,22 +153,30 @@ private void addTransitions() { // FEED_FLOW transitions { + bindTransition(SuperState.IDLE, SuperState.FEED_FLOW, flowReq.and(feedReq)); + bindTransition(SuperState.FEED, SuperState.FEED_FLOW, flowReq); - // No so sure about the end condition here. - bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, flowReq.negate()); + bindTransition(SuperState.FEED_FLOW, SuperState.FEED, flowReq.negate().and(feedReq)); - // Maybe should be a transition from idle to flow as well? In case robot doesn't already have - // a fuel + bindTransition(SuperState.FEED_FLOW, SuperState.READY, flowReq.negate().and(feedReq.negate()).and(isEmpty.negate())); + + // No so sure about the end condition here. + bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty).and(feedReq.negate())); } // SCORE_FLOW transitions { + bindTransition(SuperState.IDLE, SuperState.SCORE_FLOW, flowReq.and(scoreReq)); + bindTransition(SuperState.SCORE, SuperState.SCORE_FLOW, flowReq); - bindTransition(SuperState.SCORE_FLOW, SuperState.IDLE, flowReq.negate()); - // Maybe should be a transition from idle to flow as well? In case robot doesn't already have - // a fuel + bindTransition(SuperState.SCORE_FLOW, SuperState.SCORE, flowReq.negate().and(scoreReq)); + + bindTransition(SuperState.SCORE_FLOW, SuperState.READY, flowReq.negate().and(scoreReq.negate()).and(isEmpty.negate())); + + // No so sure about the end condition here. + bindTransition(SuperState.SCORE_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty).and(scoreReq.negate())); } // Transition from any state to SPIT for anti jamming From 0d9371bff15d8a0b6301a5fe69edde1e74cd0611 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 17 Jan 2026 13:52:09 -0800 Subject: [PATCH 14/32] Implement commands for shooter subsystem --- src/main/java/frc/robot/Superstructure.java | 33 ++++++++++---- .../subsystems/shooter/ShooterSubsystem.java | 44 ++++++++++++++++--- 2 files changed, 64 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index c7fb3f8..2d293df 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.hood.HoodSubsystem; import frc.robot.subsystems.indexer.IndexerSubsystem; import frc.robot.subsystems.intake.IntakeSubsystem; +import frc.robot.subsystems.shooter.ShooterSubsystem; import frc.robot.subsystems.swerve.SwerveSubsystem; import frc.robot.utils.CommandXboxControllerSubsystem; import frc.robot.utils.FieldUtils.FeedTargets; @@ -51,7 +51,7 @@ public Trigger getTrigger() { private final SwerveSubsystem swerve; private final IndexerSubsystem indexer; private final IntakeSubsystem intake; - private final HoodSubsystem shooter; // TODO: COMBINE WITH SHOOTER FLYWHEEL WHEN ITS MERGED + private final ShooterSubsystem shooter; private final CommandXboxControllerSubsystem driver; private final CommandXboxControllerSubsystem operator; @@ -87,7 +87,7 @@ public Superstructure( SwerveSubsystem swerve, IndexerSubsystem indexer, IntakeSubsystem intake, - HoodSubsystem shooter, + ShooterSubsystem shooter, CommandXboxControllerSubsystem driver, CommandXboxControllerSubsystem operator) { this.swerve = swerve; @@ -118,7 +118,12 @@ private void addTriggers() { intakeReq = driver.leftTrigger().and(DriverStation::isTeleop).or(Autos.autoIntakeReq); - feedReq = driver.rightBumper().and(DriverStation::isTeleop).and(() -> shouldFeed == true).or(Autos.autoFeedReq); + feedReq = + driver + .rightBumper() + .and(DriverStation::isTeleop) + .and(() -> shouldFeed == true) + .or(Autos.autoFeedReq); flowReq = operator.rightTrigger(); @@ -159,10 +164,16 @@ private void addTransitions() { bindTransition(SuperState.FEED_FLOW, SuperState.FEED, flowReq.negate().and(feedReq)); - bindTransition(SuperState.FEED_FLOW, SuperState.READY, flowReq.negate().and(feedReq.negate()).and(isEmpty.negate())); + bindTransition( + SuperState.FEED_FLOW, + SuperState.READY, + flowReq.negate().and(feedReq.negate()).and(isEmpty.negate())); // No so sure about the end condition here. - bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty).and(feedReq.negate())); + bindTransition( + SuperState.FEED_FLOW, + SuperState.IDLE, + flowReq.negate().and(isEmpty).and(feedReq.negate())); } // SCORE_FLOW transitions @@ -173,10 +184,16 @@ private void addTransitions() { bindTransition(SuperState.SCORE_FLOW, SuperState.SCORE, flowReq.negate().and(scoreReq)); - bindTransition(SuperState.SCORE_FLOW, SuperState.READY, flowReq.negate().and(scoreReq.negate()).and(isEmpty.negate())); + bindTransition( + SuperState.SCORE_FLOW, + SuperState.READY, + flowReq.negate().and(scoreReq.negate()).and(isEmpty.negate())); // No so sure about the end condition here. - bindTransition(SuperState.SCORE_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty).and(scoreReq.negate())); + bindTransition( + SuperState.SCORE_FLOW, + SuperState.IDLE, + flowReq.negate().and(isEmpty).and(scoreReq.negate())); } // Transition from any state to SPIT for anti jamming diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index c3b121e..477ee5c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -5,12 +5,14 @@ package frc.robot.subsystems.shooter; import com.google.common.base.Supplier; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.hood.HoodIO; import frc.robot.subsystems.hood.HoodIOInputsAutoLogged; -import java.util.function.DoubleSupplier; +import frc.robot.utils.autoaim.AutoAim; +import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; import org.littletonrobotics.junction.Logger; public class ShooterSubsystem extends SubsystemBase { @@ -28,12 +30,44 @@ public ShooterSubsystem(HoodIO hoodIO, FlywheelIO flywheelIO) { this.flywheelIO = flywheelIO; } - public Command shoot(DoubleSupplier voltage) { - return this.run(() -> flywheelIO.setFlywheelVoltage(voltage.getAsDouble())); + public Command shoot(Supplier robotPoseSupplier) { + return this.run( + () -> { + ShotData shotData = + AutoAim.HUB_SHOT_TREE.get(AutoAim.distanceToHub(robotPoseSupplier.get())); + hoodIO.setHoodPosition(shotData.hoodRotation()); + flywheelIO.setFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); + }); } - public Command feed(DoubleSupplier voltage) { - return this.run(() -> flywheelIO.setFlywheelVoltage(voltage.getAsDouble())); + public Command feed(Supplier robotPoseSupplier, Supplier feedTarget) { + return this.run( + () -> { + ShotData shotData = + AutoAim.FEED_SHOT_TREE.get( + robotPoseSupplier + .get() + .getTranslation() + .getDistance(feedTarget.get().getTranslation())); + hoodIO.setHoodPosition(shotData.hoodRotation()); + flywheelIO.setFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); + }); + } + + public Command rest() { + return this.run( + () -> { + hoodIO.setHoodPosition(Rotation2d.kZero); // TODO: TUNE TUCKED POSITION IF NEEDED + flywheelIO.setFlywheelVoltage(0.0); + }); + } + + public Command spit() { + return this.run( + () -> { + hoodIO.setHoodPosition(Rotation2d.kZero); + flywheelIO.setFlywheelVelocity(20); + }); // TODO: TUNE HOOD POS AND FLYWHEEL VELOCITY } public Command setHoodPositionCommand(Supplier hoodPosition) { From 3afa80594d573d89a680e68eeb744cf498cad509 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 17 Jan 2026 14:17:44 -0800 Subject: [PATCH 15/32] Sim constants from CAD --- src/main/java/frc/robot/subsystems/hood/HoodIO.java | 2 +- src/main/java/frc/robot/subsystems/hood/HoodIOSim.java | 6 +++--- src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java | 2 ++ .../java/frc/robot/subsystems/shooter/FlywheelIOSim.java | 2 +- .../java/frc/robot/subsystems/shooter/ShooterSubsystem.java | 3 ++- 5 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/hood/HoodIO.java b/src/main/java/frc/robot/subsystems/hood/HoodIO.java index 97ba8ee..3225ef8 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodIO.java @@ -81,7 +81,7 @@ public static TalonFXConfiguration getHoodConfiguration() { config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - config.Feedback.SensorToMechanismRatio = ShooterSubsystem.GEAR_RATIO; + config.Feedback.SensorToMechanismRatio = ShooterSubsystem.HOOD_GEAR_RATIO; config.Slot0.GravityType = GravityTypeValue.Elevator_Static; diff --git a/src/main/java/frc/robot/subsystems/hood/HoodIOSim.java b/src/main/java/frc/robot/subsystems/hood/HoodIOSim.java index 83d4ff4..187dd2f 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodIOSim.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodIOSim.java @@ -18,7 +18,7 @@ public class HoodIOSim extends HoodIO { private final DCMotorSim hoodPhysicsSim = new DCMotorSim( LinearSystemId.createDCMotorSystem( - DCMotor.getKrakenX44Foc(1), 0.01, ShooterSubsystem.GEAR_RATIO), + DCMotor.getKrakenX44Foc(1), 0.01, ShooterSubsystem.HOOD_GEAR_RATIO), DCMotor.getKrakenX44Foc(1)); // will get updated when i get specs @@ -48,9 +48,9 @@ public HoodIOSim(CANBus canbus) { // rotor position stuff added later when i have access to onshape hoodMotorSim.setRawRotorPosition( - hoodPhysicsSim.getAngularPositionRad() * (ShooterSubsystem.GEAR_RATIO)); + hoodPhysicsSim.getAngularPositionRad() * (ShooterSubsystem.HOOD_GEAR_RATIO)); hoodMotorSim.setRotorVelocity( - hoodPhysicsSim.getAngularVelocityRPM() / 60.0 * ShooterSubsystem.GEAR_RATIO); + hoodPhysicsSim.getAngularVelocityRPM() / 60.0 * ShooterSubsystem.HOOD_GEAR_RATIO); }); simNotifier.startPeriodic(simLoopPeriod); } diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index ce9149c..44d60c5 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -90,6 +90,8 @@ public static TalonFXConfiguration getFlywheelConfiguration() { config.MotorOutput.NeutralMode = NeutralModeValue.Coast; config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive; + config.Feedback.SensorToMechanismRatio = ShooterSubsystem.FLYWHEEL_GEAR_RATIO; + config.Slot0.kS = 0.2; config.Slot0.kV = 0.12; config.Slot0.kP = 0.3; diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java index 777f37c..63969b5 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java @@ -24,7 +24,7 @@ public class FlywheelIOSim extends FlywheelIO { TalonFXSimState leaderFxSimState; DCMotorSim physicsSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60Foc(2), 0, 0), + LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60Foc(2), 0.0036, ShooterSubsystem.FLYWHEEL_GEAR_RATIO), DCMotor.getKrakenX60Foc(2)); private final double simLoopPeriod = 0.002; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 477ee5c..b225d98 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -19,7 +19,8 @@ public class ShooterSubsystem extends SubsystemBase { HoodIO hoodIO; HoodIOInputsAutoLogged hoodinputs = new HoodIOInputsAutoLogged(); - public static double GEAR_RATIO = 147.0 / 13.0; + public static double HOOD_GEAR_RATIO = 147.0 / 13.0; + public static double FLYWHEEL_GEAR_RATIO = 28.0 / 24.0; FlywheelIO flywheelIO; FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); From e7e0ed5e74846669074fa4a7b43681af7203131c Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 17 Jan 2026 14:19:05 -0800 Subject: [PATCH 16/32] Spotless --- src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java index 63969b5..956959f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIOSim.java @@ -24,7 +24,8 @@ public class FlywheelIOSim extends FlywheelIO { TalonFXSimState leaderFxSimState; DCMotorSim physicsSim = new DCMotorSim( - LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60Foc(2), 0.0036, ShooterSubsystem.FLYWHEEL_GEAR_RATIO), + LinearSystemId.createDCMotorSystem( + DCMotor.getKrakenX60Foc(2), 0.0036, ShooterSubsystem.FLYWHEEL_GEAR_RATIO), DCMotor.getKrakenX60Foc(2)); private final double simLoopPeriod = 0.002; From 7e78f43b0ed78d078e0a49105e42aac9e6da4bed Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 17 Jan 2026 14:54:14 -0800 Subject: [PATCH 17/32] Tune flywheel velocity pid in sim --- src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index 44d60c5..65c29fc 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -92,9 +92,9 @@ public static TalonFXConfiguration getFlywheelConfiguration() { config.Feedback.SensorToMechanismRatio = ShooterSubsystem.FLYWHEEL_GEAR_RATIO; - config.Slot0.kS = 0.2; - config.Slot0.kV = 0.12; - config.Slot0.kP = 0.3; + config.Slot0.kS = 0.0; + config.Slot0.kV = 0.15; // From sim + config.Slot0.kP = 10.0; config.Slot0.kD = 0.0; config.CurrentLimits.StatorCurrentLimit = 120.0; From 9d214c9573ebc8c7ae869265c5a67fa2bd863be6 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 17 Jan 2026 15:01:40 -0800 Subject: [PATCH 18/32] Tune hood pid in sim --- src/main/java/frc/robot/subsystems/hood/HoodIO.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/hood/HoodIO.java b/src/main/java/frc/robot/subsystems/hood/HoodIO.java index 3225ef8..fd4d080 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodIO.java @@ -49,7 +49,7 @@ public static class HoodIOInputs { private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); public HoodIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) { - hoodMotor = new TalonFX(1, canbus); // TODO motorid + hoodMotor = new TalonFX(18, canbus); // TODO motorid hoodMotor.getConfigurator().apply(HoodIO.getHoodConfiguration()); hoodPositionRotations = hoodMotor.getPosition(); @@ -85,10 +85,10 @@ public static TalonFXConfiguration getHoodConfiguration() { config.Slot0.GravityType = GravityTypeValue.Elevator_Static; - config.Slot0.kS = 0.24; - config.Slot0.kG = 0.56; - config.Slot0.kV = 0.6; - config.Slot0.kP = 110.0; + config.Slot0.kS = 0.0; + config.Slot0.kG = 0.0; + config.Slot0.kV = 1.1; + config.Slot0.kP = 5.0; config.Slot0.kD = 0.0; config.CurrentLimits.StatorCurrentLimit = 80.0; From 7811295c789e83a1fb1437ec0cd55cacb8d46935 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 17 Jan 2026 15:14:03 -0800 Subject: [PATCH 19/32] Rename hood to shooter in robot.java --- src/main/java/frc/robot/Robot.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ab6f713..b1f6c69 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -112,7 +112,7 @@ public enum RobotType { // canivore, new RollerIOReal(0, IndexerSubsystem.getIndexerConfigs())); private final LEDSubsystem leds; - private final ShooterSubsystem hood = + private final ShooterSubsystem shooter = new ShooterSubsystem( ROBOT_TYPE == RobotType.REAL ? new HoodIO(HoodIO.getHoodConfiguration(), canivore) @@ -146,7 +146,7 @@ public enum RobotType { private Trigger zeroingReq = driver.b(); private final Superstructure superstructure = - new Superstructure(swerve, indexer, intake, hood, driver, operator); + new Superstructure(swerve, indexer, intake, shooter, driver, operator); private final Autos autos; private Optional lastAlliance = Optional.empty(); From 8f035b8c770bdb394d765ca70b806917b4e7b52c Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sat, 17 Jan 2026 15:24:45 -0800 Subject: [PATCH 20/32] CAN ids per [#19](https://github.com/HighlanderRobotics/Rebuilt/pull/19) --- src/main/java/frc/robot/Robot.java | 8 ++++---- src/main/java/frc/robot/subsystems/hood/HoodIO.java | 2 +- .../java/frc/robot/subsystems/shooter/FlywheelIO.java | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b1f6c69..aa93fc9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -100,9 +100,9 @@ public enum RobotType { new IndexerSubsystem( canivore, (ROBOT_TYPE == RobotType.REAL) - ? new RollerIOReal(10, IndexerSubsystem.getIndexerConfigs()) + ? new RollerIOReal(9, IndexerSubsystem.getIndexerConfigs()) : new RollerIOCTRESim( - 11, + 9, IndexerSubsystem.getIndexerConfigs(), new DCMotorSim( LinearSystemId.createDCMotorSystem( @@ -123,9 +123,9 @@ public enum RobotType { private final IntakeSubsystem intake = new IntakeSubsystem( (ROBOT_TYPE == RobotType.REAL) - ? new RollerIOReal(13, IntakeSubsystem.getIntakeIOConfig()) + ? new RollerIOReal(8, IntakeSubsystem.getIntakeIOConfig()) : new RollerIOCTRESim( - 13, + 8, IntakeSubsystem.getIntakeIOConfig(), new DCMotorSim( LinearSystemId.createDCMotorSystem( diff --git a/src/main/java/frc/robot/subsystems/hood/HoodIO.java b/src/main/java/frc/robot/subsystems/hood/HoodIO.java index fd4d080..675b888 100644 --- a/src/main/java/frc/robot/subsystems/hood/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/hood/HoodIO.java @@ -49,7 +49,7 @@ public static class HoodIOInputs { private VelocityVoltage velocityVoltage = new VelocityVoltage(0.0).withEnableFOC(true); public HoodIO(TalonFXConfiguration talonFXConfiguration, CANBus canbus) { - hoodMotor = new TalonFX(18, canbus); // TODO motorid + hoodMotor = new TalonFX(11, canbus); hoodMotor.getConfigurator().apply(HoodIO.getHoodConfiguration()); hoodPositionRotations = hoodMotor.getPosition(); diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index 65c29fc..beddf1a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -51,8 +51,8 @@ public static class FlywheelIOInputs { // todo: tune acceleration public FlywheelIO(TalonFXConfiguration config, CANBus canbus) { - flywheelLeader = new TalonFX(10, canbus); - flywheelFollower = new TalonFX(11, canbus); + flywheelLeader = new TalonFX(12, canbus); + flywheelFollower = new TalonFX(13, canbus); flywheelLeader.getConfigurator().apply(config); flywheelFollower.getConfigurator().apply(config); From 9a58a76e4b9759834d8828c25112349a1989fdb6 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 18 Jan 2026 16:01:38 -0800 Subject: [PATCH 21/32] Cleanup --- .../java/frc/robot/subsystems/shooter/ShooterSubsystem.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 49be89e..5e69396 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -60,7 +60,7 @@ public Command shoot(Supplier robotPoseSupplier) { ShotData shotData = AutoAim.HUB_SHOT_TREE.get(AutoAim.distanceToHub(robotPoseSupplier.get())); hoodIO.setHoodPosition(shotData.hoodRotation()); - flywheelIO.setFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); + flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); }); } @@ -74,7 +74,7 @@ public Command feed(Supplier robotPoseSupplier, Supplier feedTar .getTranslation() .getDistance(feedTarget.get().getTranslation())); hoodIO.setHoodPosition(shotData.hoodRotation()); - flywheelIO.setFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); + flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); }); } @@ -90,7 +90,7 @@ public Command spit() { return this.run( () -> { hoodIO.setHoodPosition(Rotation2d.kZero); - flywheelIO.setFlywheelVelocity(20); + flywheelIO.setMotionProfiledFlywheelVelocity(20); }); // TODO: TUNE HOOD POS AND FLYWHEEL VELOCITY } From 9eccde9e0ad19c651808c9cb2368bc6b4fcca8db Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 18 Jan 2026 16:05:36 -0800 Subject: [PATCH 22/32] Integrate kicker motor --- .../java/frc/robot/subsystems/indexer/IndexerSubsystem.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index 0c0516a..d0b92c8 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -87,6 +87,7 @@ public Command index() { return this.run( () -> { indexRollerIO.setRollerVoltage(5); + kickerIO.setRollerVoltage(-5); }); } @@ -94,6 +95,7 @@ public Command indexToShoot() { return this.run( () -> { indexRollerIO.setRollerVoltage(10); + kickerIO.setRollerVoltage(5); }); } @@ -101,6 +103,7 @@ public Command outtake() { return this.run( () -> { indexRollerIO.setRollerVoltage(-5); + kickerIO.setRollerVoltage(-5); }); } @@ -108,6 +111,7 @@ public Command rest() { return this.run( () -> { indexRollerIO.setRollerVoltage(0.0); + kickerIO.setRollerVoltage(0.0); }); } From 5135ed57b54a0c3a9cd2b9ebbba0d417c8d991bf Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 18 Jan 2026 17:11:02 -0800 Subject: [PATCH 23/32] Requested small changes --- .../frc/robot/subsystems/indexer/IndexerSubsystem.java | 9 --------- src/main/java/frc/robot/subsystems/shooter/HoodIO.java | 3 +-- .../frc/robot/subsystems/shooter/ShooterSubsystem.java | 4 ++-- .../frc/robot/utils/autoaim/InterpolatingShotTree.java | 4 ++-- 4 files changed, 5 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java index d0b92c8..af761a4 100644 --- a/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java @@ -65,15 +65,6 @@ public Command stopKicker() { public Command shoot() { return this.run(() -> kickerIO.setRollerVoltage(0)); } - ; - - public boolean isFull(boolean firstBeamBreak, boolean secondBeamBreak) { - if (firstBeamBreak && secondBeamBreak) { - return true; - } else { - return false; - } - } public boolean isEmpty() { return !firstCANRangeInputs.isDetected && !secondCANRangeInputs.isDetected; diff --git a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java index 66f4759..e852e44 100644 --- a/src/main/java/frc/robot/subsystems/shooter/HoodIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/HoodIO.java @@ -14,7 +14,6 @@ import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.controls.VoltageOut; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.geometry.Rotation2d; @@ -82,7 +81,7 @@ public static TalonFXConfiguration getHoodConfiguration() { config.Feedback.SensorToMechanismRatio = ShooterSubsystem.HOOD_GEAR_RATIO; - config.Slot0.GravityType = GravityTypeValue.Elevator_Static; + // config.Slot0.GravityType = GravityTypeValue.Arm_Cosine; Potentially need, maybe not tho. config.Slot0.kS = 0.0; config.Slot0.kG = 0.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 5e69396..20e8c24 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -59,7 +59,7 @@ public Command shoot(Supplier robotPoseSupplier) { () -> { ShotData shotData = AutoAim.HUB_SHOT_TREE.get(AutoAim.distanceToHub(robotPoseSupplier.get())); - hoodIO.setHoodPosition(shotData.hoodRotation()); + hoodIO.setHoodPosition(shotData.hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); }); } @@ -73,7 +73,7 @@ public Command feed(Supplier robotPoseSupplier, Supplier feedTar .get() .getTranslation() .getDistance(feedTarget.get().getTranslation())); - hoodIO.setHoodPosition(shotData.hoodRotation()); + hoodIO.setHoodPosition(shotData.hoodAngle()); flywheelIO.setMotionProfiledFlywheelVelocity(shotData.flywheelVelocityRotPerSec()); }); } diff --git a/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java b/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java index 2f71925..deaebd9 100644 --- a/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java +++ b/src/main/java/frc/robot/utils/autoaim/InterpolatingShotTree.java @@ -6,7 +6,7 @@ public class InterpolatingShotTree { public record ShotData( - Rotation2d hoodRotation, double flywheelVelocityRotPerSec, double flightTimeSec) {} + Rotation2d hoodAngle, double flywheelVelocityRotPerSec, double flightTimeSec) {} private final TreeMap map = new TreeMap<>(); @@ -88,7 +88,7 @@ private ShotData interpolate(ShotData startValue, ShotData endValue, double t) { return new ShotData( Rotation2d.fromRadians( MathUtil.interpolate( - startValue.hoodRotation().getRadians(), endValue.hoodRotation().getRadians(), t)), + startValue.hoodAngle().getRadians(), endValue.hoodAngle().getRadians(), t)), MathUtil.interpolate( startValue.flywheelVelocityRotPerSec(), endValue.flywheelVelocityRotPerSec(), t), MathUtil.interpolate(startValue.flightTimeSec(), endValue.flightTimeSec(), t)); From d6834eb020e5fc3721682de6a74df874e948334a Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 18 Jan 2026 17:11:48 -0800 Subject: [PATCH 24/32] Remove IDLE -> FLOW transitions --- src/main/java/frc/robot/Superstructure.java | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 2d293df..6d06e3b 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -158,8 +158,6 @@ private void addTransitions() { // FEED_FLOW transitions { - bindTransition(SuperState.IDLE, SuperState.FEED_FLOW, flowReq.and(feedReq)); - bindTransition(SuperState.FEED, SuperState.FEED_FLOW, flowReq); bindTransition(SuperState.FEED_FLOW, SuperState.FEED, flowReq.negate().and(feedReq)); @@ -178,8 +176,6 @@ private void addTransitions() { // SCORE_FLOW transitions { - bindTransition(SuperState.IDLE, SuperState.SCORE_FLOW, flowReq.and(scoreReq)); - bindTransition(SuperState.SCORE, SuperState.SCORE_FLOW, flowReq); bindTransition(SuperState.SCORE_FLOW, SuperState.SCORE, flowReq.negate().and(scoreReq)); From 7c2bf0c9d36d98a802ae3be73caf9b05ce3e0ea7 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 18 Jan 2026 17:18:13 -0800 Subject: [PATCH 25/32] Add spin up state --- src/main/java/frc/robot/Superstructure.java | 13 +++++++------ .../frc/robot/subsystems/shooter/FlywheelIO.java | 16 ++++++++++++---- .../subsystems/shooter/ShooterSubsystem.java | 8 ++++++++ 3 files changed, 27 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 6d06e3b..66d61cc 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -25,6 +25,7 @@ public enum SuperState { IDLE, INTAKE, READY, + SPIN_UP, FEED, FEED_FLOW, SCORE, @@ -142,17 +143,17 @@ private void addTransitions() { bindTransition( SuperState.INTAKE, SuperState.READY, (intakeReq.negate().and(isEmpty.negate())).or(isFull)); - bindTransition(SuperState.INTAKE, SuperState.FEED, feedReq); - - bindTransition(SuperState.INTAKE, SuperState.SCORE, scoreReq); + bindTransition(SuperState.INTAKE, SuperState.SPIN_UP, feedReq.or(scoreReq)); bindTransition(SuperState.READY, SuperState.INTAKE, intakeReq.and(isFull.negate())); - bindTransition(SuperState.READY, SuperState.FEED, feedReq); + bindTransition(SuperState.READY, SuperState.SPIN_UP, scoreReq.or(feedReq)); + + bindTransition(SuperState.SPIN_UP, SuperState.SCORE, scoreReq.and(shooter::atFlywheelVelocitySetpoint)); - bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty); + bindTransition(SuperState.SPIN_UP, SuperState.FEED, feedReq.and(shooter::atFlywheelVelocitySetpoint)); - bindTransition(SuperState.READY, SuperState.SCORE, scoreReq); + bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty); bindTransition(SuperState.SCORE, SuperState.IDLE, isEmpty); diff --git a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java index ac0f845..ac355ed 100644 --- a/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/FlywheelIO.java @@ -26,13 +26,13 @@ public class FlywheelIO { @AutoLog public static class FlywheelIOInputs { - public double flywheelLeaderVelocityMetersPerSecond = 0.0; + public double flywheelLeaderVelocityRotationsPerSecond = 0.0; public double flywheelLeaderStatorCurrentAmps = 0.0; public double flywheelLeaderSupplyCurrentAmp = 0.0; public double flywheelLeaderVoltage = 0.0; public double flywheelLeaderTempC = 0.0; - public double flywheelFollowerVelocityMetersPerSecond = 0.0; + public double flywheelFollowerVelocityRotationsPerSecond = 0.0; public double flywheelFollowerStatorCurrentAmps = 0.0; public double flywheelFollowerSupplyCurrentAmp = 0.0; public double flywheelFollowerVoltage = 0.0; @@ -59,6 +59,8 @@ public static class FlywheelIOInputs { private MotionMagicVelocityVoltage motionMagicVelocityVoltage = new MotionMagicVelocityVoltage(0.0).withEnableFOC(true).withAcceleration(100); + private double velocitySetpointRotPerSec = 0.0; + // todo: tune acceleration public FlywheelIO(TalonFXConfiguration config, CANBus canbus) { @@ -126,10 +128,12 @@ public void setFlywheelVoltage(double volts) { } public void setMotionProfiledFlywheelVelocity(double flywheelVelocity) { + velocitySetpointRotPerSec = flywheelVelocity; flywheelLeader.setControl(motionMagicVelocityVoltage.withVelocity(flywheelVelocity)); } public void stop() { // thought i should add a stop command, dont think i had to though + velocitySetpointRotPerSec = 0.0; flywheelLeader.setControl(voltageOut.withOutput(0.0)); } @@ -146,16 +150,20 @@ public void updateInputs(FlywheelIOInputs inputs) { flywheelFollowerSupplyCurrent, flywheelFollowerTemp); - inputs.flywheelLeaderVelocityMetersPerSecond = flywheelLeaderVelocity.getValueAsDouble(); + inputs.flywheelLeaderVelocityRotationsPerSecond = flywheelLeaderVelocity.getValueAsDouble(); inputs.flywheelLeaderVoltage = flywheelLeaderVoltage.getValueAsDouble(); inputs.flywheelLeaderStatorCurrentAmps = flywheelLeaderStatorCurrent.getValueAsDouble(); inputs.flywheelLeaderSupplyCurrentAmp = flywheelLeaderSupplyCurrent.getValueAsDouble(); inputs.flywheelLeaderTempC = flywheelLeaderTemp.getValueAsDouble(); - inputs.flywheelFollowerVelocityMetersPerSecond = flywheelFollowerVelocity.getValueAsDouble(); + inputs.flywheelFollowerVelocityRotationsPerSecond = flywheelFollowerVelocity.getValueAsDouble(); inputs.flywheelFollowerVoltage = flywheelFollowerVoltage.getValueAsDouble(); inputs.flywheelFollowerStatorCurrentAmps = flywheelFollowerStatorCurrent.getValueAsDouble(); inputs.flywheelFollowerSupplyCurrentAmp = flywheelFollowerSupplyCurrent.getValueAsDouble(); inputs.flywheelFollowerTempC = flywheelFollowerTemp.getValueAsDouble(); } + + public double getSetpointRotPerSec() { + return velocitySetpointRotPerSec; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 20e8c24..e296a3f 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -7,6 +7,8 @@ import static edu.wpi.first.units.Units.Volts; import com.google.common.base.Supplier; + +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; @@ -27,6 +29,8 @@ public class ShooterSubsystem extends SubsystemBase { public static double FLYWHEEL_GEAR_RATIO = 28.0 / 24.0; + public static double FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND = 5.0; // TODO: TUNE + HoodIO hoodIO; HoodIOInputsAutoLogged hoodInputs = new HoodIOInputsAutoLogged(); @@ -142,4 +146,8 @@ public Command runFlywheelSysid() { flywheelSysid.dynamic(Direction.kForward), flywheelSysid.dynamic(Direction.kReverse)); } + + public boolean atFlywheelVelocitySetpoint() { + return MathUtil.isNear(flywheelInputs.flywheelLeaderVelocityRotationsPerSecond, flywheelIO.getSetpointRotPerSec(), FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND); + } } From 0f7002e9e131c5ccdb162e863735343ff99d492e Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 18 Jan 2026 17:19:03 -0800 Subject: [PATCH 26/32] Spotless --- src/main/java/frc/robot/Superstructure.java | 8 +++++--- .../frc/robot/subsystems/shooter/ShooterSubsystem.java | 6 ++++-- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 66d61cc..be454eb 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -148,10 +148,12 @@ private void addTransitions() { bindTransition(SuperState.READY, SuperState.INTAKE, intakeReq.and(isFull.negate())); bindTransition(SuperState.READY, SuperState.SPIN_UP, scoreReq.or(feedReq)); - - bindTransition(SuperState.SPIN_UP, SuperState.SCORE, scoreReq.and(shooter::atFlywheelVelocitySetpoint)); - bindTransition(SuperState.SPIN_UP, SuperState.FEED, feedReq.and(shooter::atFlywheelVelocitySetpoint)); + bindTransition( + SuperState.SPIN_UP, SuperState.SCORE, scoreReq.and(shooter::atFlywheelVelocitySetpoint)); + + bindTransition( + SuperState.SPIN_UP, SuperState.FEED, feedReq.and(shooter::atFlywheelVelocitySetpoint)); bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index e296a3f..cb502da 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -7,7 +7,6 @@ import static edu.wpi.first.units.Units.Volts; import com.google.common.base.Supplier; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -148,6 +147,9 @@ public Command runFlywheelSysid() { } public boolean atFlywheelVelocitySetpoint() { - return MathUtil.isNear(flywheelInputs.flywheelLeaderVelocityRotationsPerSecond, flywheelIO.getSetpointRotPerSec(), FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND); + return MathUtil.isNear( + flywheelInputs.flywheelLeaderVelocityRotationsPerSecond, + flywheelIO.getSetpointRotPerSec(), + FLYWHEEL_VELOCITY_TOLERANCE_ROTATIONS_PER_SECOND); } } From 9ee841396fa16766d4f499d4bedcee58837c57f8 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 18 Jan 2026 17:36:18 -0800 Subject: [PATCH 27/32] Bind commands --- src/main/java/frc/robot/Superstructure.java | 6 ++++++ .../java/frc/robot/subsystems/shooter/ShooterSubsystem.java | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index be454eb..872a196 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -216,6 +216,12 @@ private void addCommands() { indexer.index(), shooter.rest()); // Maybe index at slower speed? + bindCommands( + SuperState.SPIN_UP, + intake.rest(), + indexer.rest(), + shooter.spinUp()); + bindCommands( SuperState.SCORE, intake.rest(), indexer.indexToShoot(), shooter.shoot(swerve::getPose)); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index cb502da..326f029 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -101,6 +101,12 @@ public Command setHoodPositionCommand(Supplier hoodPosition) { return this.run(() -> hoodIO.setHoodPosition(hoodPosition.get())); } + public Command spinUp() { + return this.run(() -> { + flywheelIO.setMotionProfiledFlywheelVelocity(80); // TODO: TUNE TO BE NEAR MOST SHOTS + }); + } + @Override public void periodic() { hoodIO.updateInputs(hoodInputs); From 453cae10a7d52f3f2cb7bbcdb7d5e98a6717a9e1 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Sun, 18 Jan 2026 17:36:31 -0800 Subject: [PATCH 28/32] Spotless --- src/main/java/frc/robot/Superstructure.java | 6 +----- .../frc/robot/subsystems/shooter/ShooterSubsystem.java | 7 ++++--- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 872a196..f5dbe64 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -216,11 +216,7 @@ private void addCommands() { indexer.index(), shooter.rest()); // Maybe index at slower speed? - bindCommands( - SuperState.SPIN_UP, - intake.rest(), - indexer.rest(), - shooter.spinUp()); + bindCommands(SuperState.SPIN_UP, intake.rest(), indexer.rest(), shooter.spinUp()); bindCommands( SuperState.SCORE, intake.rest(), indexer.indexToShoot(), shooter.shoot(swerve::getPose)); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 326f029..4c5b770 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -102,9 +102,10 @@ public Command setHoodPositionCommand(Supplier hoodPosition) { } public Command spinUp() { - return this.run(() -> { - flywheelIO.setMotionProfiledFlywheelVelocity(80); // TODO: TUNE TO BE NEAR MOST SHOTS - }); + return this.run( + () -> { + flywheelIO.setMotionProfiledFlywheelVelocity(80); // TODO: TUNE TO BE NEAR MOST SHOTS + }); } @Override From e5d91be2833b2a19b93ee701e7db3cb3633dacc3 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Mon, 19 Jan 2026 08:40:36 -0800 Subject: [PATCH 29/32] More requested changes --- src/main/java/frc/robot/Superstructure.java | 27 +++++++++++-------- .../subsystems/shooter/ShooterSubsystem.java | 10 ++----- 2 files changed, 18 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index f5dbe64..2f66835 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -25,9 +25,10 @@ public enum SuperState { IDLE, INTAKE, READY, - SPIN_UP, + SPIN_UP_FEED, FEED, FEED_FLOW, + SPIN_UP_SCORE, SCORE, SCORE_FLOW, SPIT; @@ -126,7 +127,7 @@ private void addTriggers() { .and(() -> shouldFeed == true) .or(Autos.autoFeedReq); - flowReq = operator.rightTrigger(); + flowReq = driver.leftTrigger().and(driver.rightTrigger()); antiJamReq = driver.a().or(operator.a()); @@ -143,17 +144,17 @@ private void addTransitions() { bindTransition( SuperState.INTAKE, SuperState.READY, (intakeReq.negate().and(isEmpty.negate())).or(isFull)); - bindTransition(SuperState.INTAKE, SuperState.SPIN_UP, feedReq.or(scoreReq)); + bindTransition(SuperState.INTAKE, SuperState.SPIN_UP_FEED, feedReq); bindTransition(SuperState.READY, SuperState.INTAKE, intakeReq.and(isFull.negate())); - bindTransition(SuperState.READY, SuperState.SPIN_UP, scoreReq.or(feedReq)); + bindTransition(SuperState.READY, SuperState.SPIN_UP_SCORE, scoreReq); bindTransition( - SuperState.SPIN_UP, SuperState.SCORE, scoreReq.and(shooter::atFlywheelVelocitySetpoint)); + SuperState.SPIN_UP_SCORE, SuperState.SCORE, scoreReq.and(shooter::atFlywheelVelocitySetpoint)); bindTransition( - SuperState.SPIN_UP, SuperState.FEED, feedReq.and(shooter::atFlywheelVelocitySetpoint)); + SuperState.SPIN_UP_FEED, SuperState.FEED, feedReq.and(shooter::atFlywheelVelocitySetpoint)); bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty); @@ -168,13 +169,13 @@ private void addTransitions() { bindTransition( SuperState.FEED_FLOW, SuperState.READY, - flowReq.negate().and(feedReq.negate()).and(isEmpty.negate())); + flowReq.negate().and(isEmpty.negate())); // No so sure about the end condition here. bindTransition( SuperState.FEED_FLOW, SuperState.IDLE, - flowReq.negate().and(isEmpty).and(feedReq.negate())); + flowReq.negate().and(isEmpty)); } // SCORE_FLOW transitions @@ -186,13 +187,13 @@ private void addTransitions() { bindTransition( SuperState.SCORE_FLOW, SuperState.READY, - flowReq.negate().and(scoreReq.negate()).and(isEmpty.negate())); + flowReq.negate().and(isEmpty.negate())); // No so sure about the end condition here. bindTransition( SuperState.SCORE_FLOW, SuperState.IDLE, - flowReq.negate().and(isEmpty).and(scoreReq.negate())); + flowReq.negate().and(isEmpty)); } // Transition from any state to SPIT for anti jamming @@ -216,7 +217,11 @@ private void addCommands() { indexer.index(), shooter.rest()); // Maybe index at slower speed? - bindCommands(SuperState.SPIN_UP, intake.rest(), indexer.rest(), shooter.spinUp()); + bindCommands(SuperState.SPIN_UP_SCORE, intake.rest(), indexer.rest(), shooter.shoot(swerve::getPose)); + + bindCommands(SuperState.SPIN_UP_FEED, intake.rest(), indexer.rest(), shooter.feed( + swerve::getPose, + () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); // TODO: SELECTION LOGIC bindCommands( SuperState.SCORE, intake.rest(), indexer.indexToShoot(), shooter.shoot(swerve::getPose)); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 4c5b770..4141bb4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -6,7 +6,8 @@ import static edu.wpi.first.units.Units.Volts; -import com.google.common.base.Supplier; +import java.util.function.Supplier; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -101,13 +102,6 @@ public Command setHoodPositionCommand(Supplier hoodPosition) { return this.run(() -> hoodIO.setHoodPosition(hoodPosition.get())); } - public Command spinUp() { - return this.run( - () -> { - flywheelIO.setMotionProfiledFlywheelVelocity(80); // TODO: TUNE TO BE NEAR MOST SHOTS - }); - } - @Override public void periodic() { hoodIO.updateInputs(hoodInputs); From 3625d67f6fafdd7ac40c5aacbd332f5b9cf9d828 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Mon, 19 Jan 2026 08:41:54 -0800 Subject: [PATCH 30/32] Update state machine flowchart --- notes/superstructure.md | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/notes/superstructure.md b/notes/superstructure.md index b236a1a..ddecbc3 100644 --- a/notes/superstructure.md +++ b/notes/superstructure.md @@ -10,9 +10,13 @@ IDLE <--> |intakeReq + intakeEmpty| INTAKE READY <--> |intakeReq + empty| INTAKE -READY --> |feedReq| FEED +READY --> |feedReq| SPIN_UP_FEED -READY --> |scoreReq| SCORE +READY --> |scoreReq| SPIN_UP_SCORE + +SPIN_UP_FEED --> |atVelocity| FEED + +SPIN_UP_SCORE --> |atVelocity| SCORE FEED --> |flowReq| FEED_FLOW From b70a6ef95a79818a92e98c5591a5b4c4cc06bc89 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Mon, 19 Jan 2026 08:42:09 -0800 Subject: [PATCH 31/32] Spotless --- src/main/java/frc/robot/Superstructure.java | 34 ++++++++----------- .../subsystems/shooter/ShooterSubsystem.java | 3 +- 2 files changed, 16 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 2f66835..36bc3c0 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -151,7 +151,9 @@ private void addTransitions() { bindTransition(SuperState.READY, SuperState.SPIN_UP_SCORE, scoreReq); bindTransition( - SuperState.SPIN_UP_SCORE, SuperState.SCORE, scoreReq.and(shooter::atFlywheelVelocitySetpoint)); + SuperState.SPIN_UP_SCORE, + SuperState.SCORE, + scoreReq.and(shooter::atFlywheelVelocitySetpoint)); bindTransition( SuperState.SPIN_UP_FEED, SuperState.FEED, feedReq.and(shooter::atFlywheelVelocitySetpoint)); @@ -167,15 +169,10 @@ private void addTransitions() { bindTransition(SuperState.FEED_FLOW, SuperState.FEED, flowReq.negate().and(feedReq)); bindTransition( - SuperState.FEED_FLOW, - SuperState.READY, - flowReq.negate().and(isEmpty.negate())); + SuperState.FEED_FLOW, SuperState.READY, flowReq.negate().and(isEmpty.negate())); // No so sure about the end condition here. - bindTransition( - SuperState.FEED_FLOW, - SuperState.IDLE, - flowReq.negate().and(isEmpty)); + bindTransition(SuperState.FEED_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty)); } // SCORE_FLOW transitions @@ -185,15 +182,10 @@ private void addTransitions() { bindTransition(SuperState.SCORE_FLOW, SuperState.SCORE, flowReq.negate().and(scoreReq)); bindTransition( - SuperState.SCORE_FLOW, - SuperState.READY, - flowReq.negate().and(isEmpty.negate())); + SuperState.SCORE_FLOW, SuperState.READY, flowReq.negate().and(isEmpty.negate())); // No so sure about the end condition here. - bindTransition( - SuperState.SCORE_FLOW, - SuperState.IDLE, - flowReq.negate().and(isEmpty)); + bindTransition(SuperState.SCORE_FLOW, SuperState.IDLE, flowReq.negate().and(isEmpty)); } // Transition from any state to SPIT for anti jamming @@ -217,11 +209,15 @@ private void addCommands() { indexer.index(), shooter.rest()); // Maybe index at slower speed? - bindCommands(SuperState.SPIN_UP_SCORE, intake.rest(), indexer.rest(), shooter.shoot(swerve::getPose)); + bindCommands( + SuperState.SPIN_UP_SCORE, intake.rest(), indexer.rest(), shooter.shoot(swerve::getPose)); - bindCommands(SuperState.SPIN_UP_FEED, intake.rest(), indexer.rest(), shooter.feed( - swerve::getPose, - () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); // TODO: SELECTION LOGIC + bindCommands( + SuperState.SPIN_UP_FEED, + intake.rest(), + indexer.rest(), + shooter.feed( + swerve::getPose, () -> FeedTargets.BLUE_BACK_RIGHT.getPose())); // TODO: SELECTION LOGIC bindCommands( SuperState.SCORE, intake.rest(), indexer.indexToShoot(), shooter.shoot(swerve::getPose)); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 4141bb4..9cfde0a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -6,8 +6,6 @@ import static edu.wpi.first.units.Units.Volts; -import java.util.function.Supplier; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -20,6 +18,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; import frc.robot.utils.autoaim.AutoAim; import frc.robot.utils.autoaim.InterpolatingShotTree.ShotData; +import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; public class ShooterSubsystem extends SubsystemBase { From d10a3765bb8110def0bd914bee46df4063556eb9 Mon Sep 17 00:00:00 2001 From: SCool62 Date: Mon, 19 Jan 2026 09:15:16 -0800 Subject: [PATCH 32/32] Make SPIN_UP -> shoot not require a button (automatic switch to avoid getting stuck in a state) --- src/main/java/frc/robot/Superstructure.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 36bc3c0..9be6815 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -153,10 +153,10 @@ private void addTransitions() { bindTransition( SuperState.SPIN_UP_SCORE, SuperState.SCORE, - scoreReq.and(shooter::atFlywheelVelocitySetpoint)); + new Trigger(shooter::atFlywheelVelocitySetpoint)); bindTransition( - SuperState.SPIN_UP_FEED, SuperState.FEED, feedReq.and(shooter::atFlywheelVelocitySetpoint)); + SuperState.SPIN_UP_FEED, SuperState.FEED, new Trigger(shooter::atFlywheelVelocitySetpoint)); bindTransition(SuperState.FEED, SuperState.IDLE, isEmpty);