- * This constructs the underlying hardware devices, so users should not construct the - * devices themselves. If they need the devices, they can access them through getters in the - * classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules) - { - super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *
- * This constructs the underlying hardware devices, so users should not construct the - * devices themselves. If they need the devices, they can access them through getters in the - * classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or - * set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules) - { - super( - TalonFX::new, - TalonFX::new, - CANcoder::new, - drivetrainConstants, - odometryUpdateFrequency, - modules); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *
- * This constructs the underlying hardware devices, so users should not construct the
- * devices themselves. If they need the devices, they can access them through getters in the
- * classes.
- *
- * @param drivetrainConstants Drivetrain-wide constants for the swerve drive
- * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or
- * set to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0.
- * @param odometryStandardDeviation The standard deviation for odometry calculation in the
- * form [x, y, theta]ᵀ, with units in meters and radians
- * @param visionStandardDeviation The standard deviation for vision calculation in the form
- * [x, y, theta]ᵀ, with units in meters and radians
- * @param modules Constants for each specific module
- */
- public TunerSwerveDrivetrain(
- SwerveDrivetrainConstants drivetrainConstants,
- double odometryUpdateFrequency,
- Matrix
- * Device configuration and other behaviors not exposed by TunerConstants can be customized here.
- */
-public class ModuleIOTalonFX implements ModuleIO {
- private final SwerveModuleConstants
- * This version is intended for Phoenix 6 devices on both the RIO and CANivore buses. When using a
- * CANivore, the thread uses the "waitForAll" blocking method to enable more consistent sampling.
- * This also allows Phoenix Pro users to benefit from lower latency between devices using CANivore
- * time synchronization.
- */
-public class PhoenixOdometryThread extends Thread {
- private final Lock signalsLock = new ReentrantLock(); // Prevents conflicts when registering
- // signals
- private BaseStatusSignal[] phoenixSignals = new BaseStatusSignal[0];
- private final List
- * This configuration includes:
- *
- * This method instantiates the actual hardware objects (TalonFX motors and CANcoder) that will
- * be used when running on a real robot.
- *
- * @return A RotaryMechanismReal object configured with real hardware
- */
- public static RevRotary getReal()
- {
- MotorIO io = new MotorIORev(NAME, Ports.revRotarySubsytemMotorMain, true, getREVConfig());
-
- return new RevRotary(new RotaryMechanismReal(io, CONSTANTS, Optional.empty()));
- }
-
- /**
- * Creates the simulation implementation of the rotary mechanism.
- *
- *
- * This method creates a physics-based simulation of the mechanism using WPILib's simulation
- * classes. It models the motor, moment of inertia, and other physical properties to provide
- * realistic behavior in simulation.
- *
- * @return A RotaryMechanismSim object configured for physics simulation
- */
- public static RevRotary getSim()
- {
- MotorIOSim io = new MotorIORevSim(
- NAME,
- Ports.revRotarySubsytemMotorMain,
- true,
- DCMOTOR,
- getREVConfig());
-
- return new RevRotary(new RotaryMechanismSim(
- io,
- DCMOTOR,
- MOI,
- false,
- CONSTANTS,
- Optional.empty()));
- }
-
- /**
- * Creates the log replay implementation of the rotary mechanism.
- *
- *
- * This is used with AdvantageKit's log replay feature, which allows you to replay logged data
- * and debug robot code without having the actual robot or running simulation.
- *
- * @return A RotaryMechanism object for log replay
- */
- public static RevRotary getReplay()
- {
- return new RevRotary(new RotaryMechanism(NAME, CONSTANTS) {});
- }
-
- /**
- * Method to get the appropriate RotaryMechanism based on the current robot mode.
- *
- * @return RotaryMechanism instance for the current mode (real, sim, or replay)
- */
- public static RevRotary get()
- {
- switch (Constants.currentMode) {
- case REAL:
- return getReal();
- case SIM:
- return getSim();
- case REPLAY:
- return getReplay();
- default:
- throw new IllegalStateException("Unrecognized Robot Mode");
- }
- }
-}
diff --git a/src/main/java/frc/robot/subsystems/w8_examples/rotary/Rotary.java b/src/main/java/frc/robot/subsystems/w8_examples/rotary/Rotary.java
deleted file mode 100644
index b497ffc..0000000
--- a/src/main/java/frc/robot/subsystems/w8_examples/rotary/Rotary.java
+++ /dev/null
@@ -1,132 +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.rotary;
-
-import static edu.wpi.first.units.Units.Amps;
-import static edu.wpi.first.units.Units.Degrees;
-import static edu.wpi.first.units.Units.Volts;
-import edu.wpi.first.math.filter.Debouncer;
-import edu.wpi.first.math.filter.Debouncer.DebounceType;
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.AngularVelocity;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc.lib.io.motor.MotorIO.PIDSlot;
-import frc.lib.mechanisms.rotary.RotaryMechanism;
-import frc.lib.util.LoggedTunableNumber;
-import frc.lib.util.LoggerHelper;
-import frc.robot.RobotState;
-import lombok.Getter;
-import lombok.RequiredArgsConstructor;
-
-public class Rotary extends SubsystemBase {
-
- private final RotaryMechanism io;
- private static LoggedTunableNumber STOW_SETPOINT =
- new LoggedTunableNumber("Rotary STOW", 0.0);
- private static LoggedTunableNumber RAISED_SETPOINT =
- new LoggedTunableNumber("Rotary RAISED", -90);
-
- @RequiredArgsConstructor
- @Getter
- public enum Setpoint {
- HOME(null),
- STOW(STOW_SETPOINT),
- RAISED(RAISED_SETPOINT);
-
- private final LoggedTunableNumber tunableNumber;
-
- public Angle getSetpoint()
- {
- if (tunableNumber == null) {
- return Degrees.of(0.0);
- }
- return Degrees.of(tunableNumber.get());
- }
- }
-
- private Debouncer homeDebouncer = new Debouncer(0.1, DebounceType.kRising);
- private Trigger homedTrigger;
-
- private final RobotState robotstate;
- private Setpoint setpoint = Setpoint.STOW;
-
- public Rotary(RotaryMechanism io)
- {
- this.io = io;
- this.robotstate = RobotState.getInstance();
- setSetpoint(RotaryConstants.DEFAULT_SETPOINT)
- .ignoringDisable(true)
- .schedule();
- homedTrigger =
- new Trigger(() -> homeDebouncer.calculate(io.getSupplyCurrent().gte(Amps.of(10))));
-
- }
-
- @Override
- public void periodic()
- {
- LoggerHelper.recordCurrentCommand(RotaryConstants.NAME, this);
- io.periodic();
- robotstate.setRotaryPose(io.getPoseSupplier().get());
-
- }
-
- public Command setSetpoint(Setpoint setpoint)
- {
- return this.runOnce(
- () -> io.runPosition(setpoint.getSetpoint(), RotaryConstants.CRUISE_VELOCITY,
- RotaryConstants.ACCELERATION, RotaryConstants.JERK,
- PIDSlot.SLOT_0))
- .withName("Go To " + setpoint.toString() + " Setpoint");
- };
-
- public boolean nearGoal(Angle targetPosition)
- {
- return io.nearGoal(targetPosition, RotaryConstants.TOLERANCE);
- }
-
- public Command waitUntilGoalCommand(Angle position)
- {
- return Commands.waitUntil(() -> {
- return nearGoal(position);
- });
- }
-
- public Command setGoalCommandWithWait(Setpoint setpoint)
- {
- return waitUntilGoalCommand(setpoint.getSetpoint())
- .deadlineFor(setSetpoint(setpoint))
- .withName("Go To " + setpoint.toString() + " Setpoint with wait");
- }
-
- public Command setStateCommand(Setpoint setpoint)
- {
- return this.runOnce(() -> this.setpoint = setpoint)
- .withName("Elevator Set State: " + setpoint.name());
- }
-
- public Command homeCommand()
- {
- return Commands.sequence(runOnce(() -> io.runVoltage(Volts.of(-2))),
- Commands.waitUntil(homedTrigger),
- runOnce(() -> io.setEncoderPosition(Setpoint.HOME.getSetpoint())),
- this.setStateCommand(Setpoint.STOW))
- .withName("Homing");
-
- }
-
- public AngularVelocity getVelocity()
- {
- return io.getVelocity();
- }
-
- public void close()
- {
- io.close();
- }
-}
diff --git a/src/main/java/frc/robot/subsystems/w8_examples/rotary/RotaryConstants.java b/src/main/java/frc/robot/subsystems/w8_examples/rotary/RotaryConstants.java
deleted file mode 100644
index 344c0b3..0000000
--- a/src/main/java/frc/robot/subsystems/w8_examples/rotary/RotaryConstants.java
+++ /dev/null
@@ -1,181 +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.rotary;
-
-import static edu.wpi.first.units.Units.Degrees;
-import static edu.wpi.first.units.Units.KilogramSquareMeters;
-import static edu.wpi.first.units.Units.Kilograms;
-import static edu.wpi.first.units.Units.Rotations;
-import static edu.wpi.first.units.Units.Second;
-import java.util.Optional;
-import static edu.wpi.first.units.Units.Meters;
-import com.ctre.phoenix6.configs.*;
-import com.ctre.phoenix6.signals.FeedbackSensorSourceValue;
-import com.ctre.phoenix6.signals.InvertedValue;
-import com.ctre.phoenix6.signals.NeutralModeValue;
-import edu.wpi.first.math.geometry.Translation3d;
-import edu.wpi.first.math.system.plant.DCMotor;
-import edu.wpi.first.units.AngularAccelerationUnit;
-import edu.wpi.first.units.Units;
-import edu.wpi.first.units.measure.Angle;
-import edu.wpi.first.units.measure.AngularAcceleration;
-import edu.wpi.first.units.measure.AngularVelocity;
-import edu.wpi.first.units.measure.Distance;
-import edu.wpi.first.units.measure.Mass;
-import edu.wpi.first.units.measure.MomentOfInertia;
-import edu.wpi.first.units.measure.Velocity;
-import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
-import frc.lib.io.absoluteencoder.AbsoluteEncoderIOCANCoderSim;
-import frc.lib.io.motor.MotorIOTalonFX;
-import frc.lib.io.motor.MotorIOTalonFX.TalonFXFollower;
-import frc.lib.io.motor.MotorIOTalonFXSim;
-import frc.lib.mechanisms.rotary.*;
-import frc.lib.mechanisms.rotary.RotaryMechanism.RotaryAxis;
-import frc.lib.mechanisms.rotary.RotaryMechanism.RotaryMechCharacteristics;
-import frc.robot.Constants;
-import frc.robot.Ports;
-
-
-public class RotaryConstants {
- public static String NAME = "Rotary";
-
- public static final Angle TOLERANCE = Degrees.of(2.0);
-
- public static final AngularVelocity CRUISE_VELOCITY =
- Units.RadiansPerSecond.of(1);
- public static final AngularAcceleration ACCELERATION =
- CRUISE_VELOCITY.div(0.1).per(Units.Second);
- public static final Velocity
- * This configuration includes:
- *
- * The CANcoder provides absolute position feedback, meaning it knows the true position of the
- * mechanism even after power cycling. The magnet offset calibrates the encoder's zero position.
- *
- * @param sim Whether this configuration is for simulation (true) or real robot (false). In
- * simulation, the offset is set to 0.0 since it's not needed.
- * @return A configured CANcoderConfiguration object
- */
- public static CANcoderConfiguration getCANcoderConfig(boolean sim)
- {
- CANcoderConfiguration config = new CANcoderConfiguration();
-
- config.MagnetSensor.MagnetOffset = sim ? 0.0 : ENCODER_OFFSET.in(Rotations);
-
- return config;
- }
-
- public static Rotary get()
- {
- switch (Constants.currentMode) {
- case REAL:
- return new Rotary(new RotaryMechanismReal(
- new MotorIOTalonFX(NAME, getFXConfig(), Ports.RotarySubsystemMotorMain,
- new TalonFXFollower(Ports.RotarySubsystemMotorFollower, false)),
- CONSTANTS,
- Optional.of(new AbsoluteEncoderIOCANCoderSim(Ports.RotarySubsystemEncoder,
- NAME + "Encoder", getCANcoderConfig(false)))));
- case SIM:
- return new Rotary(new RotaryMechanismSim(
- new MotorIOTalonFXSim(NAME, getFXConfig(), Ports.RotarySubsystemMotorMain,
- new TalonFXFollower(Ports.RotarySubsystemMotorFollower, false)),
- DCMOTOR, MOI, false, CONSTANTS,
- Optional.of(new AbsoluteEncoderIOCANCoderSim(Ports.RotarySubsystemEncoder,
- NAME + "Encoder", getCANcoderConfig(true)))));
- case REPLAY:
- return new Rotary(new RotaryMechanism(NAME, CONSTANTS) {});
- default:
- throw new IllegalStateException("Unrecognized Robot Mode");
- }
- }
-}
diff --git a/src/main/java/frc/robot/subsystems/w8_examples/servo1/Servo1.java b/src/main/java/frc/robot/subsystems/w8_examples/servo1/Servo1.java
deleted file mode 100644
index 9fd9304..0000000
--- a/src/main/java/frc/robot/subsystems/w8_examples/servo1/Servo1.java
+++ /dev/null
@@ -1,72 +0,0 @@
-/*
- * Copyright (C) 2025 Windham Windup
- *
- * This program is free software: you can redistribute it and/or modify it under the terms of the
- * GNU General Public License as published by the Free Software Foundation, either version 3 of the
- * License, or any later version.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without
- * even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License along with this program. If
- * not, see
- *
- *
- * @return A configured {@link SparkBaseConfig} object ready to apply to a REV Robotics SparkFlex or SparkMax motor controller
- */
- public static SparkBaseConfig getREVConfig()
- {
- SparkFlexConfig config = new SparkFlexConfig();
-
- config.voltageCompensation(12.0);
- config.idleMode(IdleMode.kBrake);
- config.inverted(false);
-
- // Add gear ratio configuration for position/velocity conversion
- config.encoder.positionConversionFactor(1.0 / GEAR_RATIO);
- config.encoder.velocityConversionFactor(1.0 / GEAR_RATIO / 60.0); // RPM to RPS
-
- // Add soft limits to match TalonFX behavior
-
- config.apply(SLOT0CONFIG);
-
- return config;
- }
-
- /**
- * Creates the real robot implementation of the rotary mechanism.
- *
- *
- *
- *
- * @return A configured TalonFXConfiguration object ready to apply to a motor controller
- */
- public static TalonFXConfiguration getFXConfig()
- {
- TalonFXConfiguration config = new TalonFXConfiguration();
-
- config.CurrentLimits.SupplyCurrentLimitEnable = false;
- config.CurrentLimits.SupplyCurrentLimit = 40.0;
- config.CurrentLimits.SupplyCurrentLowerLimit = 40.0;
- config.CurrentLimits.SupplyCurrentLowerTime = 0.1;
-
- config.CurrentLimits.StatorCurrentLimitEnable = false;
- config.CurrentLimits.StatorCurrentLimit = 80.0;
-
- config.Voltage.PeakForwardVoltage = 12.0;
- config.Voltage.PeakReverseVoltage = -12.0;
-
- config.MotorOutput.NeutralMode = NeutralModeValue.Brake;
- config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
-
- config.SoftwareLimitSwitch.ForwardSoftLimitEnable = false;
- config.SoftwareLimitSwitch.ForwardSoftLimitThreshold = MAX_ANGLE.in(Units.Rotations);
-
- config.SoftwareLimitSwitch.ReverseSoftLimitEnable = false;
- config.SoftwareLimitSwitch.ReverseSoftLimitThreshold = MIN_ANGLE.in(Units.Rotations);
-
- config.Feedback.RotorToSensorRatio = ROTOR_TO_SENSOR;
- config.Feedback.SensorToMechanismRatio = SENSOR_TO_MECHANISM;
-
- config.Feedback.FeedbackRemoteSensorID = Ports.RotarySubsystemEncoder.id();
- config.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
-
- config.Slot0 = SLOT0CONFIG;
-
- return config;
- }
-
- /**
- * Creates and returns the CANcoder absolute encoder configuration.
- *
- *