From e730cf279a990815b8a6fb68facc8b2876117445 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Tue, 10 Mar 2026 20:17:01 -0400 Subject: [PATCH 1/3] removed tilt and roller subsystems --- src/main/java/frc/robot/RobotContainer.java | 12 --- .../frc/robot/commands/roller/SpinRoller.java | 41 -------- .../frc/robot/commands/tilt/TiltDown.java | 38 -------- .../java/frc/robot/commands/tilt/TiltUp.java | 39 -------- .../frc/robot/subsystems/RollerSubsystem.java | 73 -------------- .../frc/robot/subsystems/TiltSubsystem.java | 96 ------------------- 6 files changed, 299 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/roller/SpinRoller.java delete mode 100644 src/main/java/frc/robot/commands/tilt/TiltDown.java delete mode 100644 src/main/java/frc/robot/commands/tilt/TiltUp.java delete mode 100644 src/main/java/frc/robot/subsystems/RollerSubsystem.java delete mode 100644 src/main/java/frc/robot/subsystems/TiltSubsystem.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ac3ed82b..6aa84790 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -103,7 +103,6 @@ public class RobotContainer { // Instantiate the autochooser. private final AutoChooser autoChooser; // The robot's subsystems and commands are defined here... - // private final TiltSubsystem tiltSubsystem; private final CommandXboxController controller = new CommandXboxController(Constants.XBOX_CONTROLLER_PORT); private final ClimberSubsystem climberSubsystem; @@ -141,16 +140,11 @@ public RobotContainer() { // Configure the trigger bindings switch (Constants.currentMode) { case REAL -> { - // rollerSubsystem = new RollerSubsystem(RollerSubsystem.createRealIo()); - // tiltSubsystem = new TiltSubsystem(TiltSubsystem.createRealIo()); anglerSubsystem = new AnglerSubsystem(AnglerSubsystem.createRealIo()); intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createRealIo()); hopperSubsystem = new HopperSubsystem(HopperSubsystem.createRealIo()); intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createRealIo()); turretSubsystem = new TurretSubsystem(TurretSubsystem.createRealIo()); - - - climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createRealIo()); feederSubsystem = new FeederSubsystem(FeederSubsystem.createRealIo()); shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createRealIo()); @@ -158,7 +152,6 @@ public RobotContainer() { ThreadedGyro threadedGyro = gyroIo.getThreadedGyro(); gyroSubsystem = new GyroSubsystem(gyroIo); SwerveIMU swerveIMU = new ThreadedGyroSwerveIMU(threadedGyro); - drivebase = !Constants.TESTBED ? new SwerveSubsystem( new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), swerveIMU) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null; @@ -166,8 +159,6 @@ public RobotContainer() { } case REPLAY -> { - // rollerSubsystem = new RollerSubsystem(RollerSubsystem.createMockIo()); - // tiltSubsystem = new TiltSubsystem(TiltSubsystem.createMockIo()); anglerSubsystem = new AnglerSubsystem(AnglerSubsystem.createMockIo()); intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createMockIo()); hopperSubsystem = new HopperSubsystem(HopperSubsystem.createMockIo()); @@ -185,9 +176,6 @@ public RobotContainer() { } case SIM -> { robotVisualizer = new RobotVisualizer(); - //rollerSubsystem = new// RollerSubsystem(RollerSubsystem.createSimIo(robotVisualizer)); - // tiltSubsystem = new - // TiltSubsystem(TiltSubsystem.createSimIo(robotVisualizer)); anglerSubsystem = new AnglerSubsystem(AnglerSubsystem.createSimIo(robotVisualizer)); intakeSubsystem = new IntakeSubsystem(IntakeSubsystem.createSimIo(robotVisualizer)); hopperSubsystem = new HopperSubsystem(HopperSubsystem.createSimIo(robotVisualizer)); diff --git a/src/main/java/frc/robot/commands/roller/SpinRoller.java b/src/main/java/frc/robot/commands/roller/SpinRoller.java deleted file mode 100644 index 8d5b89cd..00000000 --- a/src/main/java/frc/robot/commands/roller/SpinRoller.java +++ /dev/null @@ -1,41 +0,0 @@ -package frc.robot.commands.roller; - -import edu.wpi.first.wpilibj.Timer; -import frc.robot.constants.Constants; -import frc.robot.subsystems.RollerSubsystem; -import frc.robot.utils.logging.commands.LoggableCommand; - -public class SpinRoller extends LoggableCommand { - private final RollerSubsystem subsystem; - private final Timer timer; - - public SpinRoller(RollerSubsystem subsystem) { - timer = new Timer(); - this.subsystem = subsystem; - addRequirements(subsystem); - } - - @Override - public void initialize() { - timer.restart(); - } - - @Override - public void execute() { - subsystem.setSpeed(Constants.ROLLER_SPEED); - } - - @Override - public void end(boolean interrupted) { - subsystem.stopMotors(); - } - - @Override - public boolean isFinished() { - if (timer.hasElapsed(Constants.SPIN_TIMEOUT)) { - return true; - } else{ - return false; - } - } -} diff --git a/src/main/java/frc/robot/commands/tilt/TiltDown.java b/src/main/java/frc/robot/commands/tilt/TiltDown.java deleted file mode 100644 index 4208304c..00000000 --- a/src/main/java/frc/robot/commands/tilt/TiltDown.java +++ /dev/null @@ -1,38 +0,0 @@ -package frc.robot.commands.tilt; -//added a comment -import edu.wpi.first.wpilibj.Timer; -import frc.robot.constants.Constants; -import frc.robot.subsystems.TiltSubsystem; -import frc.robot.utils.logging.commands.LoggableCommand; -// The command makes thing tilt down - -// This command tilts the thing down -public class TiltDown extends LoggableCommand { - private final TiltSubsystem subsystem; - private final Timer timer; - public TiltDown(TiltSubsystem subsystem) { - timer = new Timer(); - this.subsystem = subsystem; - addRequirements(subsystem); - } - - @Override - public void initialize() { - timer.restart(); - } - - @Override - public void execute() { - subsystem.setSpeed(-1 * Constants.TILT_SPEED); - } - - @Override - public void end(boolean interrupted) { - subsystem.stopMotors(); - } - - @Override - public boolean isFinished() { - return (subsystem.isAtBottom() || timer.hasElapsed(Constants.TILT_TIMEOUT)); - } -} diff --git a/src/main/java/frc/robot/commands/tilt/TiltUp.java b/src/main/java/frc/robot/commands/tilt/TiltUp.java deleted file mode 100644 index 8f8f580f..00000000 --- a/src/main/java/frc/robot/commands/tilt/TiltUp.java +++ /dev/null @@ -1,39 +0,0 @@ -package frc.robot.commands.tilt; - -import edu.wpi.first.wpilibj.Timer; -import frc.robot.constants.Constants; -import frc.robot.subsystems.TiltSubsystem; -import frc.robot.utils.logging.commands.LoggableCommand; -// This command makes thing tilt up - -// This command tilts the thing up. -public class TiltUp extends LoggableCommand { - private final TiltSubsystem subsystem; - private final Timer timer; - - public TiltUp(TiltSubsystem subsystem) { - timer = new Timer(); - this.subsystem = subsystem; - addRequirements(subsystem); - } - - @Override - public void initialize() { - timer.restart(); - } - - @Override - public void execute() { - subsystem.setSpeed(Constants.TILT_SPEED); - } - - @Override - public void end(boolean interrupted) { - subsystem.stopMotors(); - } - - @Override - public boolean isFinished() { - return (subsystem.isAtTop() || timer.hasElapsed(Constants.TILT_TIMEOUT)); - } -} diff --git a/src/main/java/frc/robot/subsystems/RollerSubsystem.java b/src/main/java/frc/robot/subsystems/RollerSubsystem.java deleted file mode 100644 index 16aee79f..00000000 --- a/src/main/java/frc/robot/subsystems/RollerSubsystem.java +++ /dev/null @@ -1,73 +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.subsystems; - -import com.revrobotics.PersistMode; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkLowLevel; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkBaseConfig; -import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.Constants; -import frc.robot.utils.logging.input.MotorLoggableInputs; -import frc.robot.utils.logging.io.motor.MockSparkMaxIo; -import frc.robot.utils.logging.io.motor.RealSparkMaxIo; -import frc.robot.utils.logging.io.motor.SimSparkMaxIo; -import frc.robot.utils.logging.io.motor.SparkMaxIo; -import frc.robot.utils.simulation.MotorSimulator; -import frc.robot.utils.simulation.RobotVisualizer; - -// The Roller subsystem spins the wheel that releases the algae. - -public class RollerSubsystem extends SubsystemBase { - public static final String LOGGING_NAME = "RollerSubsystem"; - private final SparkMaxIo io; - - public RollerSubsystem(SparkMaxIo io) { - this.io = io; - } - - public void setSpeed(double speed) { - io.set(speed); - } - - public void stopMotors() { - io.stopMotor(); - } - - @Override - public void periodic() { - io.periodic(); - } - - public static SparkMaxIo createMockIo() { - return new MockSparkMaxIo(LOGGING_NAME, MotorLoggableInputs.allMetrics()); - } - - public static SparkMaxIo createRealIo() { - return new RealSparkMaxIo(LOGGING_NAME, createMotor(), MotorLoggableInputs.allMetrics()); - } - - public static SparkMaxIo createSimIo(RobotVisualizer visualizer) { - SparkMax motor = createMotor(); - return new SimSparkMaxIo(LOGGING_NAME, motor, MotorLoggableInputs.allMetrics(), - new MotorSimulator(motor, visualizer.getRollerLigament())); - } - - private static SparkMax createMotor() { - SparkMax motor = new SparkMax(Constants.ROLLER_MOTOR_ID, SparkLowLevel.MotorType.kBrushless); - SparkMaxConfig motorConfig = new SparkMaxConfig(); - motorConfig.idleMode(SparkBaseConfig.IdleMode.kBrake); - motorConfig.smartCurrentLimit(Constants.NEO_CURRENT_LIMIT); - motor.configure( - motorConfig, - ResetMode.kResetSafeParameters, - PersistMode.kPersistParameters); - - return motor; - } -} diff --git a/src/main/java/frc/robot/subsystems/TiltSubsystem.java b/src/main/java/frc/robot/subsystems/TiltSubsystem.java deleted file mode 100644 index d9b0f00b..00000000 --- a/src/main/java/frc/robot/subsystems/TiltSubsystem.java +++ /dev/null @@ -1,96 +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.subsystems; - -import com.revrobotics.PersistMode; -import com.revrobotics.ResetMode; -import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkLowLevel; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkBaseConfig; -import com.revrobotics.spark.config.SparkMaxConfig; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.constants.Constants; -import frc.robot.utils.logging.input.MotorLoggableInputs; -import frc.robot.utils.logging.io.motor.MockSparkMaxIo; -import frc.robot.utils.logging.io.motor.RealSparkMaxIo; -import frc.robot.utils.logging.io.motor.SimSparkMaxIo; -import frc.robot.utils.logging.io.motor.SparkMaxIo; -import frc.robot.utils.simulation.ArmParameters; -import frc.robot.utils.simulation.ArmSimulator; -import frc.robot.utils.simulation.RobotVisualizer; - -// The Tilt subsystem extends and retracts the Algae-Go-Bye-Bye mechanism. - -public class TiltSubsystem extends SubsystemBase { - public static final String LOGGING_NAME = "TiltSubsystem"; - private final SparkMaxIo io; - - public TiltSubsystem(SparkMaxIo io) { - this.io = io; - } - - public void setSpeed(double speed) { - io.set(speed); - } - - public void stopMotors() { - io.stopMotor(); - } - - public boolean isAtTop() { - // Arm motor is inverted - rev switch is at top - return io.isRevSwitchPressed(); - } - - public boolean isAtBottom() { - // Arm motor is inverted - fwd switch is at bottom - return io.isFwdSwitchPressed(); - } - - @Override - public void periodic() { - io.periodic(); - } - - public static SparkMaxIo createMockIo() { - return new MockSparkMaxIo(LOGGING_NAME, MotorLoggableInputs.allMetrics()); - } - - public static SparkMaxIo createRealIo() { - return new RealSparkMaxIo(LOGGING_NAME, createMotor(), MotorLoggableInputs.allMetrics()); - } - - public static SparkMaxIo createSimIo(RobotVisualizer visualizer) { - SparkMax motor = createMotor(); - ArmSimulator simulator = new ArmSimulator(motor, createParams(), visualizer.getTiltLigament()); - return new SimSparkMaxIo(LOGGING_NAME, motor, MotorLoggableInputs.allMetrics(), simulator); - } - - private static SparkMax createMotor() { - SparkMax motor = new SparkMax(Constants.TILT_MOTOR_ID, SparkLowLevel.MotorType.kBrushless); - SparkMaxConfig motorConfig = new SparkMaxConfig(); - motorConfig.idleMode(SparkBaseConfig.IdleMode.kBrake); - motorConfig.smartCurrentLimit(Constants.NEO_CURRENT_LIMIT); - motor.configure( - motorConfig, - ResetMode.kResetSafeParameters, - PersistMode.kPersistParameters); - - return motor; - } - - private static ArmParameters createParams() { - ArmParameters params = new ArmParameters(); - params.name = "Tilt"; - params.armGearing = Constants.TILT_GEARING; - params.armInertia = Constants.TILT_INERTIA; - params.armLength = Constants.TILT_LENGTH; - params.armMinAngle = Constants.TILT_MIN_ANGLE; - params.armMaxAngle = Constants.TILT_MAX_ANGLE; - params.armSimulateGravity = Constants.TILT_SIMULATE_GRAVITY; - return params; - } -} From 451f25524fb3a5c035d53cb609ec98385ca4a2e4 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Tue, 10 Mar 2026 20:20:25 -0400 Subject: [PATCH 2/3] removed constants --- .../java/frc/robot/constants/ConstantsPushbot2026.java | 2 -- .../java/frc/robot/constants/ConstantsReal2026.java | 2 -- src/main/java/frc/robot/constants/GameConstants.java | 10 ---------- 3 files changed, 14 deletions(-) diff --git a/src/main/java/frc/robot/constants/ConstantsPushbot2026.java b/src/main/java/frc/robot/constants/ConstantsPushbot2026.java index 42ed7a3d..d6eadc22 100644 --- a/src/main/java/frc/robot/constants/ConstantsPushbot2026.java +++ b/src/main/java/frc/robot/constants/ConstantsPushbot2026.java @@ -4,8 +4,6 @@ public class ConstantsPushbot2026 extends GameConstants { // ConstantsPushbot2026 is only for CANIDs and nothing else, everything else goes into GameConstants. - public static final int ROLLER_MOTOR_ID = 99; // fake id for example subsytem to prevent crashes - public static final int TILT_MOTOR_ID = 98; // fake id for example subsytem to prevent crashes public static final int INTAKE_MOTOR_ID = 4; public static final int ANGLER_MOTOR_ID = 52; public static final int HOPPER_MOTOR_ID = 3; diff --git a/src/main/java/frc/robot/constants/ConstantsReal2026.java b/src/main/java/frc/robot/constants/ConstantsReal2026.java index 45a17e47..46b765ea 100644 --- a/src/main/java/frc/robot/constants/ConstantsReal2026.java +++ b/src/main/java/frc/robot/constants/ConstantsReal2026.java @@ -10,8 +10,6 @@ public class ConstantsReal2026 extends GameConstants { public static final int ANGLER_MOTOR_ID = 52; // other CAN-ID's - public static final int ROLLER_MOTOR_ID = 99; //remove - public static final int TILT_MOTOR_ID = 98; //remove public static final int TURRET_MOTOR_ID = 19; public static final int FEEDER_MOTOR_ID = 10; public static final int HOPPER_MOTOR_ID = 3; diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 9f1dde9f..b5d42755 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -45,8 +45,6 @@ public enum Mode { public static final int XBOX_CONTROLLER_PORT = 2; //Speeds - public static final double ROLLER_SPEED = 0.25; - public static final double TILT_SPEED = -0.5; // Arm motor is inverted - use negative speed public static final double INTAKE_SPEED = -0.5; public static final double HOPPER_SPEED = 0.35;//Want to increase this later public static final double CLIMBER_SPEED_UP = 0.1; @@ -72,8 +70,6 @@ public enum Mode { public static final double ANGLER_DIAGS_ENCODER = 1; //Timeouts - public static final double SPIN_TIMEOUT = 5; - public static final double TILT_TIMEOUT = 5; public static final double HOPPER_TIMEOUT = 60; public static final double CLIMBER_TIMEOUT = 10; public static final double FEEDER_TIMEOUT = 60; @@ -84,15 +80,9 @@ public enum Mode { public static final double TURRET_TIMEOUT = 5; //Angles - public static final Rotation2d TILT_MIN_ANGLE = Rotation2d.fromDegrees(45); - public static final Rotation2d TILT_MAX_ANGLE = Rotation2d.fromDegrees(90); public static final Rotation2d ANGLER_MIN_ANGLE = Rotation2d.fromDegrees(45); public static final Rotation2d ANGLER_MAX_ANGLE = Rotation2d.fromDegrees(90); - public static final double TILT_LENGTH = 0.2; - public static final double TILT_INERTIA = 0.5; - public static final double TILT_GEARING = 45.0; - public static final boolean TILT_SIMULATE_GRAVITY = false; public static final double ANGLER_LENGTH = 0.2; public static final double ANGLER_INERTIA = 0.5; public static final double ANGLER_GEARING = 45.0; From 848dda23617aee4b31215aa7e96429e6758dacf6 Mon Sep 17 00:00:00 2001 From: Jackson Wess Date: Tue, 10 Mar 2026 20:33:28 -0400 Subject: [PATCH 3/3] Removed robot visualizer --- .../utils/simulation/RobotVisualizer.java | 26 ------------------- 1 file changed, 26 deletions(-) diff --git a/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java b/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java index 5dd874d9..6f8fd5ba 100644 --- a/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java +++ b/src/main/java/frc/robot/utils/simulation/RobotVisualizer.java @@ -12,8 +12,6 @@ public class RobotVisualizer { private final LoggedMechanism2d mech2d = new LoggedMechanism2d(2, Units.feetToMeters(7)); - private final LoggedMechanismLigament2d tiltLigament; - private final LoggedMechanismLigament2d rollerLigament; private final LoggedMechanismLigament2d intakeLigament; private final LoggedMechanismLigament2d hopperLigament; private final LoggedMechanismLigament2d climberLigament; @@ -25,22 +23,6 @@ public class RobotVisualizer { private final LoggedMechanismLigament2d intakeDeploymentLigament; public RobotVisualizer() { - LoggedMechanismRoot2d root = mech2d.getRoot("Robot Root", Constants.DRIVE_BASE_WIDTH / 2, - Constants.INITIAL_ROBOT_HEIGHT); - - LoggedMechanismLigament2d riserLigament = root.append( - new LoggedMechanismLigament2d( - "Riser", 0.35, 90, 5, new Color8Bit(Color.kDarkGray))); - this.tiltLigament = riserLigament.append( - new LoggedMechanismLigament2d( - "Tilt", - 0.5, - 90.0, - 4, - new Color8Bit(Color.kCornflowerBlue))); - this.rollerLigament = this.tiltLigament.append( - new LoggedMechanismLigament2d( - "Roller", 0.05, 180, 5, new Color8Bit(Color.kGreen))); LoggedMechanismRoot2d anglerRoot = mech2d.getRoot("Angler Root", Constants.DRIVE_BASE_WIDTH, Constants.INITIAL_ROBOT_HEIGHT); @@ -171,14 +153,6 @@ public RobotVisualizer() { } - public LoggedMechanismLigament2d getRollerLigament() { - return rollerLigament; - } - - public LoggedMechanismLigament2d getTiltLigament() { - return tiltLigament; - } - public LoggedMechanismLigament2d getIntakeLigament() { return intakeLigament; }