From 0bd71b594d8bdd8260f2494aa67f057515380c56 Mon Sep 17 00:00:00 2001
From: cherriae <zeruiz@nycstudents.net>
Date: Thu, 14 Mar 2024 14:16:25 -0400
Subject: [PATCH] added auto aim back

---
 src/main/java/frc/robot/Constants.java                       | 4 ++--
 src/main/java/frc/robot/RobotContainer.java                  | 4 ++--
 src/main/java/frc/robot/commands/shooter/AutonShoot.java     | 2 +-
 src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java | 3 ++-
 src/main/java/frc/robot/subsystems/VisionSubsystem.java      | 2 +-
 5 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 1aff700..7dd2d95 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -160,8 +160,8 @@ public static class FieldConstants {
     // FIELD WIDTH: 651.223 INCHES
 
     // x: .2472 y: 5.5946 (USE THIS)
-    public static final Pose3d SPEAKER_POSE_BLUE = new Pose3d(0.25, 5.59, SPEAKER_HEIGHT, new Rotation3d(0, 0, 180));
-    public static final Pose3d SPEAKER_POSE_RED = new Pose3d(16.29, 5.59, SPEAKER_HEIGHT, new Rotation3d(0, 0, 0)); // TODO: fix X here
+    public static final Pose3d SPEAKER_POSE_BLUE = new Pose3d(0.17, 5.3, SPEAKER_HEIGHT, new Rotation3d(0, 0, 180));
+    public static final Pose3d SPEAKER_POSE_RED = new Pose3d(16.37, 5.3, SPEAKER_HEIGHT, new Rotation3d(0, 0, 0)); // TODO: fix X here
   }
 
   public static class LEDColors {
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 73b64fd..429a393 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -150,7 +150,7 @@ private void configureBindings() {
     // driver bindings
     _driveController.L1().onTrue(Commands.runOnce(_swerveSubsystem::toggleSpeed, _swerveSubsystem));
     _driveController.R1().onTrue(Commands.runOnce(() -> _swerveSubsystem.fieldOriented = !_swerveSubsystem.fieldOriented, _swerveSubsystem));
-    _driveController.triangle().onTrue(Commands.runOnce(_swerveSubsystem::resetGyro, _swerveSubsystem));
+    _driveController.triangle().onTrue(Commands.runOnce(() -> _swerveSubsystem.resetGyro(), _swerveSubsystem));
     _driveController.cross().whileTrue(new BrakeSwerve(_swerveSubsystem, _ledSubsystem));
 
     // TESTING ONLY!!!
@@ -172,7 +172,7 @@ private void configureBindings() {
       )
     );
 
-    _driveController.R2().whileTrue(
+    _operatorController.R2().whileTrue(
       new AutoAim(
         _shooterSubsystem,
         _elevatorSubsystem,
diff --git a/src/main/java/frc/robot/commands/shooter/AutonShoot.java b/src/main/java/frc/robot/commands/shooter/AutonShoot.java
index 3701d2a..8876076 100644
--- a/src/main/java/frc/robot/commands/shooter/AutonShoot.java
+++ b/src/main/java/frc/robot/commands/shooter/AutonShoot.java
@@ -46,7 +46,7 @@ public AutonShoot(
       new ParallelRaceGroup(
         new SpinShooter(shooter, ShooterState.SHOOT).withTimeout(1.5),
         new AutoAim(shooter, elevator, leds, swerve, Presets.CLOSE_SHOOTER_ANGLE, Presets.CLOSE_ELEVATOR_HEIGHT, this::headingPreset).withTimeout(2),
-        new FeedActuate(intake, FeedMode.INTAKE).withTimeout(1)
+        new FeedActuate(intake, FeedMode.INTAKE).withTimeout(4)
       ).onlyIf(() -> !_preloadShot).andThen(() -> _preloadShot = true),
 
       new FeedActuate(intake, FeedMode.OUTTAKE).withTimeout(1),
diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
index f9ce450..bbb9208 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -237,7 +237,8 @@ public double getDriveCoeff() {
   public void driveChassis(ChassisSpeeds chassisSpeeds) {
     // IMPORTANT: X-axis and Y-axis are flipped (based on wpilib coord system)
     if (fieldOriented) {
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds, getHeading());
+      double relativeHeading = getHeading().getDegrees() + (UtilFuncs.GetAlliance() == Alliance.Red ? 180 : 0);
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds, Rotation2d.fromDegrees(relativeHeading));
     }
 
     SwerveModuleState[] moduleStates = Constants.Physical.SWERVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds,
diff --git a/src/main/java/frc/robot/subsystems/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/VisionSubsystem.java
index e048fed..db82230 100644
--- a/src/main/java/frc/robot/subsystems/VisionSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/VisionSubsystem.java
@@ -73,7 +73,7 @@ public Optional<Pose2d> getBotpose() {
     int tags = (int) botpose_array[7];
     double distance = botpose_array[9];
 
-    if (tags < 2) return Optional.empty();
+    // if (tags < 2) return Optional.empty();
     if (distance > FieldConstants.TAG_DISTANCE_THRESHOLD) return Optional.empty();
 
     double botposeX = _xFilter.calculate(botpose_array[0]); // to get rid of the weird origin outlier