Add trims and get reef position

.vscode/settings.json

@@ -56,5 +56,6 @@
     "edu.wpi.first.math.proto.*",
     "edu.wpi.first.math.**.proto.*",
     "edu.wpi.first.math.**.struct.*",
-  ]
+  ],
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
 }

src/main/java/frc4388/robot/Constants.java

@@ -52,6 +52,7 @@ import edu.wpi.first.units.measure.Distance;
 import frc4388.utility.CanDevice;
 import frc4388.utility.Gains;
 import frc4388.utility.LEDPatterns;
+import frc4388.utility.ReefPositionHelper;
 import frc4388.utility.Trim;
 
 /**
@@ -103,7 +104,7 @@ public final class Constants {
         public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270
         // public static final double FORWARD_OFFSET = 0; // 0, 90, 180, 270
 
-        public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 1.d, 10);
+        // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
 
         private static final class ModuleSpecificConstants {
             //Front Left
@@ -199,11 +200,14 @@ public final class Constants {
             public static final Gains XY_GAINS = new Gains(3,0,0);
             public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
 
+            public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.05, 0);
+
                     
             public static final double XY_TOLERANCE = 0.05;
             public static final double ROT_TOLERANCE = 1;
                     
-            public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
+            // public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
+            // public static final Pose2d targetpos = 
         }
     
     
@@ -311,7 +315,9 @@ public final class Constants {
 
     public static final class FieldConstants {
         public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2025Reefscape.loadAprilTagLayoutField();
-        public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(13);
+        public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5);
+        public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20);
+
 
 
         // Test april tag field layout

src/main/java/frc4388/robot/RobotContainer.java

@@ -42,7 +42,9 @@ import frc4388.robot.subsystems.SwerveDrive;
 
 // Utilites
 import frc4388.utility.DeferredBlock;
+import frc4388.utility.ReefPositionHelper;
 import frc4388.utility.Subsystem;
+import frc4388.utility.ReefPositionHelper.Side;
 import frc4388.utility.configurable.ConfigurableString;
 
 /**
@@ -165,6 +167,15 @@ public class RobotContainer {
         // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
         //     .onTrue(new InstantCommand(() -> SwerveDriveConstants.POINTLESS_TRIM.load()));
 
+        
+        new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
+            .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp()));
+
+        new Trigger(() -> getDeadbandedDriverController().getPOV() == 180)
+            .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
+
+            
+
         // ! /* Speed */
         new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
             .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
@@ -181,7 +192,7 @@ public class RobotContainer {
         // ?  /* Operator Buttons */
 
         new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
-            .onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos));
+            .onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision));
         
         new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
             .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); 
@@ -237,7 +248,7 @@ public class RobotContainer {
      */
     public Command getAutonomousCommand() {
         //return autoPlayback;
-        return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos);
+        return new GotoPositionCommand(m_robotSwerveDrive, m_vision);
     }
 
     /**

src/main/java/frc4388/robot/commands/GotoPositionCommand.java

@@ -9,6 +9,8 @@ import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.subsystems.Vision;
 import frc4388.utility.Gains;
+import frc4388.utility.ReefPositionHelper;
+import frc4388.utility.ReefPositionHelper.Side;
 import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
 import frc4388.utility.UtilityStructs.AutoRecordingFrame;
 import frc4388.utility.controller.VirtualController;
@@ -32,10 +34,9 @@ public class GotoPositionCommand extends Command {
      * @param SwerveDrive m_robotSwerveDrive
      */
 
-    public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos) {
+    public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) {
         this.swerveDrive = swerveDrive;
         this.vision = vision;
-        this.targetpos = targetpos;
         addRequirements(swerveDrive);
     }
 
@@ -43,6 +44,7 @@ public class GotoPositionCommand extends Command {
     public void initialize() {
         xPID.initialize();
         yPID.initialize();
+        this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.RIGHT, AutoConstants.X_OFFSET_TRIM.get());
     }
     
     double xerr;

src/main/java/frc4388/robot/subsystems/Vision.java

@@ -40,7 +40,8 @@ public class Vision extends Subsystem {
 
     private PhotonCamera camera;
 
-    private boolean isTag = false;
+    private boolean isTagDetected = false;
+    private boolean isTagProcessed = false;
     private Pose2d lastVisionPose = new Pose2d();
     private Pose2d lastPhysOdomPose = new Pose2d();
 
@@ -54,10 +55,15 @@ public class Vision extends Subsystem {
     .getLayout(getSubsystemName(), BuiltInLayouts.kList)
     .withSize(2, 2);
 
-    GenericEntry sbTag = subsystemLayout
+    GenericEntry sbTagDetected = subsystemLayout
     .add("Tag Detected", false)
     .withWidget(BuiltInWidgets.kBooleanBox)
     .getEntry();
+
+    GenericEntry sbTagProcessed = subsystemLayout
+    .add("Tag Processed", false)
+    .withWidget(BuiltInWidgets.kBooleanBox)
+    .getEntry();
     
     GenericEntry sbCamConnected = subsystemLayout
     .add("Camera Connnected", false)
@@ -78,7 +84,8 @@ public class Vision extends Subsystem {
         // var results = camera.getAllUnreadResults();
         // if (results.size() == 0) return;
         // var result = results.get(results.size()-1);
-        isTag = result.hasTargets();
+        isTagDetected = result.hasTargets();
+        isTagProcessed = false;
         
         // Optional<MultiTargetPNPResult> multitag = result.getMultiTagResult();
 
@@ -94,8 +101,8 @@ public class Vision extends Subsystem {
 
         // System.out.println(isTag);
 
-        if(!isTag){
+        if(!isTagDetected){
-            sbTag.setBoolean(isTag);
+            // sbTagDetected.setBoolean(isTagDetected);
             field.setRobotPose(getPose2d());
             return;
         }
@@ -104,12 +111,13 @@ public class Vision extends Subsystem {
 
         // In case the pose estimator fails to estimate the pose, fallback to physical odometry.
         if(EstimatedRobotPose.isEmpty()){
-            isTag = false;
+            // sbTagProcessed.setBoolean(isTagProcessed);
-            sbTag.setBoolean(isTag);
+            field.setRobotPose(lastVisionPose);
-            field.setRobotPose(getPose2d());
             return;
         }
 
+        isTagProcessed = true;
+
         lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d();
         // lastVisionPose.rotateBy(Rotation2d.k180deg);
         // lastVisionPose = new Pose2d(
@@ -234,7 +242,7 @@ public class Vision extends Subsystem {
     }
 
     public Pose2d getPose2d() {
-        if(isTag)
+        if(isTagDetected && isTagProcessed)
             return lastVisionPose;
         else
             return lastPhysOdomPose;
@@ -245,7 +253,7 @@ public class Vision extends Subsystem {
     }
 
     public boolean isTag(){
-        return isTag;
+        return isTagDetected && isTagProcessed;
     }
 
 
@@ -271,7 +279,8 @@ public class Vision extends Subsystem {
 
     @Override
     public void queryStatus() {
-        sbTag.setBoolean(isTag);
+        sbTagDetected.setBoolean(isTagDetected);
+        sbTagProcessed.setBoolean(isTagProcessed);
         sbCamConnected.setBoolean(camera.isConnected());
         // field.setRobotPose(getPose2d());
     }

src/main/java/frc4388/utility/ReefPositionHelper.java

@@ -15,7 +15,7 @@ public class ReefPositionHelper {
         RIGHT
     }
 
-    public static Pose2d[] RED_TAGS = {
+    public static final Pose2d[] RED_TAGS = {
         FieldConstants.kTagLayout.getTagPose(6).get().toPose2d(),
         FieldConstants.kTagLayout.getTagPose(7).get().toPose2d(),
         FieldConstants.kTagLayout.getTagPose(8).get().toPose2d(),
@@ -24,7 +24,7 @@ public class ReefPositionHelper {
         FieldConstants.kTagLayout.getTagPose(11).get().toPose2d()
     };
 
-    public static Pose2d[] BLUE_TAGS = {
+    public static final Pose2d[] BLUE_TAGS = {
         FieldConstants.kTagLayout.getTagPose(17).get().toPose2d(),
         FieldConstants.kTagLayout.getTagPose(18).get().toPose2d(),
         FieldConstants.kTagLayout.getTagPose(19).get().toPose2d(),
@@ -34,7 +34,7 @@ public class ReefPositionHelper {
     };
 
     public static double distanceTo(Pose2d first, Pose2d second){
-        return Math.sqrt(  Math.pow(first.getX()  -  second.getX(),2) + Math.pow(first.getY()  -  second.getY(),2));
+        return Math.sqrt(Math.pow(first.getX()  -  second.getX(),2) + Math.pow(first.getY()  -  second.getY(),2));
     }
 
 
@@ -48,13 +48,16 @@ public class ReefPositionHelper {
         double minDistance = distanceTo(position,minPos);
 
         for(int i = 1; i < locations.length; i++){
-            double dist = distanceTo(locations[i],minPos);
+            double dist = distanceTo(locations[i],position);
             if(dist < minDistance){
+                System.out.println(i);
                 minPos = locations[i];
                 minDistance = dist;
             }
         }
 
+        System.out.println(minPos);
+
         return minPos;
     }
 
@@ -73,16 +76,20 @@ public class ReefPositionHelper {
     }
 
     public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim) {
-        return getNearestTag(horisontalOffset(position, side == Side.LEFT ? -(xtrim+FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET) : (xtrim+FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET))); 
+        return offset(getNearestTag(position), 
+        (side == Side.LEFT ? -(FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET) : (FieldConstants.HORISONTAL_SCORING_POSITION_OFFSET)) + xtrim,
+        FieldConstants.VERTICAL_SCORING_POSITION_OFFSET); 
     }
 
 
-    public static Pose2d horisontalOffset(Pose2d oldPose, double xoffset){
+    public static Pose2d offset(Pose2d oldPose, double xoffset, double yoffset){
         Translation2d oldTranslation = oldPose.getTranslation();
-        Rotation2d rot = oldPose.getRotation();
+        
+        double rot = oldPose.getRotation().getRadians();
+
         return new Pose2d(new Translation2d(
-            oldTranslation.getX() + rot.getCos() * xoffset,
+            oldTranslation.getX() + Math.cos(rot + Math.PI/2) * xoffset + Math.cos(rot) * yoffset,
-            oldTranslation.getY() + rot.getSin() * xoffset
+            oldTranslation.getY() + Math.sin(rot + Math.PI/2) * xoffset + Math.sin(rot) * yoffset
         ), oldPose.getRotation());
     }
 }