From 5c71425fa4a06110c701dfa8c33d1592e7bf212d Mon Sep 17 00:00:00 2001 From: Europa Date: Fri, 15 Mar 2024 15:43:35 -0400 Subject: [PATCH] It works --- src/main/java/frc/robot/RobotContainer.kt | 8 +++++++- .../robot/commands/pivot/QuickPivotCommand.kt | 16 +++++++--------- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.kt b/src/main/java/frc/robot/RobotContainer.kt index f346fd97..3634c2c7 100644 --- a/src/main/java/frc/robot/RobotContainer.kt +++ b/src/main/java/frc/robot/RobotContainer.kt @@ -2,6 +2,7 @@ package frc.robot import LauncherSubsystem import com.pathplanner.lib.auto.NamedCommands +import com.pathplanner.lib.util.GeometryUtil import edu.wpi.first.math.MathUtil import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab @@ -161,12 +162,17 @@ object RobotContainer { controller.rightBumper().toggleOnTrue(manualClimb) controller.rightStick().onTrue(InstantCommand(SwerveSubsystem::toggleFieldOriented)) controller.button(Constants.OperatorConstants.BACK_BUTTON) - .onTrue(PrintCommand("Toggle Between Absolute and Angular Velocity")) // TODO: Implement Toggle + .onTrue(Commands.runOnce( + { + SwerveSubsystem.resetOdometry(GeometryUtil.flipFieldPose(SwerveSubsystem.getPose())) + } + )) // TODO: Implement Toggle controller.button(Constants.OperatorConstants.START_BUTTON) .onTrue(Commands.runOnce({ SwerveSubsystem.resetOdometry(Constants.FIELD_LOCATIONS.SUBWOOFER_POSE) })) // TODO: Implement Intake Command Override + LauncherSubsystem.noteTrigger.and(controller.a()).onTrue(launchCommand) // TODO: Implement Priming diff --git a/src/main/java/frc/robot/commands/pivot/QuickPivotCommand.kt b/src/main/java/frc/robot/commands/pivot/QuickPivotCommand.kt index 8f6d27ce..d05bac2e 100644 --- a/src/main/java/frc/robot/commands/pivot/QuickPivotCommand.kt +++ b/src/main/java/frc/robot/commands/pivot/QuickPivotCommand.kt @@ -34,14 +34,17 @@ class QuickPivotCommand(target: Double, auto: Boolean, autoAim: Boolean) : Comma } override fun initialize() { - if(autoAim) { pose = SwerveSubsystem.getPose() if (DriverStation.getAlliance() == Optional.of(DriverStation.Alliance.Red)) { - speakerPose = GeometryUtil.flipFieldPose(speakerPose) + speakerPose = GeometryUtil.flipFieldPose(Constants.FIELD_LOCATIONS.SUBWOOFER_POSE) + } else { + speakerPose = Constants.FIELD_LOCATIONS.SUBWOOFER_POSE } xDistanceMeters = Math.abs(speakerPose.x - pose.x) + println(xDistanceMeters) + println(target) target = calculateAngle(xDistanceMeters.meters) } @@ -67,12 +70,7 @@ class QuickPivotCommand(target: Double, auto: Boolean, autoAim: Boolean) : Comma } override fun end(interrupted: Boolean) { - if (!this.auto) { - val hold = HoldTargetCommand(target) - hold.schedule() - } else { - pivotSubsystem.holdArm(target) - println("Auto Pivot Stopped") - } + pivotSubsystem.holdArm(target) + println("Auto Pivot Stopped") } }