From df86ba09449de913e986e317a82916970d76668e Mon Sep 17 00:00:00 2001 From: s-zhdanova Date: Sat, 11 Jan 2025 15:50:02 -0500 Subject: [PATCH] fix ktlint --- .../kotlin/frc/team449/ControllerBindings.kt | 25 +-- src/main/kotlin/frc/team449/Robot.kt | 4 - .../AutoScoreCommandConstants.kt | 178 ++++++++++-------- .../autoscoreCommands/AutoScoreCommands.kt | 57 +++--- .../frc/team449/subsystems/light/Light.kt | 3 - 5 files changed, 140 insertions(+), 127 deletions(-) diff --git a/src/main/kotlin/frc/team449/ControllerBindings.kt b/src/main/kotlin/frc/team449/ControllerBindings.kt index 9ab1d80..969a483 100644 --- a/src/main/kotlin/frc/team449/ControllerBindings.kt +++ b/src/main/kotlin/frc/team449/ControllerBindings.kt @@ -24,22 +24,25 @@ class ControllerBindings( private fun robotBindings() { /** Call robot functions you create below */ val autoscore = AutoScoreCommands(robot.drive, robot.poseSubsystem) - driveController.x().onTrue(ConditionalCommand( - autoscore.moveToReefCommand(AutoScoreCommandConstants.reefLocation.Location1B), - autoscore.moveToReefCommand(AutoScoreCommandConstants.reefLocation.Location1R), - {DriverStation.getAlliance().get() == DriverStation.Alliance.Blue} - )) - driveController.a().onTrue(ConditionalCommand( - autoscore.moveToProcessorCommandRed(), - autoscore.moveToProcessorCommandBlue(), - {DriverStation.getAlliance().get() == DriverStation.Alliance.Blue} - )) + driveController.x().onTrue( + ConditionalCommand( + autoscore.moveToReefCommand(AutoScoreCommandConstants.reefLocation.Location1B), + autoscore.moveToReefCommand(AutoScoreCommandConstants.reefLocation.Location1R), + { DriverStation.getAlliance().get() == DriverStation.Alliance.Blue } + ) + ) + driveController.a().onTrue( + ConditionalCommand( + autoscore.moveToProcessorCommandRed(), + autoscore.moveToProcessorCommandBlue(), + { DriverStation.getAlliance().get() == DriverStation.Alliance.Blue } + ) + ) } private fun nonRobotBindings() { // slowDrive() - if (RobotBase.isSimulation()) resetOdometrySim() resetGyro() diff --git a/src/main/kotlin/frc/team449/Robot.kt b/src/main/kotlin/frc/team449/Robot.kt index dd54b35..11fe270 100644 --- a/src/main/kotlin/frc/team449/Robot.kt +++ b/src/main/kotlin/frc/team449/Robot.kt @@ -1,8 +1,5 @@ package frc.team449 -import edu.wpi.first.networktables.BooleanSubscriber -import edu.wpi.first.networktables.NetworkTable -import edu.wpi.first.networktables.NetworkTableInstance import edu.wpi.first.wpilibj.PowerDistribution import edu.wpi.first.wpilibj2.command.button.CommandXboxController import frc.team449.subsystems.RobotConstants @@ -14,7 +11,6 @@ import frc.team449.system.AHRS import monologue.Annotations.Log import monologue.Logged - class Robot : RobotBase(), Logged { val driveController = CommandXboxController(0) diff --git a/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommandConstants.kt b/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommandConstants.kt index 22d41b4..48a35fd 100644 --- a/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommandConstants.kt +++ b/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommandConstants.kt @@ -48,199 +48,223 @@ class AutoScoreCommandConstants() { fun radians(degree: Int): Double { return Units.degreesToRadians(degree.toDouble()) } - //FILLER VALUES CURRENTLY + // FILLER VALUES CURRENTLY - //processor pose values - //BLUE + // processor pose values + // BLUE val processorTranslation2dBlue = Translation2d(6.358, 0.622) - val processorRotation2dBlue = Rotation2d(3*Math.PI / 2) // in radians + val processorRotation2dBlue = Rotation2d(3 * Math.PI / 2) // in radians val processorPoseBlue = Pose2d(processorTranslation2dBlue, processorRotation2dBlue) - //RED + + // RED val processorTranslation2dRed = Translation2d(11.520, 7.500) val processorRotation2dRed = Rotation2d(Math.PI / 2) // in radians val processorPoseRed = Pose2d(processorTranslation2dRed, processorRotation2dRed) - //coral intake pose values - //BLUE - //TOP + // coral intake pose values + // BLUE + // TOP val coralIntakeTranslation2dBlueTop = Translation2d(16.454, 6.990) val coralIntakeRotation2dBlueTop = Rotation2d(radians(55)) // in radians val coralIntakePoseBlueTop = Pose2d(coralIntakeTranslation2dBlueTop, coralIntakeRotation2dBlueTop) - //BOTTOM + + // BOTTOM val coralIntakeTranslation2dBlueBottom = Translation2d(16.454, 1.111) val coralIntakeRotation2dBlueBottom = Rotation2d(radians(-55)) // in radians val coralIntakePoseBlueBottom = Pose2d(coralIntakeTranslation2dBlueBottom, coralIntakeRotation2dBlueBottom) - //RED - //TOP + + // RED + // TOP val coralIntakeTranslation2dRedTop = Translation2d(1.214, 6.990) val coralIntakeRotation2dRedTop = Rotation2d(radians(125)) // in radians val coralIntakePoseRedTop = Pose2d(coralIntakeTranslation2dRedTop, coralIntakeRotation2dRedTop) - //BOTTOM + + // BOTTOM val coralIntakeTranslation2dRedBottom = Translation2d(1.214, 1.111) val coralIntakeRotation2dRedBottom = Rotation2d(radians(-125)) // in radians val coralIntakePoseRedBottom = Pose2d(coralIntakeTranslation2dRedBottom, coralIntakeRotation2dRedBottom) - - //net pose values + // net pose values val centerOfField = 8.808 - val netTranslationDistance: Double = 2.152; - //BLUE + val netTranslationDistance: Double = 2.152 + + // BLUE val netRotation2dBlue = Rotation2d(0.9) // in radians - //RED - val netRotation2dRed = Rotation2d(Math.PI) // in radians + // RED + val netRotation2dRed = Rotation2d(Math.PI) // in radians - //Reef pose translations are in meters (round to 3 decimal places) - //Reef pose angles are in radians - //BLUE - //reef 1st pose values + // Reef pose translations are in meters (round to 3 decimal places) + // Reef pose angles are in radians + // BLUE + // reef 1st pose values val reef1Translation2dBlue = Translation2d(4.906, 5.1359) - //RADIANS + + // RADIANS val reef1Rotation2dBlue = Rotation2d(radians(240)) val reef1PoseBlue = Pose2d(reef1Translation2dBlue, reef1Rotation2dBlue) - //reef 2nd pose values + // reef 2nd pose values val reef2Translation2dBlue = Translation2d(5.239, 4.978) - //RADIANS + + // RADIANS val reef2Rotation2dBlue = Rotation2d(radians(240)) val reef2PoseBlue = Pose2d(reef2Translation2dBlue, reef2Rotation2dBlue) - //reef 3rd pose values + // reef 3rd pose values val reef3Translation2dBlue = Translation2d(5.851, 4.156) - //RADIANS + + // RADIANS val reef3Rotation2dBlue = Rotation2d(radians(180)) val reef3PoseBlue = Pose2d(reef3Translation2dBlue, reef3Rotation2dBlue) - //reef 4th pose values + // reef 4th pose values val reef4Translation2dBlue = Translation2d(5.851, 3.876) - //RADIANS + + // RADIANS val reef4Rotation2dBlue = Rotation2d(radians(180)) val reef4PoseBlue = Pose2d(reef4Translation2dBlue, reef4Rotation2dBlue) - //reef 5th pose values + // reef 5th pose values val reef5Translation2dBlue = Translation2d(5.291, 2.966) - //RADIANS + + // RADIANS val reef5Rotation2dBlue = Rotation2d(radians(120)) val reef5PoseBlue = Pose2d(reef5Translation2dBlue, reef5Rotation2dBlue) - //reef 6th pose values - val reef6Translation2dBlue= Translation2d(5.011, 2.809) - //RADIANS + // reef 6th pose values + val reef6Translation2dBlue = Translation2d(5.011, 2.809) + + // RADIANS val reef6Rotation2dBlue = Rotation2d(radians(120)) val reef6PoseBlue = Pose2d(reef6Translation2dBlue, reef6Rotation2dBlue) - //reef 7th pose values + // reef 7th pose values val reef7Translation2dBlue = Translation2d(3.979, 2.809) - //RADIANS + + // RADIANS val reef7Rotation2dBlue = Rotation2d(radians(60)) val reef7PoseBlue = Pose2d(reef7Translation2dBlue, reef7Rotation2dBlue) - //reef 8th pose values + // reef 8th pose values val reef8Translation2dBlue = Translation2d(3.700, 2.966) - //RADIANS + + // RADIANS val reef8Rotation2dBlue = Rotation2d(radians(60)) val reef8PoseBlue = Pose2d(reef8Translation2dBlue, reef8Rotation2dBlue) - //reef 9th pose values + // reef 9th pose values val reef9Translation2dBlue = Translation2d(3.174, 3.876) - //RADIANS + + // RADIANS val reef9Rotation2dBlue = Rotation2d((radians(0))) val reef9PoseBlue = Pose2d(reef9Translation2dBlue, reef9Rotation2dBlue) - //reef 10th pose values + // reef 10th pose values val reef10Translation2dBlue = Translation2d(3.174, 4.173) - //RADIANS + + // RADIANS val reef10Rotation2dBlue = Rotation2d(radians(0)) val reef10PoseBlue = Pose2d(reef10Translation2dBlue, reef10Rotation2dBlue) - //reef 11th pose values + // reef 11th pose values val reef11Translation2dBlue = Translation2d(3.7340, 4.9601) - //RADIANS + + // RADIANS val reef11Rotation2dBlue = Rotation2d(radians(-60)) val reef11PoseBlue = Pose2d(reef11Translation2dBlue, reef11Rotation2dBlue) - //reef 12th pose values + // reef 12th pose values val reef12Translation2dBlue = Translation2d(4.0490, 5.153) - //RADIANS + + // RADIANS val reef12Rotation2dBlue = Rotation2d(radians(-60)) val reef12PoseBlue = Pose2d(reef12Translation2dBlue, reef12Rotation2dBlue) - - - //RED - //reef 1st pose values + // RED + // reef 1st pose values val reef1Translation2dRed = Translation2d(13.549, 5.136) - //RADIANS + + // RADIANS val reef1Rotation2dRed = Rotation2d(radians(240)) val reef1PoseRed = Pose2d(reef1Translation2dRed, reef1Rotation2dRed) - //reef 2nd pose values + // reef 2nd pose values val reef2Translation2dRed = Translation2d(13.794, 4.943) - //RADIANS + + // RADIANS val reef2Rotation2dRed = Rotation2d(radians(240)) val reef2PoseRed = Pose2d(reef2Translation2dRed, reef2Rotation2dRed) - //reef 3rd pose values + // reef 3rd pose values val reef3Translation2dRed = Translation2d(14.301, 4.121) - //RADIANS + + // RADIANS val reef3Rotation2dRed = Rotation2d(radians(180)) val reef3PoseRed = Pose2d(reef3Translation2dRed, reef3Rotation2dRed) - //reef 4th pose values + // reef 4th pose values val reef4Translation2dRed = Translation2d(14.267, 3.859) - //RADIANS + + // RADIANS val reef4Rotation2dRed = Rotation2d(radians(180)) val reef4PoseRed = Pose2d(reef4Translation2dRed, reef4Rotation2dRed) - //reef 5th pose values + // reef 5th pose values val reef5Translation2dRed = Translation2d(13.864, 2.966) - //RADIANS + + // RADIANS val reef5Rotation2dRed = Rotation2d(radians(120)) val reef5PoseRed = Pose2d(reef5Translation2dRed, reef5Rotation2dRed) - //reef 6th pose values - val reef6Translation2dRed= Translation2d(13.601, 2.809) - //RADIANS + // reef 6th pose values + val reef6Translation2dRed = Translation2d(13.601, 2.809) + + // RADIANS val reef6Rotation2dRed = Rotation2d(radians(120)) val reef6PoseRed = Pose2d(reef6Translation2dRed, reef6Rotation2dRed) - //reef 7th pose values + // reef 7th pose values val reef7Translation2dRed = Translation2d(12.534, 2.809) - //RADIANS + + // RADIANS val reef7Rotation2dRed = Rotation2d(radians(60)) val reef7PoseRed = Pose2d(reef7Translation2dRed, reef7Rotation2dRed) - //reef 8th pose values + // reef 8th pose values val reef8Translation2dRed = Translation2d(12.237, 2.966) - //RADIANS + + // RADIANS val reef8Rotation2dRed = Rotation2d(radians(60)) val reef8PoseRed = Pose2d(reef8Translation2dRed, reef8Rotation2dRed) - //reef 9th pose values + // reef 9th pose values val reef9Translation2dRed = Translation2d(11.730, 3.876) - //RADIANS + + // RADIANS val reef9Rotation2dRed = Rotation2d((radians(0))) val reef9PoseRed = Pose2d(reef9Translation2dRed, reef9Rotation2dRed) - //reef 10th pose values + // reef 10th pose values val reef10Translation2dRed = Translation2d(11.730, 4.173) - //RADIANS + + // RADIANS val reef10Rotation2dRed = Rotation2d(radians(0)) val reef10PoseRed = Pose2d(reef10Translation2dRed, reef10Rotation2dRed) - //reef 11th pose values + // reef 11th pose values val reef11Translation2dRed = Translation2d(12.307, 4.995) - //RADIANS + + // RADIANS val reef11Rotation2dRed = Rotation2d(radians(-60)) val reef11PoseRed = Pose2d(reef11Translation2dRed, reef11Rotation2dRed) - //reef 12th pose values + // reef 12th pose values val reef12Translation2dRed = Translation2d(12.604, 5.171) - //RADIANS + + // RADIANS val reef12Rotation2dRed = Rotation2d(radians(-60)) val reef12PoseRed = Pose2d(reef12Translation2dRed, reef12Rotation2dRed) - } - -} \ No newline at end of file +} diff --git a/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommands.kt b/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommands.kt index b352080..6172ab5 100644 --- a/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommands.kt +++ b/src/main/kotlin/frc/team449/commands/autoscoreCommands/AutoScoreCommands.kt @@ -2,33 +2,29 @@ package frc.team449.commands.autoscoreCommands import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.geometry.Translation2d -import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.InstantCommand import frc.team449.auto.choreo.PIDPoseAlign -import frc.team449.commands.autoscoreCommands.AutoScoreCommandConstants.Companion import frc.team449.subsystems.drive.swerve.SwerveDrive import frc.team449.subsystems.vision.PoseSubsystem -class AutoScoreCommands ( +class AutoScoreCommands( private val drive: SwerveDrive, - private val poseSubsystem: PoseSubsystem, + private val poseSubsystem: PoseSubsystem ) { - - init { + init { } /** - * This command moves the robot to one of the twelve reef locations - * (location 1 is the most vertical location on the right, going + * This command moves the robot to one of the twelve reef locations * (location 1 is the most vertical location on the right, going * clockwise) * @param reefLocation a reefLocationEnum that defines which spot to go to, defined with the numeric system above. */ - fun moveToReefCommand(reefLocation: AutoScoreCommandConstants.reefLocation, - ) : Command { - var reefNumericalLocation = reefLocation.ordinal + 1; - //RANDOM POSE so that compiler does not complain about undefined when command returned. + fun moveToReefCommand( + reefLocation: AutoScoreCommandConstants.reefLocation + ): Command { + var reefNumericalLocation = reefLocation.ordinal + 1 + // RANDOM POSE so that compiler does not complain about undefined when command returned. var reefPose = Pose2d(AutoScoreCommandConstants.reef1Translation2dRed, AutoScoreCommandConstants.reef1Rotation2dRed) when (reefNumericalLocation) { @@ -58,17 +54,17 @@ class AutoScoreCommands ( 24 -> reefPose = AutoScoreCommandConstants.reef12PoseRed } - return PIDPoseAlign(drive, poseSubsystem, reefPose) } + /** * this command scores the coral on the reef * level that is passed in. * @param reefLevel a reefLevel enum that determines which level to score the coral on */ - fun putCoralInReef(reefLevel: AutoScoreCommandConstants.reefLevel) : Command { - //we don't have score yet, but we're setting up stuff for future - //we won't have to account for alliance here + fun putCoralInReef(reefLevel: AutoScoreCommandConstants.reefLevel): Command { + // we don't have score yet, but we're setting up stuff for future + // we won't have to account for alliance here when (reefLevel) { AutoScoreCommandConstants.reefLevel.L1 -> TODO() AutoScoreCommandConstants.reefLevel.L2 -> TODO() @@ -76,16 +72,16 @@ class AutoScoreCommands ( AutoScoreCommandConstants.reefLevel.L4 -> TODO() } } + /** - * moves robot to processor location using - * swerve drive. + * moves robot to processor location using * swerve drive. */ - fun moveToProcessorCommandBlue() : Command { + fun moveToProcessorCommandBlue(): Command { var returnCommand = PIDPoseAlign(drive, poseSubsystem, AutoScoreCommandConstants.processorPoseBlue) returnCommand = PIDPoseAlign(drive, poseSubsystem, AutoScoreCommandConstants.processorPoseBlue) return returnCommand } - fun moveToProcessorCommandRed() : Command { + fun moveToProcessorCommandRed(): Command { var returnCommand = PIDPoseAlign(drive, poseSubsystem, AutoScoreCommandConstants.processorPoseRed) returnCommand = PIDPoseAlign(drive, poseSubsystem, AutoScoreCommandConstants.processorPoseRed) return returnCommand @@ -98,16 +94,16 @@ class AutoScoreCommands ( * @param isAtTopSource a boolean representing if we're intaking from the top or the bottom source. True if top, false if bottom. */ - fun moveToCoralIntakeCommandBlue(isAtTopSource : Boolean) : Command { + fun moveToCoralIntakeCommandBlue(isAtTopSource: Boolean): Command { var returnCommand = PIDPoseAlign(drive, poseSubsystem, AutoScoreCommandConstants.coralIntakePoseBlueTop) - if(!isAtTopSource) { + if (!isAtTopSource) { returnCommand = PIDPoseAlign(drive, poseSubsystem, AutoScoreCommandConstants.coralIntakePoseBlueBottom) } return returnCommand } - fun moveToCoralIntakeCommandRed(isAtTopSource : Boolean) : Command { + fun moveToCoralIntakeCommandRed(isAtTopSource: Boolean): Command { var returnCommand = PIDPoseAlign(drive, poseSubsystem, AutoScoreCommandConstants.coralIntakePoseRedTop) - if(!isAtTopSource) { + if (!isAtTopSource) { returnCommand = PIDPoseAlign(drive, poseSubsystem, AutoScoreCommandConstants.coralIntakePoseRedBottom) } return returnCommand @@ -121,9 +117,9 @@ class AutoScoreCommands ( * drive. * @param onRedAllianceSide a boolean representing which side of the field we're on. If true, the robot moves to the red alliance side to score net. */ - fun moveToNetCommand(onRedAllianceSide: Boolean) : Command { + fun moveToNetCommand(onRedAllianceSide: Boolean): Command { var netPose = Pose2d(Translation2d(AutoScoreCommandConstants.centerOfField - AutoScoreCommandConstants.netTranslationDistance, poseSubsystem.pose.y), AutoScoreCommandConstants.netRotation2dBlue) - if(onRedAllianceSide) { + if (onRedAllianceSide) { netPose = Pose2d(Translation2d(AutoScoreCommandConstants.centerOfField + AutoScoreCommandConstants.netTranslationDistance, poseSubsystem.pose.y), AutoScoreCommandConstants.netRotation2dBlue) } return PIDPoseAlign(drive, poseSubsystem, netPose) @@ -133,9 +129,6 @@ class AutoScoreCommands ( * intakes coral from coral intake * */ fun intakeCoralCommand() { - //we don't have coral intake yet, just setting up. + // we don't have coral intake yet, just setting up. } - - - -} \ No newline at end of file +} diff --git a/src/main/kotlin/frc/team449/subsystems/light/Light.kt b/src/main/kotlin/frc/team449/subsystems/light/Light.kt index ea1c5c6..eefa5a4 100644 --- a/src/main/kotlin/frc/team449/subsystems/light/Light.kt +++ b/src/main/kotlin/frc/team449/subsystems/light/Light.kt @@ -1,8 +1,5 @@ package frc.team449.subsystems.light -import edu.wpi.first.networktables.BooleanSubscriber -import edu.wpi.first.networktables.NetworkTable -import edu.wpi.first.networktables.NetworkTableInstance import edu.wpi.first.wpilibj.AddressableLED import edu.wpi.first.wpilibj.AddressableLEDBuffer import edu.wpi.first.wpilibj.RobotBase