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);