From 7ed8d41eaf4a00b406f088430e86ed87512c25c1 Mon Sep 17 00:00:00 2001 From: rakrakon <68773850+rakrakon@users.noreply.github.com> Date: Wed, 20 Nov 2024 16:48:59 +0200 Subject: [PATCH 1/5] Convert everything to runOnce --- src/main/kotlin/frc/robot/RobotContainer.kt | 4 ++-- src/main/kotlin/frc/robot/subsystems/climb/Climb.kt | 8 +++++--- src/main/kotlin/frc/robot/subsystems/conveyor/Conveyor.kt | 4 ++-- src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt | 2 +- src/main/kotlin/frc/robot/subsystems/hood/Hood.kt | 2 +- src/main/kotlin/frc/robot/subsystems/intake/Intake.kt | 6 +++++- src/main/kotlin/frc/robot/subsystems/shooter/Shooter.kt | 2 +- .../kotlin/frc/robot/subsystems/swerve/ModuleIOSim.kt | 2 +- .../frc/robot/subsystems/swerve/ModuleIOSparkMax.kt | 2 +- .../kotlin/frc/robot/subsystems/swerve/ModuleIOTalonFX.kt | 2 +- 10 files changed, 20 insertions(+), 14 deletions(-) diff --git a/src/main/kotlin/frc/robot/RobotContainer.kt b/src/main/kotlin/frc/robot/RobotContainer.kt index b9c46b74..4f630df1 100644 --- a/src/main/kotlin/frc/robot/RobotContainer.kt +++ b/src/main/kotlin/frc/robot/RobotContainer.kt @@ -99,7 +99,7 @@ object RobotContainer { driverController().back() .whileTrue(gripper.setRollerPower(0.4)) .onFalse(gripper.stop()) - driverController().start().whileTrue(intake.reset()) + driverController().start().whileTrue(intake.reset()).onFalse(intake.finishReset()) driverController().povUp().whileTrue(climb.openClimb()) driverController().povDown().whileTrue(climb.closeClimb()) @@ -115,7 +115,7 @@ object RobotContainer { operatorController().cross().onTrue(gripper.enableSensor()) operatorController().circle().onTrue(gripper.disableSensor()) - operatorController().options().whileTrue(intake.reset()) + operatorController().options().whileTrue(intake.reset()).onFalse(intake.finishReset()) } fun getAutonomousCommand(): Command = autoChooser.selected diff --git a/src/main/kotlin/frc/robot/subsystems/climb/Climb.kt b/src/main/kotlin/frc/robot/subsystems/climb/Climb.kt index 98639730..ef58ded9 100644 --- a/src/main/kotlin/frc/robot/subsystems/climb/Climb.kt +++ b/src/main/kotlin/frc/robot/subsystems/climb/Climb.kt @@ -4,6 +4,7 @@ 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.SubsystemBase +import edu.wpi.first.wpilibj2.command.WaitUntilCommand import frc.robot.lib.finallyDo import org.littletonrobotics.junction.AutoLogOutput import org.littletonrobotics.junction.Logger @@ -38,15 +39,16 @@ class Climb private constructor(private val io: ClimbIO) : SubsystemBase() { } fun unlock(): Command { - return Commands.run(io::openStopper).until { isStopperStuck }.andThen(io::disableStopper) + return Commands.runOnce(io::openStopper).andThen(Commands.waitUntil { isStopperStuck }) + .andThen(io::disableStopper) } fun lock(): Command { - return Commands.run(io::closeStopper).until { isStopperStuck }.andThen(io::disableStopper) + return Commands.runOnce(io::closeStopper).andThen(Commands.waitUntil { isStopperStuck }).andThen(io::disableStopper) } fun setPower(power: DoubleSupplier): Command { - return run { io.setPower(power.asDouble) } + return runOnce { io.setPower(power.asDouble) } } fun stop(): Command = setPower { 0.0 }.withTimeout(0.02) diff --git a/src/main/kotlin/frc/robot/subsystems/conveyor/Conveyor.kt b/src/main/kotlin/frc/robot/subsystems/conveyor/Conveyor.kt index 9d7819d5..6e2b50a9 100644 --- a/src/main/kotlin/frc/robot/subsystems/conveyor/Conveyor.kt +++ b/src/main/kotlin/frc/robot/subsystems/conveyor/Conveyor.kt @@ -45,13 +45,13 @@ class Conveyor private constructor(private val io: ConveyorIO) : SubsystemBase() timer.reset() } - fun setVelocity(velocitySupplier: () -> Measure>): Command = run { + fun setVelocity(velocitySupplier: () -> Measure>): Command = runOnce { val velocity = velocitySupplier.invoke() velocitySetpoint = velocity io.setVelocity(velocity) } - fun setVelocity(velocity: Measure>): Command = run { + fun setVelocity(velocity: Measure>): Command = runOnce { velocitySetpoint = velocity io.setVelocity(velocity) } diff --git a/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt b/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt index 4fce1467..bd4ec85c 100644 --- a/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt +++ b/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt @@ -43,7 +43,7 @@ class Gripper private constructor(private val io: GripperIO): SubsystemBase() { } fun setRollerPower(power: Double): Command { - return run { + return runOnce { rollerPowerSetPoint = power io.setRollerMotorPower(power) }.withName("Set Roller Power") diff --git a/src/main/kotlin/frc/robot/subsystems/hood/Hood.kt b/src/main/kotlin/frc/robot/subsystems/hood/Hood.kt index d96a58e6..9e205754 100644 --- a/src/main/kotlin/frc/robot/subsystems/hood/Hood.kt +++ b/src/main/kotlin/frc/robot/subsystems/hood/Hood.kt @@ -81,7 +81,7 @@ class Hood private constructor(private val io: HoodIO) : SubsystemBase() { } fun setAngle(angleSupplier: () -> Measure): Command { - return run { + return runOnce { val angle = angleSupplier.invoke() angleSetpoint = angle io.setAngle(angle) diff --git a/src/main/kotlin/frc/robot/subsystems/intake/Intake.kt b/src/main/kotlin/frc/robot/subsystems/intake/Intake.kt index fb99eeda..ba674842 100644 --- a/src/main/kotlin/frc/robot/subsystems/intake/Intake.kt +++ b/src/main/kotlin/frc/robot/subsystems/intake/Intake.kt @@ -85,7 +85,11 @@ class Intake private constructor(private val io: IntakeIO) : SubsystemBase() { } fun reset(): Command { - return Commands.run({ io.setAnglePower(-0.3) }).finallyDo(Runnable { + return Commands.runOnce({ io.setAnglePower(-0.3) }) + } + + fun finishReset(): Command { + return Commands.runOnce({ io.resetEncoder() io.setAnglePower(0.0) }) diff --git a/src/main/kotlin/frc/robot/subsystems/shooter/Shooter.kt b/src/main/kotlin/frc/robot/subsystems/shooter/Shooter.kt index d0f4478a..c50171e5 100644 --- a/src/main/kotlin/frc/robot/subsystems/shooter/Shooter.kt +++ b/src/main/kotlin/frc/robot/subsystems/shooter/Shooter.kt @@ -77,7 +77,7 @@ class Shooter private constructor(private val io: ShooterIO) : SubsystemBase() { } fun setVelocity(velocitySupplier: () -> Measure>): Command { - return run { + return runOnce { val velocity = velocitySupplier.invoke() topVelocitySetpoint = velocity bottomVelocitySetpoint = velocity diff --git a/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOSim.kt b/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOSim.kt index 7f1556b4..ab1338f2 100644 --- a/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOSim.kt @@ -104,7 +104,7 @@ class ModuleIOSim() : ModuleIO { } override fun checkModule(): Command? { - return Commands.run( + return Commands.runOnce( { driveControlRequest.withVelocity(0.8 * SwerveConstants.MAX_X_Y_VELOCITY) driveMotor.setControl(driveControlRequest) diff --git a/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOSparkMax.kt b/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOSparkMax.kt index b8f11d9d..94b78874 100644 --- a/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOSparkMax.kt +++ b/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOSparkMax.kt @@ -128,7 +128,7 @@ class ModuleIOSparkMax( } override fun checkModule(): Command? { - return Commands.run( + return Commands.runOnce( { driveMotor.set(0.8) angleMotor.set(0.2) diff --git a/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOTalonFX.kt b/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOTalonFX.kt index f93d214b..93dac91a 100644 --- a/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOTalonFX.kt +++ b/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOTalonFX.kt @@ -126,7 +126,7 @@ class ModuleIOTalonFX( } override fun checkModule(): Command? { - return Commands.run( + return Commands.runOnce( { driveMotor.set(0.8) angleMotor.set(0.2) From 7bea4dd6ab884e0965d37a9490e80ef62b9a330f Mon Sep 17 00:00:00 2001 From: rakrakon <68773850+rakrakon@users.noreply.github.com> Date: Wed, 20 Nov 2024 16:48:59 +0200 Subject: [PATCH 2/5] Convert everything to runOnce --- src/main/kotlin/frc/robot/RobotContainer.kt | 4 ++-- .../kotlin/frc/robot/subsystems/climb/Climb.kt | 8 +++++--- .../frc/robot/subsystems/conveyor/Conveyor.kt | 4 ++-- .../kotlin/frc/robot/subsystems/gripper/Gripper.kt | 2 +- src/main/kotlin/frc/robot/subsystems/hood/Hood.kt | 2 +- .../kotlin/frc/robot/subsystems/intake/Intake.kt | 14 ++++++++------ .../kotlin/frc/robot/subsystems/shooter/Shooter.kt | 2 +- .../frc/robot/subsystems/swerve/ModuleIOSim.kt | 2 +- .../robot/subsystems/swerve/ModuleIOSparkMax.kt | 2 +- .../frc/robot/subsystems/swerve/ModuleIOTalonFX.kt | 2 +- 10 files changed, 23 insertions(+), 19 deletions(-) diff --git a/src/main/kotlin/frc/robot/RobotContainer.kt b/src/main/kotlin/frc/robot/RobotContainer.kt index d6c1ecb7..07d916bd 100644 --- a/src/main/kotlin/frc/robot/RobotContainer.kt +++ b/src/main/kotlin/frc/robot/RobotContainer.kt @@ -107,7 +107,7 @@ object RobotContainer { driverController().back() .whileTrue(gripper.setRollerPower(0.4)) .onFalse(gripper.stop()) - driverController().start().whileTrue(intake.reset()) + driverController().start().whileTrue(intake.reset()).onFalse(intake.finishReset()) driverController().povUp().whileTrue(climb.openClimb()) driverController().povDown().whileTrue(climb.closeClimb()) @@ -123,7 +123,7 @@ object RobotContainer { operatorController().cross().onTrue(gripper.enableSensor()) operatorController().circle().onTrue(gripper.disableSensor()) - operatorController().options().whileTrue(intake.reset()) + operatorController().options().whileTrue(intake.reset()).onFalse(intake.finishReset()) } fun getAutonomousCommand(): Command = autoChooser.get() diff --git a/src/main/kotlin/frc/robot/subsystems/climb/Climb.kt b/src/main/kotlin/frc/robot/subsystems/climb/Climb.kt index 9058d6ed..caf0005e 100644 --- a/src/main/kotlin/frc/robot/subsystems/climb/Climb.kt +++ b/src/main/kotlin/frc/robot/subsystems/climb/Climb.kt @@ -4,6 +4,7 @@ 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.SubsystemBase +import edu.wpi.first.wpilibj2.command.WaitUntilCommand import frc.robot.lib.finallyDo import org.littletonrobotics.junction.AutoLogOutput import org.littletonrobotics.junction.Logger @@ -38,15 +39,16 @@ class Climb private constructor(private val io: ClimbIO) : SubsystemBase() { } fun unlock(): Command { - return Commands.run(io::openStopper).until { isStopperStuck }.andThen(io::disableStopper) + return Commands.runOnce(io::openStopper).andThen(Commands.waitUntil { isStopperStuck }) + .andThen(io::disableStopper) } fun lock(): Command { - return Commands.run(io::closeStopper).until { isStopperStuck }.andThen(io::disableStopper) + return Commands.runOnce(io::closeStopper).andThen(Commands.waitUntil { isStopperStuck }).andThen(io::disableStopper) } fun setPower(power: DoubleSupplier): Command { - return run { io.setPower(power.asDouble) } + return runOnce { io.setPower(power.asDouble) } } fun stop(): Command = setPower { 0.0 }.withTimeout(0.02) diff --git a/src/main/kotlin/frc/robot/subsystems/conveyor/Conveyor.kt b/src/main/kotlin/frc/robot/subsystems/conveyor/Conveyor.kt index d83d790c..1cc66535 100644 --- a/src/main/kotlin/frc/robot/subsystems/conveyor/Conveyor.kt +++ b/src/main/kotlin/frc/robot/subsystems/conveyor/Conveyor.kt @@ -46,13 +46,13 @@ class Conveyor private constructor(private val io: ConveyorIO) : SubsystemBase() timer.reset() } - fun setVelocity(velocitySupplier: () -> AngularVelocity): Command = run { + fun setVelocity(velocitySupplier: () -> AngularVelocity): Command = runOnce { val velocity = velocitySupplier.invoke() velocitySetpoint = velocity io.setVelocity(velocity) } - fun setVelocity(velocity: AngularVelocity): Command = run { + fun setVelocity(velocity: AngularVelocity): Command = runOnce { velocitySetpoint = velocity io.setVelocity(velocity) } diff --git a/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt b/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt index e590ebfd..fa979f55 100644 --- a/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt +++ b/src/main/kotlin/frc/robot/subsystems/gripper/Gripper.kt @@ -42,7 +42,7 @@ class Gripper private constructor(private val io: GripperIO) : SubsystemBase() { } fun setRollerPower(power: Double): Command { - return run { + return runOnce { rollerPowerSetPoint = power io.setRollerMotorPower(power) }.withName("Set Roller Power") diff --git a/src/main/kotlin/frc/robot/subsystems/hood/Hood.kt b/src/main/kotlin/frc/robot/subsystems/hood/Hood.kt index c49aa4f2..c41ed41e 100644 --- a/src/main/kotlin/frc/robot/subsystems/hood/Hood.kt +++ b/src/main/kotlin/frc/robot/subsystems/hood/Hood.kt @@ -79,7 +79,7 @@ class Hood private constructor(private val io: HoodIO) : SubsystemBase() { } fun setAngle(angleSupplier: () -> Angle): Command { - return run { + return runOnce { val angle = angleSupplier.invoke() angleSetpoint = angle io.setAngle(angle) diff --git a/src/main/kotlin/frc/robot/subsystems/intake/Intake.kt b/src/main/kotlin/frc/robot/subsystems/intake/Intake.kt index f0625e39..78637682 100644 --- a/src/main/kotlin/frc/robot/subsystems/intake/Intake.kt +++ b/src/main/kotlin/frc/robot/subsystems/intake/Intake.kt @@ -83,12 +83,14 @@ class Intake private constructor(private val io: IntakeIO) : SubsystemBase() { } fun reset(): Command { - return Commands.run({ io.setAnglePower(-0.3) }).finallyDo( - Runnable { - io.resetEncoder() - io.setAnglePower(0.0) - } - ) + return Commands.runOnce({ io.setAnglePower(-0.3) }) + } + + fun finishReset(): Command { + return Commands.runOnce({ + io.resetEncoder() + io.setAnglePower(0.0) + }) } override fun periodic() { diff --git a/src/main/kotlin/frc/robot/subsystems/shooter/Shooter.kt b/src/main/kotlin/frc/robot/subsystems/shooter/Shooter.kt index 31953b5d..7bd1a54d 100644 --- a/src/main/kotlin/frc/robot/subsystems/shooter/Shooter.kt +++ b/src/main/kotlin/frc/robot/subsystems/shooter/Shooter.kt @@ -78,7 +78,7 @@ class Shooter private constructor(private val io: ShooterIO) : SubsystemBase() { } fun setVelocity(velocitySupplier: () -> AngularVelocity): Command { - return run { + return runOnce { val velocity = velocitySupplier.invoke() topVelocitySetpoint = velocity bottomVelocitySetpoint = velocity diff --git a/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOSim.kt b/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOSim.kt index 269c5663..e99a1d6e 100644 --- a/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOSim.kt +++ b/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOSim.kt @@ -103,7 +103,7 @@ class ModuleIOSim() : ModuleIO { } override fun checkModule(): Command? { - return Commands.run( + return Commands.runOnce( { driveControlRequest.withVelocity(0.8 * SwerveConstants.MAX_X_Y_VELOCITY) driveMotor.setControl(driveControlRequest) diff --git a/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOSparkMax.kt b/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOSparkMax.kt index 5065c97f..69c5de2b 100644 --- a/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOSparkMax.kt +++ b/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOSparkMax.kt @@ -127,7 +127,7 @@ class ModuleIOSparkMax( } override fun checkModule(): Command? { - return Commands.run( + return Commands.runOnce( { driveMotor.set(0.8) angleMotor.set(0.2) diff --git a/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOTalonFX.kt b/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOTalonFX.kt index 371533f3..3d18a68e 100644 --- a/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOTalonFX.kt +++ b/src/main/kotlin/frc/robot/subsystems/swerve/ModuleIOTalonFX.kt @@ -122,7 +122,7 @@ class ModuleIOTalonFX( } override fun checkModule(): Command? { - return Commands.run( + return Commands.runOnce( { driveMotor.set(0.8) angleMotor.set(0.2) From 8d4557e7879ff68f5cf466eacebffbbf38e893a8 Mon Sep 17 00:00:00 2001 From: rakrakon <68773850+rakrakon@users.noreply.github.com> Date: Wed, 20 Nov 2024 17:35:48 +0200 Subject: [PATCH 3/5] Add waitUntil in climb --- src/main/kotlin/frc/robot/subsystems/climb/Climb.kt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/climb/Climb.kt b/src/main/kotlin/frc/robot/subsystems/climb/Climb.kt index caf0005e..11c4aa5b 100644 --- a/src/main/kotlin/frc/robot/subsystems/climb/Climb.kt +++ b/src/main/kotlin/frc/robot/subsystems/climb/Climb.kt @@ -4,7 +4,6 @@ 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.SubsystemBase -import edu.wpi.first.wpilibj2.command.WaitUntilCommand import frc.robot.lib.finallyDo import org.littletonrobotics.junction.AutoLogOutput import org.littletonrobotics.junction.Logger @@ -44,7 +43,8 @@ class Climb private constructor(private val io: ClimbIO) : SubsystemBase() { } fun lock(): Command { - return Commands.runOnce(io::closeStopper).andThen(Commands.waitUntil { isStopperStuck }).andThen(io::disableStopper) + return Commands.runOnce(io::closeStopper).andThen(Commands.waitUntil { isStopperStuck }) + .andThen(io::disableStopper) } fun setPower(power: DoubleSupplier): Command { From 7b85ee0132dd849c1362df4e8d918130310b7260 Mon Sep 17 00:00:00 2001 From: rakrakon <68773850+rakrakon@users.noreply.github.com> Date: Wed, 20 Nov 2024 17:36:07 +0200 Subject: [PATCH 4/5] Rebase conflicts --- src/main/kotlin/frc/robot/lib/LoggedTunableNumber.java | 3 +-- src/main/kotlin/frc/robot/lib/Utils.java | 1 - 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/lib/LoggedTunableNumber.java b/src/main/kotlin/frc/robot/lib/LoggedTunableNumber.java index f9d6a65d..6b47d0b1 100644 --- a/src/main/kotlin/frc/robot/lib/LoggedTunableNumber.java +++ b/src/main/kotlin/frc/robot/lib/LoggedTunableNumber.java @@ -8,13 +8,12 @@ package frc.robot.lib; import frc.robot.Constants; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; - import java.util.Arrays; import java.util.HashMap; import java.util.Map; import java.util.function.Consumer; import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; /** * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or diff --git a/src/main/kotlin/frc/robot/lib/Utils.java b/src/main/kotlin/frc/robot/lib/Utils.java index 62816116..570ee311 100644 --- a/src/main/kotlin/frc/robot/lib/Utils.java +++ b/src/main/kotlin/frc/robot/lib/Utils.java @@ -7,7 +7,6 @@ import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import frc.robot.Constants; - import java.util.Comparator; import java.util.List; From a77937a62c46608cc0a1342ade7338fde527f5e2 Mon Sep 17 00:00:00 2001 From: rakrakon <68773850+rakrakon@users.noreply.github.com> Date: Wed, 20 Nov 2024 17:38:37 +0200 Subject: [PATCH 5/5] Rebase conflicts --- src/main/kotlin/frc/robot/lib/LoggedTunableNumber.java | 3 ++- src/main/kotlin/frc/robot/lib/Utils.java | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/frc/robot/lib/LoggedTunableNumber.java b/src/main/kotlin/frc/robot/lib/LoggedTunableNumber.java index 6b47d0b1..f9d6a65d 100644 --- a/src/main/kotlin/frc/robot/lib/LoggedTunableNumber.java +++ b/src/main/kotlin/frc/robot/lib/LoggedTunableNumber.java @@ -8,12 +8,13 @@ package frc.robot.lib; import frc.robot.Constants; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; + import java.util.Arrays; import java.util.HashMap; import java.util.Map; import java.util.function.Consumer; import java.util.function.DoubleSupplier; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; /** * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or diff --git a/src/main/kotlin/frc/robot/lib/Utils.java b/src/main/kotlin/frc/robot/lib/Utils.java index 570ee311..62816116 100644 --- a/src/main/kotlin/frc/robot/lib/Utils.java +++ b/src/main/kotlin/frc/robot/lib/Utils.java @@ -7,6 +7,7 @@ import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import frc.robot.Constants; + import java.util.Comparator; import java.util.List;