fix getAmpCornerPose()

StuyPulse · Aug 28, 2024 · 505426b · 505426b
1 parent 87c6920
commit 505426b
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 10 deletions.
diff --git a/...java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeaker.java b/...java/com/stuypulse/robot/commands/swerve/driveAligned/SwerveDriveDriveAlignedSpeaker.java
@@ -4,11 +4,8 @@
 import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
 import com.stuypulse.stuylib.input.Gamepad;
 
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class SwerveDriveDriveAlignedSpeaker extends SwerveDriveDriveAligned{
 
@@ -20,8 +17,6 @@ public SwerveDriveDriveAlignedSpeaker(Gamepad driver) {
     protected Rotation2d getTargetAngle() {
         Translation2d currentPose = SwerveDrive.getInstance().getPose().getTranslation();
         Translation2d speakerPose = Field.getAllianceSpeakerPose().getTranslation();
-        SmartDashboard.putNumber("test/speakerPoseX", speakerPose.getX());
-        SmartDashboard.putNumber("test/speakerPoseY", speakerPose.getY());
         return currentPose.minus(speakerPose).getAngle();
     }
 

diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/src/main/java/com/stuypulse/robot/constants/Field.java
@@ -277,11 +277,9 @@ public static Translation2d getManualFerryPosition() {
     }
 
     public static Translation2d getAmpCornerPose() {
-        Translation2d targetPose = Robot.isBlue()
-            ? new Translation2d(0.0, Field.WIDTH - 1.5)
-            : new Translation2d(0.0, 1.5);
-
-        return targetPose;
+        return Robot.isBlue()
+            ? new Translation2d(2.0, WIDTH - 1.25)
+            : new Translation2d(LENGTH - 2.0, WIDTH - 1.25);
     }
 
     // MIDLINE: 8.27