Merge branch 'master' into autos-and-intake-fixes

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

@@ -16,6 +16,7 @@ import java.util.List;
 import java.util.function.BooleanSupplier;
 
 import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.GenericHID;
@@ -115,6 +116,7 @@ public class RobotContainer {
     private String lastAutoName = "defualt.auto";
     private SendableChooser<String> autoChooser;
     private Command autoCommand;
+
     // private ConfigurableString autoplaybackName = new ConfigurableString("Auto Playback Name", lastAutoName);
     // private neoJoystickPlayback autoPlayback = new neoJoystickPlayback(m_robotSwerveDrive, 
     // () -> autoplaybackName.get(), // lastAutoName
@@ -290,7 +292,12 @@ public class RobotContainer {
         new InstantCommand(() -> {m_robotSwerveDrive.endSlowPeriod();})
     );
 
+    private Command thrustIntake = new SequentialCommandGroup(
+        new MoveForTimeCommand(m_robotSwerveDrive, new Translation2d(0,-1), new Translation2d(), 100, true),
+        new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)
+    );
     
+    private Boolean operatorManualMode = false;
     
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
@@ -338,11 +345,12 @@ public class RobotContainer {
         }, m_robotSwerveDrive)
         .withName("SwerveDrive DefaultCommand"));
         m_robotSwerveDrive.setToSlow();
-        // m_robotElevator.setDefaultCommand(new RunCommand(() -> {
-        //     m_robotElevator.driveElevatorStick(m_operatorXbox.getRight());
-        // }, m_robotElevator)
-        // .withName("Elevator"));
 
+        m_robotElevator.setDefaultCommand(new RunCommand(() -> {
+            m_robotElevator.manualEndeffectorVel(getDeadbandedOperatorController().getLeftY());
+            m_robotElevator.manualElevatorVel(getDeadbandedOperatorController().getRightY());
+        }, m_robotElevator)
+        .withName("Default Manual Controls"));
 
         makeAutoChooser();
         SmartDashboard.putData("Auto Chooser", autoChooser);
@@ -387,98 +395,90 @@ public class RobotContainer {
      * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
      */
     private void configureButtonBindings() {
-
-        boolean driverPreferenceControls = false;
-
         // ? /* Driver Buttons */
 
-        if (!driverPreferenceControls) {
+        DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
 
-            DualJoystickButton(getDeadbandedDriverController(), getVirtualDriverController(), XboxController.A_BUTTON)
+        // ! /* Speed */
-                .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
+        new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
+            .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
         
-            // ! /* Speed */
+        new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
-            new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
-                .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
 
-            new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
+        new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON)
-                .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
+            .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.activateLuigiMode()));
         
-            new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON)
+        new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
-                .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.activateLuigiMode()));
+            .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.deactivateLuigiMode()));
 
-            new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
+        // While Left Trigger Pressed: Trims
-                .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.deactivateLuigiMode()));
+        new Trigger(() -> getDeadbandedDriverController().getPOV() == 0 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
+            .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp()));
 
-            new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
+        new Trigger(() -> getDeadbandedDriverController().getPOV() == 180  && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
-                .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
+            .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown()));
 
+        new Trigger(() -> getDeadbandedDriverController().getPOV() == 90 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
+            .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp()));
 
+        new Trigger(() -> getDeadbandedDriverController().getPOV() == 270 && getDeadbandedDriverController().getLeftTriggerAxis() > 0.8)
+            .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
         
+        // While Left Trigger NOT Pressed: Fine Alignment
+        new Trigger(() -> getDeadbandedDriverController().getPOV() != -1 && !(getDeadbandedDriverController().getLeftTriggerAxis() > 0.8))
+            .whileTrue(new RunCommand(
+                () -> m_robotSwerveDrive.driveFine(
+                    new Translation2d(
+                        1, 
+                        Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV())
+                    ), 
+                    getDeadbandedDriverController().getRight(), 0.15
+                ), m_robotSwerveDrive))
+            .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
         
-            new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
+        new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
-                .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepUp()));
+            .onTrue(thrustIntake.asProxy());
           
-            new Trigger(() -> getDeadbandedDriverController().getPOV() == 180)
+        new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
-                .onTrue(new InstantCommand(() -> AutoConstants.Y_OFFSET_TRIM.stepDown()));
+            .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar)); 
         
-            new Trigger(() -> getDeadbandedDriverController().getPOV() == 90)
-                .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepUp()));
-
-            new Trigger(() -> getDeadbandedDriverController().getPOV() == 270)
-                .onTrue(new InstantCommand(() -> AutoConstants.X_OFFSET_TRIM.stepDown()));
-
-
-
-
-            new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
-                .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive));
-
-        } else {
-
-        }
             
         // ?  /* Operator Buttons */
+        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator));
         
-        if (!driverPreferenceControls) {
+        DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator));
         
-            new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
+        // Button box
-                .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar)); 
 
+        new JoystickButton(getButtonBox(), ButtonBox.Five)
+            .onTrue(AprilLidarAlignL4Left);
         
-            DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
+        new JoystickButton(getButtonBox(), ButtonBox.One)
-                .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator));
+            .onTrue(AprilLidarAlignL4Right);
 
-            DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
+        new JoystickButton(getButtonBox(), ButtonBox.Six)
-                .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator));
+            .onTrue(AprilLidarAlignL3Left);
         
+        new JoystickButton(getButtonBox(), ButtonBox.Two)
+            .onTrue(AprilLidarAlignL3Right);
         
-            new JoystickButton(getButtonBox(), ButtonBox.Five)
+        new JoystickButton(getButtonBox(), ButtonBox.Seven)
-                .onTrue(AprilLidarAlignL4Left);
+            .onTrue(AprilLidarAlignL2Left);
         
-            new JoystickButton(getButtonBox(), ButtonBox.One)
+        new JoystickButton(getButtonBox(), ButtonBox.Three)
-                .onTrue(AprilLidarAlignL4Right);
+            .onTrue(AprilLidarAlignL2Right);
     
-            new JoystickButton(getButtonBox(), ButtonBox.Six)
+        // Lower algae removal
-                .onTrue(AprilLidarAlignL3Left);
+        new JoystickButton(getButtonBox(), ButtonBox.Eight)
+            .onTrue(leftAlgaeRemove);
         
-            new JoystickButton(getButtonBox(), ButtonBox.Two)
+        // Upper algae removal
-                .onTrue(AprilLidarAlignL3Right);
+        new JoystickButton(getButtonBox(), ButtonBox.Four)
-            
+            .onTrue(rightAlgaeRemove);
-            new JoystickButton(getButtonBox(), ButtonBox.Seven)
-                .onTrue(AprilLidarAlignL2Left);
-            
-            new JoystickButton(getButtonBox(), ButtonBox.Three)
-                .onTrue(AprilLidarAlignL2Right);
-            
-                
-            // Lower algae removal
-            new JoystickButton(getButtonBox(), ButtonBox.Eight)
-                .onTrue(leftAlgaeRemove);
-            
-            // Upper algae removal
-            new JoystickButton(getButtonBox(), ButtonBox.Four)
-                .onTrue(rightAlgaeRemove);
             
 
         // Cancel button
@@ -489,106 +489,58 @@ public class RobotContainer {
                 m_robotSwerveDrive.endSlowPeriod();
             }, m_robotElevator));
 
-            // Score prep        
+        // Manual Mode Buttons
-            // Prime 4
+        new Trigger(() -> (getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8 || getDeadbandedOperatorController().getRightTriggerAxis() >= 0.8) && operatorManualMode)
-            new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
+            .onTrue (new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator))
-                .onTrue(new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedFour), m_robotElevator));
+            .onFalse(new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringFour), m_robotElevator));
 
-            // Prime 3
+        new Trigger(() -> (getDeadbandedOperatorController().getLeftBumperButton() || getDeadbandedOperatorController().getRightBumperButton()) && operatorManualMode)
-            new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
+            .onTrue (new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator))
-                .onTrue(new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.PrimedThree), m_robotElevator));
+            .onFalse(new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.ScoringThree), m_robotElevator));
         
+        new Trigger(() -> (getDeadbandedOperatorController().getXButton() || getDeadbandedOperatorController().getYButton()) && operatorManualMode)
+            .onTrue (new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.L2Score), m_robotElevator));
         
+        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180 && operatorManualMode)
+            .onTrue (new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.BallRemoverL2Primed), m_robotElevator))
+            .onFalse(new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.BallRemoverL2Go), m_robotElevator));
         
-            //Trims
+        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0 && operatorManualMode)
-            new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
+            .onTrue (new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.BallRemoverL3Primed), m_robotElevator))
-                .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepUp()));
+            .onFalse(new InstantCommand(() ->  m_robotElevator.transitionState(CoordinationState.BallRemoverL3Go), m_robotElevator));
         
-            new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180)
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.START_BUTTON)
-                .onTrue(new InstantCommand(() -> AutoConstants.ELEVATOR_OFFSET_TRIM.stepDown()));
+            .onTrue(new InstantCommand(() -> {operatorManualMode = !operatorManualMode;}));
         
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.BACK_BUTTON)
+            .onTrue(new InstantCommand(() -> m_robotElevator.togggleAutoIntaking()));
 
-            new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90)
+        // Auto Scoring 
-                .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepUp()));
+        new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode)
+            .onTrue(AprilLidarAlignL4Left);
         
-            new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270)
+        new Trigger(() -> getDeadbandedOperatorController().getRightTriggerAxis() >= 0.8 && !operatorManualMode)
-                .onTrue(new InstantCommand(() -> AutoConstants.ARM_OFFSET_TRIM.stepDown()));
+            .onTrue(AprilLidarAlignL4Right);
 
-        } else {
+        new Trigger(() -> getDeadbandedOperatorController().getLeftBumperButton() && !operatorManualMode)
+            .onTrue(AprilLidarAlignL3Left);
         
-            // States: ready/waiting
+        new Trigger(() -> getDeadbandedOperatorController().getRightBumperButton() && !operatorManualMode)
-            DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.B_BUTTON)
+            .onTrue(AprilLidarAlignL3Right);
-                .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Waiting), m_robotElevator));
         
-            DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)
+        new Trigger(() -> getDeadbandedOperatorController().getXButton() && !operatorManualMode)
-                .onTrue(new InstantCommand(() -> m_robotElevator.transitionState(CoordinationState.Ready), m_robotElevator));
+            .onTrue(AprilLidarAlignL2Left);
         
-            //Controller Coral Scoring
+        new Trigger(() -> getDeadbandedOperatorController().getYButton() && !operatorManualMode)
-            new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8)
+            .onTrue(AprilLidarAlignL2Right);
-                .onTrue(AprilLidarAlignL4Left);
 
-            new Trigger(() -> getDeadbandedOperatorController().getLeftTriggerAxis() >= 0.8)
+        //Controller Lower Algae Removal
-                .onTrue(AprilLidarAlignL4Right);
+        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180 && !operatorManualMode)
+            .onTrue(leftAlgaeRemove);
 
-            new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
+        //Controller Upper Algae Removal
-                .onTrue(AprilLidarAlignL3Left);
+        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0 && !operatorManualMode)
-            
+            .onTrue(rightAlgaeRemove);
-            new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON)
-                .onTrue(AprilLidarAlignL3Right);
-            
-            new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
-                .onTrue(AprilLidarAlignL2Left);
-            
-            new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
-                .onTrue(AprilLidarAlignL2Right);
-
-            //Controller Lower Algae Removal
-            new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
-                .onTrue(leftAlgaeRemove);
-
-            //Controller Upper Algae Removal
-            new Trigger(() -> getDeadbandedOperatorController().getPOV() == 180)
-                .onTrue(rightAlgaeRemove);
-            
-            //Button Box Coral Scoring
-            new JoystickButton(getButtonBox(), ButtonBox.Five)
-                .onTrue(AprilLidarAlignL4Left);
-            
-            new JoystickButton(getButtonBox(), ButtonBox.One)
-                .onTrue(AprilLidarAlignL4Right);
-
-            new JoystickButton(getButtonBox(), ButtonBox.Six)
-                .onTrue(AprilLidarAlignL3Left);
-            
-            new JoystickButton(getButtonBox(), ButtonBox.Two)
-                .onTrue(AprilLidarAlignL3Right);
-            
-            new JoystickButton(getButtonBox(), ButtonBox.Seven)
-                .onTrue(AprilLidarAlignL2Left);
-            
-            new JoystickButton(getButtonBox(), ButtonBox.Three)
-                .onTrue(AprilLidarAlignL2Right);
-
-            // Button Box Lower algae removal
-            new JoystickButton(getButtonBox(), ButtonBox.Eight)
-                .onTrue(leftAlgaeRemove);
-            
-            // Button Box Upper algae removal
-            new JoystickButton(getButtonBox(), ButtonBox.Four)
-                .onTrue(rightAlgaeRemove);
-
-            // Cancel button
-            new JoystickButton(getButtonBox(), ButtonBox.White)
-                .onTrue(new InstantCommand(() -> {m_robotElevator.elevatorStop(); m_robotElevator.endeffectorStop();}, m_robotElevator));
-
-            //Manual Controls
-            m_robotElevator.setDefaultCommand(new RunCommand(() -> {
-                m_robotElevator.manualEndeffectorVel(getDeadbandedOperatorController().getLeftY());
-                m_robotElevator.manualElevatorVel(getDeadbandedOperatorController().getRightY());
-            }, m_robotElevator)
-            .withName("Default Manual Controls"));
-        
-        }
 
         // ? /* Programer Buttons (Controller 3)*/
 

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

@@ -6,6 +6,8 @@ package frc4388.robot.subsystems;
 
 import java.time.Instant;
 
+import org.opencv.ml.RTrees;
+
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.controls.PositionDutyCycle;
 import com.ctre.phoenix6.controls.PositionVoltage;
@@ -43,6 +45,8 @@ public class Elevator extends Subsystem {
   private boolean elevatorManualStop = true;
   private boolean endefectorManualStop = true;
 
+  private boolean disableAutoIntake = false;
+
   private DigitalInput basinBeamBreak;
   private DigitalInput endeffectorLimitSwitch;
 
@@ -200,6 +204,10 @@ public class Elevator extends Subsystem {
 
   }
 
+  public void togggleAutoIntaking() {
+    disableAutoIntake = !disableAutoIntake;
+  }
+
   public boolean elevatorAtReference() {
     // double elevatorRefrence = elevatorMotor.getClosedLoopReference().getValueAsDouble();
     double elevatorPosition = elevatorMotor.getPosition().getValueAsDouble();
@@ -256,6 +264,8 @@ public class Elevator extends Subsystem {
   public void manualElevatorVel(double velocity) {
     if (Math.abs(velocity) > 0.1) {
       elevatorMotor.set(velocity);
+      elevatorManualStop = false;
+      return;
     }
     if (!elevatorManualStop) {
       elevatorManualStop = true;
@@ -266,6 +276,8 @@ public class Elevator extends Subsystem {
   public void manualEndeffectorVel(double velocity) {
     if (Math.abs(velocity) > 0.1) {
       endeffectorMotor.set(velocity);
+      endefectorManualStop = false;
+      return;
     }
     if (!endefectorManualStop) {
       endefectorManualStop = true;
@@ -281,6 +293,8 @@ public class Elevator extends Subsystem {
     SmartDashboard.putNumber("endefector", endeffectorLimitSwitch.get() ? 1 : 0);
     SmartDashboard.putString("State", currentState.toString());
     
+    if (disableAutoIntake) return;
+
     if (currentState == CoordinationState.Waiting) {
       periodicWaiting();
     } else if (currentState == CoordinationState.WatingBeamTripped) {

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

@@ -51,6 +51,7 @@ public class SwerveDrive extends Subsystem {
 
     private int gear_index = SwerveDriveConstants.STARTING_GEAR;
     private boolean stopped = false;
+    private boolean robotKnowsWhereItIs = false;
 
     public double speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * SwerveDriveConstants.GEARS[gear_index];
     public double rotSpeedAdjust = SwerveDriveConstants.MAX_ROT_SPEED;
@@ -59,6 +60,7 @@ public class SwerveDrive extends Subsystem {
 
     public Pose2d initalPose2d = null;
 
+
     public double rotTarget = 0.0;
     public Rotation2d orientRotTarget = new Rotation2d();
     public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
@@ -170,24 +172,7 @@ public class SwerveDrive extends Subsystem {
                 SmartDashboard.putBoolean("drift correction", true);
             }
 
-            // // SmartDashboard.putBoolean("drift correction", true);
            
-            // // rot = ((rotTarget - gyro.getAngle()) / 360) *
-            // SwerveDriveConstants.ROT_CORRECTION_SPEED;
-
-            // }
-
-            // // Use the left joystick to set speed. Apply a cubic curve and the set max
-            // speed.
-            // Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
-            // // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00),
-            // Math.pow(speed.getY(), 3.00));
-
-            // // Convert field-relative speeds to robot-relative speeds.
-            // // chassisSpeeds = chassisSpeeds.
-            // chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 *
-            // speed.getY(), (-1 * rightStick.getX() * rotSpeedAdjust) - rot_correction,
-            // gyro.getRotation2d().times(-1));
         } else { // Create robot-relative speeds.
             swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
                     .withVelocityX(leftStick.getX() * speedAdjust)
@@ -196,6 +181,16 @@ public class SwerveDrive extends Subsystem {
         }
     }
 
+    public void driveFine(Translation2d leftStick, Translation2d rightStick, double percentOutput) {
+        stopped = false;
+        // Create robot-relative speeds.
+        swerveDriveTrain.setControl(new SwerveRequest.RobotCentric()
+            .withVelocityX(leftStick.getX() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
+            .withVelocityY(-leftStick.getY() * SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * percentOutput)
+            .withRotationalRate(rightStick.getX() * rotSpeedAdjust));
+    }
+
+
     public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical
                                                                                                // reason to have a robot
                                                                                                // relitive version of
@@ -291,6 +286,8 @@ public class SwerveDrive extends Subsystem {
 
     public void resetGyro() {
         swerveDriveTrain.tareEverything();
+        robotKnowsWhereItIs = false;
+        rotTarget = 0;
     }
 
 
@@ -321,6 +318,14 @@ public class SwerveDrive extends Subsystem {
         vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time));
 
         if (vision.isTag()) {
+            Pose2d pose = vision.getPose2d();
+            if (!robotKnowsWhereItIs) {
+                robotKnowsWhereItIs = true;
+                Pose2d currPose = getPose2d();
+                double deltaAngle = pose.getRotation().getDegrees() - currPose.getRotation().getDegrees();
+                rotTarget += deltaAngle;
+            }
+            
             swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time);
         }