From fa38602e24a349ce9f0e7253b5a165e18c3bfb7f Mon Sep 17 00:00:00 2001 From: jpothen8 Date: Tue, 14 May 2024 20:02:35 -0400 Subject: [PATCH] make ktlint happy --- .../frc/team449/control/auto/PIDPoseAlign.kt | 2 +- .../team449/control/holonomic/SwerveModule.kt | 2 +- .../auto/routines/FourPieceAmpHelper.kt | 10 ---------- .../robot2024/subsystems/ControllerBindings.kt | 18 +++++++++--------- .../subsystems/shooter/SpinShooter.kt | 1 - 5 files changed, 11 insertions(+), 22 deletions(-) diff --git a/src/main/kotlin/frc/team449/control/auto/PIDPoseAlign.kt b/src/main/kotlin/frc/team449/control/auto/PIDPoseAlign.kt index 326738c..a1c583a 100644 --- a/src/main/kotlin/frc/team449/control/auto/PIDPoseAlign.kt +++ b/src/main/kotlin/frc/team449/control/auto/PIDPoseAlign.kt @@ -26,7 +26,7 @@ class PIDPoseAlign( private val yController: PIDController = PIDController(AutoConstants.DEFAULT_Y_KP, 0.0, 0.0), private val thetaController: PIDController = PIDController(AutoConstants.DEFAULT_ROTATION_KP, 0.0, 0.0), poseTol: Pose2d = Pose2d(0.035, 0.035, Rotation2d(0.035)), - private val timeout: Double = 0.65, + private val timeout: Double = 0.65 ) : Command() { private val timer = Timer() diff --git a/src/main/kotlin/frc/team449/control/holonomic/SwerveModule.kt b/src/main/kotlin/frc/team449/control/holonomic/SwerveModule.kt index 6dc7c53..de50f36 100644 --- a/src/main/kotlin/frc/team449/control/holonomic/SwerveModule.kt +++ b/src/main/kotlin/frc/team449/control/holonomic/SwerveModule.kt @@ -113,7 +113,7 @@ open class SwerveModule( turningMotor.set( turnPid + sign(desiredState.angle.radians - turningMotor.position) * - SwerveConstants.STEER_KS + SwerveConstants.STEER_KS ) } diff --git a/src/main/kotlin/frc/team449/robot2024/auto/routines/FourPieceAmpHelper.kt b/src/main/kotlin/frc/team449/robot2024/auto/routines/FourPieceAmpHelper.kt index e53de16..0ce6df4 100644 --- a/src/main/kotlin/frc/team449/robot2024/auto/routines/FourPieceAmpHelper.kt +++ b/src/main/kotlin/frc/team449/robot2024/auto/routines/FourPieceAmpHelper.kt @@ -44,16 +44,6 @@ class FourPieceAmpHelper( ) ) - shot2Offset - init { - println(FieldConstants.BLUE_SPEAKER_POSE.getDistance( - Translation2d( - 3.7641966342926025, - 6.230502605438232 - ) - )) - println(shot1PivotAngle) - } - override val routine = ChoreoRoutine( thetaController = PIDController(2.85, 0.0, 0.075), diff --git a/src/main/kotlin/frc/team449/robot2024/subsystems/ControllerBindings.kt b/src/main/kotlin/frc/team449/robot2024/subsystems/ControllerBindings.kt index f49cf4c..7ea94e6 100644 --- a/src/main/kotlin/frc/team449/robot2024/subsystems/ControllerBindings.kt +++ b/src/main/kotlin/frc/team449/robot2024/subsystems/ControllerBindings.kt @@ -278,15 +278,15 @@ class ControllerBindings( ) ) ) - .andThen( - robot.pivot.moveAmp() - .alongWith( - robot.shooter.scoreAmp(), - WaitUntilCommand { robot.pivot.inAmpTolerance() && robot.shooter.atAmpSetpoint() } - .andThen(robot.feeder.intake()) - ) - .until { robot.infrared.get() && robot.closeToShooterInfrared.get() } - ) + .andThen( + robot.pivot.moveAmp() + .alongWith( + robot.shooter.scoreAmp(), + WaitUntilCommand { robot.pivot.inAmpTolerance() && robot.shooter.atAmpSetpoint() } + .andThen(robot.feeder.intake()) + ) + .until { robot.infrared.get() && robot.closeToShooterInfrared.get() } + ) ) // driveController.leftBumper().onTrue( diff --git a/src/main/kotlin/frc/team449/robot2024/subsystems/shooter/SpinShooter.kt b/src/main/kotlin/frc/team449/robot2024/subsystems/shooter/SpinShooter.kt index abec6d8..8a75a45 100644 --- a/src/main/kotlin/frc/team449/robot2024/subsystems/shooter/SpinShooter.kt +++ b/src/main/kotlin/frc/team449/robot2024/subsystems/shooter/SpinShooter.kt @@ -781,7 +781,6 @@ open class SpinShooter( println(leftController.k.get(0, 0)) println(rightController.k.get(0, 0)) - return if (RobotBase.isReal()) { SpinShooter( rightMotor,