From 76b2b194c8e14f547ecdf490519ce65cea113b93 Mon Sep 17 00:00:00 2001
From: Liam Sykes <2491nomythic@gmail.com>
Date: Wed, 20 Nov 2024 18:35:55 -0600
Subject: [PATCH] fixed shooting while moving backwards and forwards, and
 patched odometry glitches

---
 src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java | 2 +-
 src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java   | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java
index 274e4e7..4ee9e64 100644
--- a/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/AngleShooterSubsystem.java
@@ -172,7 +172,7 @@ public double calculateSpeakerAngle() {
 			offsetSpeakerX = Field.SHOOTER_RED_SPEAKER_X - targetOffset.getX();
 			offsetSpeakerY = Field.RED_SPEAKER_Y - targetOffset.getY();
 		} else {
-			offsetSpeakerX = Field.SHOOTER_BLUE_SPEAKER_X - targetOffset.getX();
+			offsetSpeakerX = Field.SHOOTER_BLUE_SPEAKER_X + targetOffset.getX();
 			offsetSpeakerY = Field.BLUE_SPEAKER_Y - targetOffset.getY();
 		}
 		double offsetDeltaX = Math.abs(dtvalues.getX() - offsetSpeakerX);
diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java
index f576cb1..c7b7b53 100644
--- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java
@@ -324,7 +324,7 @@ public void updateOdometryWithVision() {
 			}
 			if(!doRejectUpdate) {
 				Logger.recordOutput("Vision/MergesPose", estimate.pose);
-				odometer.addVisionMeasurement(new Pose2d(estimate.pose.getX(), estimate.pose.getY(), getGyroscopeRotation()), estimate.timestampSeconds);
+				odometer.addVisionMeasurement(estimate.pose, estimate.timestampSeconds);
 			}
 			RobotState.getInstance().LimelightsUpdated = true;
 		} else {
@@ -435,7 +435,7 @@ public double calculateSpeakerAngleMoving(){
 			correctionDirection = -1;
 			speakerX = Field.ROBOT_RED_SPEAKER_X;
 		}
-		offsetSpeakerX = speakerX+(targetOffset.getX()*correctionDirection);
+		offsetSpeakerX = speakerX-(targetOffset.getX()*correctionDirection);
 		offsetSpeakerY = speakerY+(targetOffset.getY()*correctionDirection);
 		offsetDeltaX = Math.abs(dtvalues.getX() - offsetSpeakerX);
 		offsetDeltaY = Math.abs(dtvalues.getY() - offsetSpeakerY);