Update

.vscode/settings.json

@@ -58,5 +58,6 @@
     "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",
-  "wpilib.autoStartRioLog": false
+  "wpilib.autoStartRioLog": false,
+  "java.debug.settings.onBuildFailureProceed": true
 }

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

@@ -37,12 +37,6 @@ import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import frc4388.utility.controller.VirtualController;
 import frc4388.robot.commands.MoveForTimeCommand;
 import frc4388.robot.commands.MoveUntilSuply;
-import frc4388.robot.commands.alignment.DriveToReef;
-import frc4388.robot.commands.alignment.DriveUntilLiDAR;
-import frc4388.robot.commands.alignment.LidarAlign;
-import frc4388.robot.commands.wait.waitElevatorRefrence;
-import frc4388.robot.commands.wait.waitEndefectorRefrence;
-import frc4388.robot.commands.wait.waitFeedCoral;
 import frc4388.robot.commands.wait.waitSupplier;
 import frc4388.robot.constants.Constants;
 import frc4388.robot.constants.Constants.AutoConstants;
@@ -55,15 +49,12 @@ import com.pathplanner.lib.commands.PathPlannerAuto;
 
 // Subsystems
 import frc4388.robot.subsystems.LED;
-import frc4388.robot.subsystems.elevator.Elevator;
-import frc4388.robot.subsystems.elevator.Elevator.CoordinationState;
 import frc4388.robot.subsystems.lidar.LiDAR;
 import frc4388.robot.subsystems.swerve.SwerveDrive;
 import frc4388.robot.subsystems.vision.Vision;
 // Utilites
 import frc4388.utility.DeferredBlock;
 import frc4388.utility.compute.TimesNegativeOne;
-import frc4388.utility.compute.ReefPositionHelper.Side;
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -80,14 +71,9 @@ public class RobotContainer {
     /* Subsystems */
     public final LED m_robotLED = new LED();
     public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera);
-    public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED);
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
     // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
 
-    public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef");
-    public final LiDAR reverseLidar = new LiDAR(m_robotMap.reverseLidar, "Reverse");
-
-
     /* Controllers */
     private final DeadbandedXboxController m_driverXbox   = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
     private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);

src/main/java/frc4388/robot/RobotMap.java

@@ -17,12 +17,9 @@ import com.ctre.phoenix6.hardware.CANcoder;
 import com.ctre.phoenix6.swerve.SwerveDrivetrain;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DigitalInput;
-import frc4388.robot.constants.Constants.ElevatorConstants;
 import frc4388.robot.constants.Constants.LiDARConstants;
 import frc4388.robot.constants.Constants.SimConstants;
 import frc4388.robot.constants.Constants.VisionConstants;
-import frc4388.robot.subsystems.elevator.ElevatorIO;
-import frc4388.robot.subsystems.elevator.ElevatorReal;
 import frc4388.robot.subsystems.lidar.LiDAR;
 import frc4388.robot.subsystems.lidar.LidarIO;
 import frc4388.robot.subsystems.lidar.LidarReal;
@@ -59,9 +56,6 @@ public class RobotMap {
     /* Swreve Drive Subsystem */
     public final SwerveIO swerveDrivetrain;
 
-    /* Elevator Subsystem */
-    public final ElevatorIO elevatorIO;
-
     public RobotMap(SimConstants.Mode mode) {
         switch (mode) {
             case REAL:
@@ -88,26 +82,9 @@ public class RobotMap {
 
                 swerveDrivetrain = new SwerveReal(swerveDrivetrainReal);
 
-                // Configure elevator
-
-                TalonFX elevator = new TalonFX(ElevatorConstants.ELEVATOR_ID.id);
-                TalonFX endeffector = new TalonFX(ElevatorConstants.ENDEFFECTOR_ID.id);
-                
-
-                DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
-                DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
-                DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH);
-
-                elevatorIO = new ElevatorReal(elevator, endeffector, basinLimitSwitch, endeffectorLimitSwitch, IRIntakeBeam);
-
-
-
                 // Fault
                 FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
                 
-                FaultTalonFX.addDevice(elevator, "Elevator");
-                FaultTalonFX.addDevice(endeffector, "Endeffector");
-                
                 FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive");
                 FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer");
                 FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder");
@@ -130,7 +107,6 @@ public class RobotMap {
                 reefLidar = new LidarIO() {};
                 reverseLidar = new LidarIO() {};
                 swerveDrivetrain = new SwerveIO() {};
-                elevatorIO = new ElevatorIO() {};
                 break;
         }
     }

src/main/java/frc4388/robot/commands/Autos/neoPlaybackChooser.java

@@ -1,4 +1,4 @@
-package frc4388.robot.commands.autos;
+package frc4388.robot.commands.Autos;
 // package frc4388.robot.commands.Autos;
 
 // import java.io.File;

src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java

@@ -2,7 +2,7 @@
 // Open Source Software; you can modify and/or share it under the terms of
 // the WPILib BSD license file in the root directory of this project.
 
-package frc4388.robot.commands.swerve;
+package frc4388.robot.commands.Swerve;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import frc4388.robot.commands.PID;

src/main/java/frc4388/robot/commands/alignment/DriveToReef.java

@@ -1,188 +0,0 @@
-package frc4388.robot.commands.alignment;
-
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.constants.Constants.AutoConstants;
-import frc4388.robot.subsystems.swerve.SwerveDrive;
-import frc4388.robot.subsystems.vision.Vision;
-import frc4388.utility.compute.ReefPositionHelper;
-import frc4388.utility.compute.TimesNegativeOne;
-import frc4388.utility.compute.ReefPositionHelper.Side;
-import frc4388.utility.structs.Gains;
-
-public class DriveToReef extends Command {
-    
-
-    // private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998);
-    // private Translation2d translation2d= new Translation2d(16.579342-0.15,5.547867999999999);
-
-    private PID xPID = new PID(AutoConstants.XY_GAINS, 0);
-    private PID yPID = new PID(AutoConstants.XY_GAINS, 0);
-    // private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
-    private Pose2d targetpos;
-
-    SwerveDrive swerveDrive;
-    Vision vision;
-    double distance;
-    Side side;
-    boolean waitVelocityZero;
-
-    /**
-     * Command to drive robot to position.
-     * @param SwerveDrive m_robotSwerveDrive
-     */
-
-    public DriveToReef(SwerveDrive swerveDrive, Vision vision, double distance, Side side, boolean waitVelocityZero) {
-        this.swerveDrive = swerveDrive;
-        this.vision = vision;
-        this.distance = distance;
-        this.side = side;
-        this.waitVelocityZero = waitVelocityZero && false;
-        addRequirements(swerveDrive);
-    }
-
-
-    public static double tagRelativeXError = -1;
-    private static void setTagRelativeXError(double val){
-        tagRelativeXError = val;
-    }
-
-    @Override
-    public void initialize() {
-        xPID.initialize();
-        yPID.initialize();
-        this.targetpos = ReefPositionHelper.getNearestPosition(
-            this.vision.getPose2d(), 
-            side, 
-            Units.inchesToMeters(AutoConstants.X_OFFSET_TRIM.get()), 
-            distance + Units.inchesToMeters(AutoConstants.Y_OFFSET_TRIM.get())
-        );
-    }
-    
-    double xerr;
-    double yerr;
-    double roterr;
-
-    double xoutput;
-    double youtput;
-    double rotoutput;
-
-    @Override
-    public void execute() {
-        xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
-        yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis);
-        // xerr = targetpos.getX() - vision.getPose2d().getX();
-        // yerr = targetpos.getX() - vision.getPose2d().getY();
-
-        // roterr = TimesNegativeOne.invert(targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(), TimesNegativeOne.isRed);
-
-        roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees()));
-
-        if(roterr > 180){
-            roterr -= 360;
-        }else if(roterr < -180){
-            roterr += 360;
-        }
-
-        // SmartDashboard.putNumber("Rotational PID target", targetpos.getRotation().getDegrees());
-        // SmartDashboard.putNumber("Rotational PID position", vision.getPose2d().getRotation().getDegrees());
-        // SmartDashboard.putNumber("Rotational PID error", roterr);
-
-        SmartDashboard.putNumber("PID X Error", xerr);
-        SmartDashboard.putNumber("PID Y Error", yerr);
-        SmartDashboard.putNumber("PID Rot Error", roterr);
-
-        xoutput = xPID.update(xerr);
-        youtput = yPID.update(yerr);
-        // rotoutput = rotPID.update(roterr);
-
-        xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
-        youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
-        // rotoutput *= Math.abs(roterr) < AutoConstants.ROT_TOLERANCE ? 0 : 1;
-        
-
-
-        Translation2d leftStick = new Translation2d(
-            Math.max(Math.min(-youtput, 1), -1),
-            Math.max(Math.min(-xoutput, 1), -1)
-            // 0,0
-        );
-
-        // Translation2d rightStick = new Translation2d(
-        //     Math.max(Math.min(rotoutput, 1), -1), 
-        //    0
-        // );
-
-        setTagRelativeXError(
-            new Translation2d(xerr, yerr).getAngle()
-            .rotateBy(targetpos.getRotation())
-            .getCos());
-
-        swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation());
-        // swerveDrive.driveWithInputOrientation(leftStick, rightStick, true);
-    }
-
-    @Override
-    public final boolean isFinished() {
-        boolean finished = (
-            (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
-            (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
-            (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) &&
-            (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
-        );
-        // System.out.println(finished);
-
-        if(finished)
-            swerveDrive.softStop();
-
-        return finished;
-                // this statement is a boolean in and of itself
-    }
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-    private class PID {
-        protected Gains  gains;
-        private   double output    = 0;
-
-
-        /** Creates a new PelvicInflammatoryDisease. */
-        public PID(Gains gains, double tolerance) {
-            this.gains     = gains;
-        }
-
-        // Called when the command is initially scheduled.
-        public final void initialize() {
-            output = 0;
-        }
-
-        private double prevError, cumError = 0;
-        
-        // Called every time the scheduler runs while the command is scheduled.
-        public double update(double error) {
-            cumError += error * .02; // 20 ms
-            double delta = error - prevError;
-
-            output = error * gains.kP;
-            output += cumError * gains.kI;
-            output += delta * gains.kD;
-            output += gains.kF;
-
-            return output;
-        }
-    }
-}

src/main/java/frc4388/robot/commands/alignment/DriveUntilLiDAR.java

@@ -1,48 +0,0 @@
-package frc4388.robot.commands.alignment;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.lidar.LiDAR;
-import frc4388.robot.subsystems.swerve.SwerveDrive;
-
-// Command to repeat a joystick movement for a specific time.
-public class DriveUntilLiDAR extends Command {
-    private final SwerveDrive swerveDrive;
-    private final Translation2d leftStick;
-    private final Translation2d rightStick;
-    private final LiDAR m_lidar;
-    private final double mindistance;
-
-    public DriveUntilLiDAR(
-        SwerveDrive swerveDrive, 
-        Translation2d leftStick, 
-        Translation2d rightStick, 
-        LiDAR lidar,
-        double mindistance) {
-            addRequirements(swerveDrive);
-
-        this.swerveDrive = swerveDrive;
-        this.leftStick = leftStick;
-        this.rightStick = rightStick;
-        this.m_lidar = lidar;
-        this.mindistance = mindistance;
-    }
-
-    @Override
-    public void initialize() {
-    }
-
-    @Override
-    public void execute() {
-        swerveDrive.driveFine(leftStick, rightStick, 0.3);
-    }
-
-    @Override
-    public boolean isFinished() {
-        if (Math.abs(m_lidar.getDistance()) < mindistance) {
-            swerveDrive.softStop();
-            return true;
-        }
-        return false;
-    }
-}

src/main/java/frc4388/robot/commands/alignment/LidarAlign.java

@@ -1,107 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.commands.alignment;
-
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.lidar.LiDAR;
-import frc4388.robot.subsystems.swerve.SwerveDrive;
-
-/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
-public class LidarAlign extends Command {
-  private SwerveDrive swerveDrive;
-  private LiDAR lidar;  
-
-  private int currentFinderTick;
-  // private int tickFoundPipe;
-  private boolean foundReef;
-  private boolean headedRight;
-  private double speed;
-  private int bounces;
-  private double additionalDistance = 0;
-  // private final boolean constructedHeadedRight;
-
-  /** Creates a new LidarAlign. */
-  public LidarAlign(SwerveDrive swerveDrive, LiDAR lidar) {//, boolean headedRight) {
-    // Use addRequirements() here to declare subsystem dependencies.
-
-    this.swerveDrive = swerveDrive;
-    this.lidar = lidar;
-
-    addRequirements(swerveDrive, lidar);
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {
-    this.currentFinderTick = 0;
-    this.speed = 0.4; // TODO: find good speed for this
-    this.foundReef = false;
-    this.headedRight = (DriveToReef.tagRelativeXError < 0);
-    this.additionalDistance = 0;
-    this.bounces = 0;
-  }
-
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {
-    if (lidar.withinDistance()) {
-      swerveDrive.softStop(); 
-      foundReef = true;
-      return;
-    }
-
-    if (currentFinderTick > (15 + additionalDistance)) { //arbutrary threshhold for now.
-      headedRight = !headedRight;
-      currentFinderTick *= -1;
-      bounces++;
-      additionalDistance += 5;
-      if (bounces == 5) return;
-    }
-    double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI;
-    double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef
-    SmartDashboard.putNumber("Rel Angle", relAngle);
-    SmartDashboard.putNumber("heading", currentHeading);
-    if (!headedRight) {
-      swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle));
-    } else {
-      swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle));
-    }
-
-    currentFinderTick++;
-  }
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {
-
-  }
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    if (lidar.getDistance() < 4) {
-      swerveDrive.stopModules();
-      return true;
-    } else if (foundReef && lidar.withinDistance()) { // spot on
-      swerveDrive.stopModules();
-      return true;
-    } else if (foundReef && !lidar.withinDistance()) { // over shot
-      speed = speed / 2;
-      headedRight = !headedRight;
-      currentFinderTick = 0;
-      foundReef = false;
-      return false;
-    } else if (bounces >= 3) {
-      swerveDrive.stopModules();
-      return true;
-    } else {
-      return false;
-    }
-  }
-}

src/main/java/frc4388/robot/constants/BuildConstants.java

@@ -5,14 +5,14 @@ package frc4388.robot.constants;
  */
 public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
-  public static final String MAVEN_NAME = "2025RidgeScape";
+  public static final String MAVEN_NAME = "Robot-Essentials";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 173;
+  public static final int GIT_REVISION = 123;
-  public static final String GIT_SHA = "2b7bb1224195b9e001e60b895ee04d63abdfc513";
+  public static final String GIT_SHA = "56841636f984f1a691134b4f1ae8862353846b54";
-  public static final String GIT_DATE = "2025-07-17 09:15:19 MDT";
+  public static final String GIT_DATE = "2025-10-30 16:40:04 MDT";
-  public static final String GIT_BRANCH = "advantagekit";
+  public static final String GIT_BRANCH = "calibration-system";
-  public static final String BUILD_DATE = "2025-07-17 11:55:31 MDT";
+  public static final String BUILD_DATE = "2025-10-30 17:18:25 MDT";
-  public static final long BUILD_UNIX_TIME = 1752774931371L;
+  public static final long BUILD_UNIX_TIME = 1761866305954L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

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

@@ -69,6 +69,7 @@ public final class Constants {
         public static final double MIN_ROT_PID_OUTPUT = 0.0;
 
         public static final double VELOCITY_THRESHHOLD = 0.01;
+        public static final double STOP_VELOCITY = 2;
     }
   
 

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

@@ -1,106 +0,0 @@
-package frc4388.utility.compute;
-
-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.math.util.Units;
-import frc4388.robot.constants.Constants.AutoConstants;
-import frc4388.robot.constants.Constants.FieldConstants;
-
-public class ReefPositionHelper {
-    public enum Side {
-        LEFT,
-        RIGHT,
-        CENTER,
-        FAR_LEFT
-    }
-
-    public static final Pose2d[] RED_TAGS = {
-        FieldConstants.kTagLayout.getTagPose(6).get().toPose2d(),
-        FieldConstants.kTagLayout.getTagPose(7).get().toPose2d(),
-        FieldConstants.kTagLayout.getTagPose(8).get().toPose2d(),
-        FieldConstants.kTagLayout.getTagPose(9).get().toPose2d(),
-        FieldConstants.kTagLayout.getTagPose(10).get().toPose2d(),
-        FieldConstants.kTagLayout.getTagPose(11).get().toPose2d()
-    };
-
-    public static final Pose2d[] BLUE_TAGS = {
-        FieldConstants.kTagLayout.getTagPose(17).get().toPose2d(),
-        FieldConstants.kTagLayout.getTagPose(18).get().toPose2d(),
-        FieldConstants.kTagLayout.getTagPose(19).get().toPose2d(),
-        FieldConstants.kTagLayout.getTagPose(20).get().toPose2d(),
-        FieldConstants.kTagLayout.getTagPose(21).get().toPose2d(),
-        FieldConstants.kTagLayout.getTagPose(22).get().toPose2d()
-    };
-
-    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));
-    }
-
-
-    /*
-    * Function to loop through a list of tag locations to figure out closest one
-    */
-    public static Pose2d getNearestTag(Pose2d[] locations, Pose2d position){
-        if(locations.length <= 0) return new Pose2d();
-
-        Pose2d minPos = locations[0];
-        double minDistance = distanceTo(position,minPos);
-
-        for(int i = 1; i < locations.length; i++){
-            double dist = distanceTo(locations[i],position);
-            if(dist < minDistance){
-                minPos = locations[i];
-                minDistance = dist;
-            }
-        }
-        
-        System.out.println(minPos.getRotation().getDegrees());
-
-        return minPos;
-    }
-
-    /*
-     * Function to find closest tag location based on side
-     */
-    public static Pose2d getNearestTag(Pose2d position) {
-
-        if(TimesNegativeOne.isRed)
-            return getNearestTag(RED_TAGS, position);
-        else
-            return getNearestTag(BLUE_TAGS, position);
-    }
-
-    public static Pose2d getNearestPosition(Pose2d position, Side side, double xtrim, double ydistance) {
-        return offset(getNearestTag(position), 
-        getSide(side) + xtrim,
-        ydistance); 
-    }
-
-    public static double getSide(Side side){
-        switch(side) {
-            case LEFT:
-                return -(AutoConstants.X_SCORING_POSITION_OFFSET);
-            case FAR_LEFT:
-                return -(AutoConstants.X_SCORING_POSITION_OFFSET+Units.inchesToMeters(8));
-            case RIGHT:
-                return (AutoConstants.X_SCORING_POSITION_OFFSET);
-            case CENTER:
-                return 0;
-        }
-        assert false;
-        return 0;
-    }
-
-
-    public static Pose2d offset(Pose2d oldPose, double xoffset, double yoffset){
-        Translation2d oldTranslation = oldPose.getTranslation();
-        
-        double rot = oldPose.getRotation().getRadians();
-
-        return new Pose2d(new Translation2d(
-            oldTranslation.getX() + Math.cos(rot + Math.PI/2) * xoffset + Math.cos(rot) * yoffset,
-            oldTranslation.getY() + Math.sin(rot + Math.PI/2) * xoffset + Math.sin(rot) * yoffset
-        ), oldPose.getRotation().rotateBy(Rotation2d.k180deg));
-    }
-}