It works

Team2537 · Mar 15, 2024 · 5c71425 · 5c71425
1 parent cb60105
commit 5c71425
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 10 deletions.
diff --git 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
@@ -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")
     }
 }