Odometry Added

Author: falOn-Dev

src/main/java/frc/robot/RobotContainer.kt

@@ -23,6 +23,7 @@ import frc.robot.commands.intake.ToggleIntakeCommand
 import frc.robot.commands.launcher.*
 import frc.robot.commands.pivot.*
 import frc.robot.commands.swerve.*
+import frc.robot.commands.vision.UpdateOdometryCommand
 import frc.robot.subsystems.ClimbSubsystem
 import frc.robot.subsystems.IntakeSubsystem
 import frc.robot.subsystems.PivotSubsystem
@@ -162,15 +163,11 @@ object RobotContainer {
         controller.rightBumper().toggleOnTrue(manualClimb)
         controller.rightStick().onTrue(InstantCommand(SwerveSubsystem::toggleFieldOriented))
         controller.button(Constants.OperatorConstants.BACK_BUTTON)
-            .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
+            .onTrue(UpdateOdometryCommand()) // 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