fixed shooting while moving backwards and forwards, and patched odome…

…try glitches
2491-NoMythic · Nov 21, 2024 · 76b2b19 · 76b2b19
1 parent e98822c
commit 76b2b19
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 3 deletions.
diff --git 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
@@ -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);