From b0826d1e8555698ced08b11d9989bdb7f77c4a10 Mon Sep 17 00:00:00 2001 From: Cameron Earle Date: Sat, 13 Jan 2024 15:29:07 -0500 Subject: [PATCH] Rename subsystems and switch to non async queue --- build.gradle | 1 + gradlew | 0 src/main/java/frc/robot/RobotContainer.java | 8 +-- .../drive/PhoenixOdometryThread.java | 4 +- .../drive/SparkMaxOdometryThread.java | 4 +- .../{FeederIO.java => KitbotFeederIO.java} | 2 +- ...eederIOSim.java => KitbotFeederIOSim.java} | 2 +- ...rkMax.java => KitbotFeederIOSparkMax.java} | 4 +- ...{FlywheelIO.java => KitbotFlywheelIO.java} | 2 +- ...eelIOSim.java => KitbotFlywheelIOSim.java} | 4 +- ...Max.java => KitbotFlywheelIOSparkMax.java} | 4 +- .../kitbotshooter/KitbotShooter.java | 62 +++++++++---------- 12 files changed, 49 insertions(+), 48 deletions(-) mode change 100644 => 100755 gradlew rename src/main/java/frc/robot/subsystems/kitbotshooter/{FeederIO.java => KitbotFeederIO.java} (94%) rename src/main/java/frc/robot/subsystems/kitbotshooter/{FeederIOSim.java => KitbotFeederIOSim.java} (94%) rename src/main/java/frc/robot/subsystems/kitbotshooter/{FeederIOSparkMax.java => KitbotFeederIOSparkMax.java} (93%) rename src/main/java/frc/robot/subsystems/kitbotshooter/{FlywheelIO.java => KitbotFlywheelIO.java} (95%) rename src/main/java/frc/robot/subsystems/kitbotshooter/{FlywheelIOSim.java => KitbotFlywheelIOSim.java} (93%) rename src/main/java/frc/robot/subsystems/kitbotshooter/{FlywheelIOSparkMax.java => KitbotFlywheelIOSparkMax.java} (97%) diff --git a/build.gradle b/build.gradle index 327ca606..f29d4ec6 100644 --- a/build.gradle +++ b/build.gradle @@ -151,6 +151,7 @@ spotless { toggleOffOn() googleJavaFormat() removeUnusedImports() + indentWithSpaces(4) trimTrailingWhitespace() endWithNewline() } diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c27ce205..d72b2e1e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -70,7 +70,7 @@ public RobotContainer() { // new ModuleIOTalonFX(1), // new ModuleIOTalonFX(2), // new ModuleIOTalonFX(3)); - shooter = new KitbotShooter(new FlywheelIOSparkMax(), new FeederIOSparkMax()); + shooter = new KitbotShooter(new KitbotFlywheelIOSparkMax(), new KitbotFeederIOSparkMax()); break; case SIM: @@ -82,7 +82,7 @@ public RobotContainer() { new ModuleIOSim(), new ModuleIOSim(), new ModuleIOSim()); - shooter = new KitbotShooter(new FlywheelIOSim(), new FeederIOSim()); + shooter = new KitbotShooter(new KitbotFlywheelIOSim(), new KitbotFeederIOSim()); break; default: @@ -94,7 +94,7 @@ public RobotContainer() { new ModuleIO() {}, new ModuleIO() {}, new ModuleIO() {}); - shooter = new KitbotShooter(new FlywheelIO() {}, new FeederIO() {}); + shooter = new KitbotShooter(new KitbotFlywheelIO() {}, new KitbotFeederIO() {}); break; } @@ -116,7 +116,7 @@ public RobotContainer() { "Flywheel FF Characterization", Commands.sequence( Commands.runOnce( - () -> shooter.setCurrentMode(KitbotShooter.MODE.CHARACTERIZING), shooter), + () -> shooter.setCurrentMode(KitbotShooter.Mode.CHARACTERIZING), shooter), new FeedForwardCharacterization( shooter, shooter::runFlywheelVolts, shooter::getFlywheelVelocityRadPerSec), Commands.runOnce(() -> shooter.setCurrentMode(null)))); diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 33a44481..2a2b1cf7 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -17,10 +17,10 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.ParentDevice; +import java.util.ArrayDeque; import java.util.ArrayList; import java.util.List; import java.util.Queue; -import java.util.concurrent.ArrayBlockingQueue; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; @@ -55,7 +55,7 @@ private PhoenixOdometryThread() { } public Queue registerSignal(ParentDevice device, StatusSignal signal) { - Queue queue = new ArrayBlockingQueue<>(100); + Queue queue = new ArrayDeque<>(100); signalsLock.lock(); Drive.odometryLock.lock(); try { diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java index bbe5267a..93fe1df1 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java @@ -14,10 +14,10 @@ package frc.robot.subsystems.drive; import edu.wpi.first.wpilibj.Notifier; +import java.util.ArrayDeque; import java.util.ArrayList; import java.util.List; import java.util.Queue; -import java.util.concurrent.ArrayBlockingQueue; import java.util.function.DoubleSupplier; /** @@ -47,7 +47,7 @@ private SparkMaxOdometryThread() { } public Queue registerSignal(DoubleSupplier signal) { - Queue queue = new ArrayBlockingQueue<>(100); + Queue queue = new ArrayDeque<>(100); Drive.odometryLock.lock(); try { signals.add(signal); diff --git a/src/main/java/frc/robot/subsystems/kitbotshooter/FeederIO.java b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIO.java similarity index 94% rename from src/main/java/frc/robot/subsystems/kitbotshooter/FeederIO.java rename to src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIO.java index e49bda64..9f672e13 100644 --- a/src/main/java/frc/robot/subsystems/kitbotshooter/FeederIO.java +++ b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIO.java @@ -2,7 +2,7 @@ import org.littletonrobotics.junction.AutoLog; -public interface FeederIO { +public interface KitbotFeederIO { @AutoLog class FeederIOInputs { public double feederPositionRads = 0.0; diff --git a/src/main/java/frc/robot/subsystems/kitbotshooter/FeederIOSim.java b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIOSim.java similarity index 94% rename from src/main/java/frc/robot/subsystems/kitbotshooter/FeederIOSim.java rename to src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIOSim.java index 016532a0..27daadf5 100644 --- a/src/main/java/frc/robot/subsystems/kitbotshooter/FeederIOSim.java +++ b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIOSim.java @@ -5,7 +5,7 @@ import edu.wpi.first.wpilibj.simulation.FlywheelSim; import frc.robot.Constants; -public class FeederIOSim implements FeederIO { +public class KitbotFeederIOSim implements KitbotFeederIO { private FlywheelSim sim = new FlywheelSim(DCMotor.getNeoVortex(1), (1.0 / 1.0), 0.002); private double positionRads; diff --git a/src/main/java/frc/robot/subsystems/kitbotshooter/FeederIOSparkMax.java b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIOSparkMax.java similarity index 93% rename from src/main/java/frc/robot/subsystems/kitbotshooter/FeederIOSparkMax.java rename to src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIOSparkMax.java index d7ca78b3..f284ef2e 100644 --- a/src/main/java/frc/robot/subsystems/kitbotshooter/FeederIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFeederIOSparkMax.java @@ -7,13 +7,13 @@ import edu.wpi.first.math.util.Units; import frc.robot.Constants; -public class FeederIOSparkMax implements FeederIO { +public class KitbotFeederIOSparkMax implements KitbotFeederIO { private static final double GEARING = (1.0 / 1.0); private final CANSparkMax motor; private final RelativeEncoder encoder; - public FeederIOSparkMax() { + public KitbotFeederIOSparkMax() { motor = new CANSparkMax(3, CANSparkLowLevel.MotorType.kBrushless); motor.restoreFactoryDefaults(); diff --git a/src/main/java/frc/robot/subsystems/kitbotshooter/FlywheelIO.java b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIO.java similarity index 95% rename from src/main/java/frc/robot/subsystems/kitbotshooter/FlywheelIO.java rename to src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIO.java index 777b44a0..d35b17d6 100644 --- a/src/main/java/frc/robot/subsystems/kitbotshooter/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIO.java @@ -2,7 +2,7 @@ import org.littletonrobotics.junction.AutoLog; -public interface FlywheelIO { +public interface KitbotFlywheelIO { @AutoLog class FlywheelIOInputs { public double[] flywheelPositionRads = new double[] {}; diff --git a/src/main/java/frc/robot/subsystems/kitbotshooter/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIOSim.java similarity index 93% rename from src/main/java/frc/robot/subsystems/kitbotshooter/FlywheelIOSim.java rename to src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIOSim.java index db1e6419..6aa87c73 100644 --- a/src/main/java/frc/robot/subsystems/kitbotshooter/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIOSim.java @@ -8,7 +8,7 @@ import frc.robot.Constants; import java.util.Arrays; -public class FlywheelIOSim implements FlywheelIO { +public class KitbotFlywheelIOSim implements KitbotFlywheelIO { private static final SimpleMotorFeedforward ffModel = new SimpleMotorFeedforward(0.0, 0.0, 0.0); private static final PIDController feedback = new PIDController(0.0, 0.0, 0.0); @@ -22,7 +22,7 @@ public class FlywheelIOSim implements FlywheelIO { public void updateInputs(FlywheelIOInputs inputs) { positionRads += sim.getAngularVelocityRadPerSec() * (Constants.loopPeriodMs / 1000.0); inputs.flywheelPositionRads = new double[] {positionRads, positionRads}; - inputs.flywheelPositionRads = new double[2]; + inputs.flywheelVelocityRadPerSec = new double[2]; Arrays.fill(inputs.flywheelPositionRads, sim.getAngularVelocityRadPerSec()); inputs.flywheelAppliedVolts = new double[] {inputVolts, inputVolts}; } diff --git a/src/main/java/frc/robot/subsystems/kitbotshooter/FlywheelIOSparkMax.java b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIOSparkMax.java similarity index 97% rename from src/main/java/frc/robot/subsystems/kitbotshooter/FlywheelIOSparkMax.java rename to src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIOSparkMax.java index 5ce63d2b..bf771915 100644 --- a/src/main/java/frc/robot/subsystems/kitbotshooter/FlywheelIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotFlywheelIOSparkMax.java @@ -10,7 +10,7 @@ import frc.robot.Constants; import java.util.Arrays; -public class FlywheelIOSparkMax implements FlywheelIO { +public class KitbotFlywheelIOSparkMax implements KitbotFlywheelIO { // FIND THESE CONSTANTS private static final boolean masterInverted = false; private static final boolean followerInverted = false; @@ -21,7 +21,7 @@ public class FlywheelIOSparkMax implements FlywheelIO { private final CANSparkMax leader, follower; private final RelativeEncoder masterEncoder, followerEncoder; - public FlywheelIOSparkMax() { + public KitbotFlywheelIOSparkMax() { // FIND ID leader = new CANSparkMax(0, CANSparkLowLevel.MotorType.kBrushless); follower = new CANSparkMax(1, CANSparkLowLevel.MotorType.kBrushless); diff --git a/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotShooter.java b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotShooter.java index 7656fd6c..bd1f03b1 100644 --- a/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotShooter.java +++ b/src/main/java/frc/robot/subsystems/kitbotshooter/KitbotShooter.java @@ -13,69 +13,69 @@ public class KitbotShooter extends SubsystemBase { private static double flywheelIntakeVoltage = -5.0; private static double feederIntakeVoltage = -5.0; - private FlywheelIO flywheelIO; - private FeederIO feederIO; + private KitbotFlywheelIO kitbotFlywheelIO; + private KitbotFeederIO kitbotFeederIO; private FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); private FeederIOInputsAutoLogged feederInputs = new FeederIOInputsAutoLogged(); - private MODE currentMode = null; + private Mode currentMode = null; - public KitbotShooter(FlywheelIO flywheelIO, FeederIO feederIO) { - this.flywheelIO = flywheelIO; - this.feederIO = feederIO; + public KitbotShooter(KitbotFlywheelIO kitbotFlywheelIO, KitbotFeederIO kitbotFeederIO) { + this.kitbotFlywheelIO = kitbotFlywheelIO; + this.kitbotFeederIO = kitbotFeederIO; } @Override public void periodic() { // update inputs - flywheelIO.updateInputs(flywheelInputs); - feederIO.updateInputs(feederInputs); + kitbotFlywheelIO.updateInputs(flywheelInputs); + kitbotFeederIO.updateInputs(feederInputs); // process inputs Logger.processInputs("KitbotShooter/Flywheel", flywheelInputs); Logger.processInputs("KitbotShooter/Feeder", feederInputs); // check for current mode if (currentMode == null) { - currentMode = DriverStation.isEnabled() ? MODE.ENABLED : MODE.DISABLED; - flywheelIO.setBrakeMode(DriverStation.isEnabled()); - feederIO.setBrakeMode(DriverStation.isEnabled()); + currentMode = DriverStation.isEnabled() ? Mode.ENABLED : Mode.DISABLED; + kitbotFlywheelIO.setBrakeMode(DriverStation.isEnabled()); + kitbotFeederIO.setBrakeMode(DriverStation.isEnabled()); } - if (currentMode == MODE.DISABLED) { - flywheelIO.setBrakeMode(false); - feederIO.setBrakeMode(false); - } else if (currentMode == MODE.ENABLED) { + if (currentMode == Mode.DISABLED) { + kitbotFlywheelIO.setBrakeMode(false); + kitbotFeederIO.setBrakeMode(false); + } else if (currentMode == Mode.ENABLED) { // other stuff - } else if (currentMode == MODE.CHARACTERIZING) { + } else if (currentMode == Mode.CHARACTERIZING) { } } /** Set input voltage for flywheel */ public void runFlywheelVolts(double volts) { - flywheelIO.runVolts(volts); + kitbotFlywheelIO.runVolts(volts); } /** Set input voltage for feeder */ public void runFeederVolts(double volts) { - feederIO.runVolts(volts); + kitbotFeederIO.runVolts(volts); } /** Run flywheel at velocity */ public void runFlywheelVelocity(double rpm) { - if (currentMode != MODE.ENABLED) return; + if (currentMode != Mode.ENABLED) return; double rps = Units.rotationsPerMinuteToRadiansPerSecond(rpm); - flywheelIO.runVelocity(Units.rotationsToRadians(rps)); + kitbotFlywheelIO.runVelocity(Units.rotationsToRadians(rps)); } - public void setCurrentMode(MODE mode) { + public void setCurrentMode(Mode mode) { currentMode = mode; } @AutoLogOutput(key = "KitbotShooter/FlywheelRPM") public double getFlywheelRPM() { - return flywheelIO.getVelocityRPM(); + return kitbotFlywheelIO.getVelocityRPM(); } @AutoLogOutput(key = "KitbotShooter/FlywheelVelocityRadPerSec") @@ -95,17 +95,17 @@ public double getFeederCurrent() { public Command intakeCommand() { return Commands.startEnd( - () -> { - flywheelIO.runVolts(flywheelIntakeVoltage); - feederIO.runVolts(feederIntakeVoltage); - }, - () -> { - flywheelIO.runVolts(0.0); - feederIO.runVolts(0.0); - }); + () -> { + kitbotFlywheelIO.runVolts(flywheelIntakeVoltage); + kitbotFeederIO.runVolts(feederIntakeVoltage); + }, + () -> { + kitbotFlywheelIO.runVolts(0.0); + kitbotFeederIO.runVolts(0.0); + }); } - public enum MODE { + public enum Mode { ENABLED, DISABLED, CHARACTERIZING;