From b4f399e7ffd5583c61967744ffaf9624673c8a7e Mon Sep 17 00:00:00 2001
From: judeconnolly <judeconnolly@seattleacademy.org>
Date: Thu, 25 Jan 2024 16:54:41 -0800
Subject: [PATCH 01/99] Arm Skeleton

---
 src/main/java/frc/robot/subsystems/Arm.java | 32 +++++++++++++++++----
 1 file changed, 27 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index ed0fbc5..2cdf6f4 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -1,19 +1,41 @@
 package frc.robot.subsystems;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.ctre.phoenix6.hardware.CANcoder;
 
 // How to make Subsystem (ignore image instructions, code is out of date, just look at written general instructions): https://compendium.readthedocs.io/en/latest/tasks/subsystems/subsystems.html
 // Command based programming: https://docs.wpilib.org/en/stable/docs/software/commandbased/what-is-command-based.html
 // Subsystem Documentation documentation: https://docs.wpilib.org/en/stable/docs/software/commandbased/subsystems.html
 
-/**
- * How to make a Subsystem:
- * 1. Copy this file, remember that class name has to match 
- */
+
 
 public class Arm extends SubsystemBase {
+
+    private final CANSparkMax leftArmMotor;
+    private final CANcoder leftArmEncoder;
+
+    private final CANSparkMax rightArmMotor;
+    private final CANcoder rightArmEncoder;
+
+    private final PIDController armRaisePIDController;
+
     /** Constructor. Creates a new ExampleSubsystem. */
-    public Arm() {
+    public Arm(int leftMotorId, int leftEncoderId, int rightMotorId, int rightEncoderId) {
+
+        leftArmMotor = new CANSparkMax(leftMotorId,MotorType.kBrushless);
+        leftArmEncoder = new CANcoder(leftEncoderId);
+
+        rightArmMotor = new CANSparkMax(rightMotorId,MotorType.kBrushless);
+        rightArmEncoder = new CANcoder(rightEncoderId);
+
+        armRaisePIDController = new PIDController(
+            0,
+            0,
+            0
+            );
 
     }
 

From 64a185394aafdc023e25adaaa5b936eeed078740 Mon Sep 17 00:00:00 2001
From: judeconnolly <judeconnolly@seattleacademy.org>
Date: Wed, 31 Jan 2024 17:50:48 -0800
Subject: [PATCH 02/99] setArmAngleRadians

---
 src/main/java/frc/robot/Constants.java      |  5 +++++
 src/main/java/frc/robot/subsystems/Arm.java | 24 ++++++++++++++++++++-
 2 files changed, 28 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index c486d24..05c5283 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -91,5 +91,10 @@ public static class SwerveDrivetrainConstants {
         public static final double MODULE_LOCATION_X = 0.25;
         public static final double MODULE_LOCATION_Y = 0.25;
     }
+
+    public static class ArmConstants {
+
+        public static final double 
+    }
 }
 
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 2cdf6f4..374760e 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -1,9 +1,14 @@
 package frc.robot.subsystems;
 
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.ArmConstants;
+
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.hardware.CANcoder;
 
 // How to make Subsystem (ignore image instructions, code is out of date, just look at written general instructions): https://compendium.readthedocs.io/en/latest/tasks/subsystems/subsystems.html
@@ -16,12 +21,18 @@ public class Arm extends SubsystemBase {
 
     private final CANSparkMax leftArmMotor;
     private final CANcoder leftArmEncoder;
-
+    //<p>leftArmEncoder may be useless, as the right arm equivelent is the position for both.</p>
     private final CANSparkMax rightArmMotor;
     private final CANcoder rightArmEncoder;
 
+    private final StatusSignal<Double> armPosition;
+
     private final PIDController armRaisePIDController;
 
+    private Rotation2d armRotation2d;
+
+    
+
     /** Constructor. Creates a new ExampleSubsystem. */
     public Arm(int leftMotorId, int leftEncoderId, int rightMotorId, int rightEncoderId) {
 
@@ -37,6 +48,14 @@ public Arm(int leftMotorId, int leftEncoderId, int rightMotorId, int rightEncode
             0
             );
 
+        armPosition = rightArmEncoder.getAbsolutePosition();
+
+    }
+
+    public void setArmAngleRadians(double desiredRadian) {
+        if (desiredRadian < ArmConstants.MAXIMUM_ARM_RADIANS || ArmConstants.MINIMUM_ARM_RADIANS > desiredRadian) {
+            armRotation2d = Rotation2d.fromRadians(desiredRadian);
+        }
     }
 
     /**
@@ -47,5 +66,8 @@ public Arm(int leftMotorId, int leftEncoderId, int rightMotorId, int rightEncode
     @Override
     public void periodic() {
         // This method will be called once per scheduler run
+        final double armSpeed = armRaisePIDController.calculate(armPosition,);
+        leftArmMotor.set(armSpeed);
+        rightArmMotor.set(armSpeed);
     }
 }

From 81e71cf5b1668268dad20e772666c87097139b80 Mon Sep 17 00:00:00 2001
From: judeconnolly <judeconnolly@seattleacademy.org>
Date: Thu, 1 Feb 2024 15:53:14 -0800
Subject: [PATCH 03/99] Previous setArmAngleRadians commit completion

forgot to save before committing on my last commit
---
 src/main/java/frc/robot/Constants.java      |  3 ++-
 src/main/java/frc/robot/subsystems/Arm.java | 11 ++++++++---
 2 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 05c5283..fc5e7bb 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -94,7 +94,8 @@ public static class SwerveDrivetrainConstants {
 
     public static class ArmConstants {
 
-        public static final double 
+        public static final double MAXIMUM_ARM_RADIANS = 3;
+        public static final double MINIMUM_ARM_RADIANS = 0;
     }
 }
 
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 374760e..f64d50d 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -53,6 +53,7 @@ public Arm(int leftMotorId, int leftEncoderId, int rightMotorId, int rightEncode
     }
 
     public void setArmAngleRadians(double desiredRadian) {
+        //maximum should ALWAYS be a greater value then minimum
         if (desiredRadian < ArmConstants.MAXIMUM_ARM_RADIANS || ArmConstants.MINIMUM_ARM_RADIANS > desiredRadian) {
             armRotation2d = Rotation2d.fromRadians(desiredRadian);
         }
@@ -66,8 +67,12 @@ public void setArmAngleRadians(double desiredRadian) {
     @Override
     public void periodic() {
         // This method will be called once per scheduler run
-        final double armSpeed = armRaisePIDController.calculate(armPosition,);
-        leftArmMotor.set(armSpeed);
-        rightArmMotor.set(armSpeed);
+
+        final double armSpeed = armRaisePIDController.calculate(armPosition.refresh().getValueAsDouble(),armRotation2d.getRotations());
+        if(!(armSpeed == 0)){
+            leftArmMotor.set(armSpeed);
+            rightArmMotor.set(armSpeed);
+        }
+        
     }
 }

From 103fcb107e6229991ecedee6f3b71d7ef7b19a00 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Thu, 1 Feb 2024 16:07:34 -0800
Subject: [PATCH 04/99] remove unused imports

---
 src/main/java/frc/robot/commands/Autos.java                     | 2 +-
 .../commands/SwerveRemoteOperation/SwerveDriveXboxControl.java  | 2 --
 2 files changed, 1 insertion(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java
index 0e1ade2..1cca8aa 100644
--- a/src/main/java/frc/robot/commands/Autos.java
+++ b/src/main/java/frc/robot/commands/Autos.java
@@ -11,7 +11,7 @@ public static Command testingAuto(SwerveDrivetrain drivetrain) {
         return Commands.sequence(
             new AutoDriveTo(drivetrain, new Translation2d(1, 0))
             // new WaitCommand(1),
-            // new AutoDriveTo(drivetrain, new Translation2d(-1, 0)),
+            // new AutoDriveTo(drivetrain, new Translation2d(-1, 0))
         );
     }
 
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
index 281f0a8..2640cb7 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
@@ -2,7 +2,6 @@
 
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.Constants.RobotMovementConstants;
 import frc.robot.Constants.DriverConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
 
@@ -64,7 +63,6 @@ public void execute() {
      * Whether the command has finished. Once a command finishes, the scheduler will call its end() method and un-schedule it.
      * Always return false since we never want to end in this case.
      */
-
     @Override
     public boolean isFinished() {
         return false;

From ad0bf0229d7544b76d5528af0d7adf928d52d52d Mon Sep 17 00:00:00 2001
From: judeconnolly <judeconnolly@seattleacademy.org>
Date: Thu, 1 Feb 2024 17:13:55 -0800
Subject: [PATCH 05/99] Set Arm To Intake/Amp/Speaker Pos

changed everything from radians to degrees
---
 src/main/java/frc/robot/Constants.java      |  8 ++++++--
 src/main/java/frc/robot/subsystems/Arm.java | 22 ++++++++++++++++-----
 2 files changed, 23 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index fc5e7bb..8b02bb4 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -94,8 +94,12 @@ public static class SwerveDrivetrainConstants {
 
     public static class ArmConstants {
 
-        public static final double MAXIMUM_ARM_RADIANS = 3;
-        public static final double MINIMUM_ARM_RADIANS = 0;
+        public static final double MAXIMUM_ARM_DEGREES = 3;
+        public static final double MINIMUM_ARM_DEGREES = 0;
+
+        public static final double ARM_AMP_SHOOTING_DEGREES = 0;
+        public static final double ARM_SPEAKER_SHOOTING_DEGREES = 0;
+        public static final double ARM_INTAKE_DEGREES = 0;
     }
 }
 
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index f64d50d..6184d82 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -21,7 +21,7 @@ public class Arm extends SubsystemBase {
 
     private final CANSparkMax leftArmMotor;
     private final CANcoder leftArmEncoder;
-    //<p>leftArmEncoder may be useless, as the right arm equivelent is the position for both.</p>
+    //<p>leftArmEncoder may be useless, as the right arm equivelent is used for the position for both.</p>
     private final CANSparkMax rightArmMotor;
     private final CANcoder rightArmEncoder;
 
@@ -52,13 +52,25 @@ public Arm(int leftMotorId, int leftEncoderId, int rightMotorId, int rightEncode
 
     }
 
-    public void setArmAngleRadians(double desiredRadian) {
+    public void setArmAngleRadians(double desiredDegree) {
         //maximum should ALWAYS be a greater value then minimum
-        if (desiredRadian < ArmConstants.MAXIMUM_ARM_RADIANS || ArmConstants.MINIMUM_ARM_RADIANS > desiredRadian) {
-            armRotation2d = Rotation2d.fromRadians(desiredRadian);
+        if (desiredDegree < ArmConstants.MAXIMUM_ARM_DEGREES || ArmConstants.MINIMUM_ARM_DEGREES > desiredDegree) {
+            armRotation2d = Rotation2d.fromDegrees(desiredDegree);
         }
     }
 
+    public void setArmToAmpPosition() {
+        armRotation2d = Rotation2d.fromRadians(ArmConstants.ARM_AMP_SHOOTING_DEGREES);
+    }
+
+    public void setArmToSpeakerPosition() {
+        armRotation2d = Rotation2d.fromRadians(ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES);
+    }
+
+    public void setArmToIntakePosition() {
+        armRotation2d = Rotation2d.fromRadians(ArmConstants.ARM_INTAKE_DEGREES);
+    }
+
     /**
      * This method is called periodically by the CommandScheduler, about every 20ms.
      * It should be used for updating subsystem-specific state that you don't want to offload to a Command.
@@ -69,7 +81,7 @@ public void periodic() {
         // This method will be called once per scheduler run
 
         final double armSpeed = armRaisePIDController.calculate(armPosition.refresh().getValueAsDouble(),armRotation2d.getRotations());
-        if(!(armSpeed == 0)){
+        if(armSpeed != 0){
             leftArmMotor.set(armSpeed);
             rightArmMotor.set(armSpeed);
         }

From 9f3e332a6a16a819912696e252b5e07e57c392a3 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Thu, 1 Feb 2024 17:40:23 -0800
Subject: [PATCH 06/99] Field relative drive works w/ joystick. No auto switch

---
 shuffleboard.json                                   | 13 +++++++++++++
 src/main/java/frc/robot/RobotContainer.java         |  5 +++--
 .../SwerveDriveJoystickControl.java                 |  3 +++
 .../SwerveDriveXboxControl.java                     | 13 ++++++++++---
 4 files changed, 29 insertions(+), 5 deletions(-)

diff --git a/shuffleboard.json b/shuffleboard.json
index f1dea89..86fb906 100644
--- a/shuffleboard.json
+++ b/shuffleboard.json
@@ -102,6 +102,19 @@
               "Colors/Color when true": "#7CFC00FF",
               "Colors/Color when false": "#8B0000FF"
             }
+          },
+          "9,2": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/Bot Name",
+              "_title": "Bot Name",
+              "_glyph": 148,
+              "_showGlyph": false
+            }
           }
         }
       }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 2f0b0d0..9cc53f9 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -54,7 +54,7 @@ public class RobotContainer {
 			SwerveModuleConstants.ANGULAR_MOTOR_ENCODER_OFFSET_BR,
             new Translation2d(-SwerveDrivetrainConstants.MODULE_LOCATION_X, -SwerveDrivetrainConstants.MODULE_LOCATION_Y));
 
-    private final AHRS gyro = new AHRS(I2C.Port.kMXP);
+    private final AHRS gyro = new AHRS(I2C.Port.kOnboard);
 
     private final SwerveDrivetrain drivetrain = new SwerveDrivetrain(gyro, swerveModuleFL, swerveModuleFR, swerveModuleBL, swerveModuleBR);
 
@@ -71,7 +71,8 @@ public RobotContainer() {
     public void setUpDriveController() {
         // Create joysticks
         final GenericHID genericHID = new GenericHID(DriverConstants.DRIVER_JOYSTICK_PORT);
-        final HIDType genericHIDType = genericHID.getType();
+        // final HIDType genericHIDType = genericHID.getType();
+        final HIDType genericHIDType = GenericHID.HIDType.kHIDJoystick;
 
         SmartDashboard.putString("Drive Controller", genericHIDType.toString());
         SmartDashboard.putString("Bot Name", Constants.currentBot.toString());
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
index 758bf81..012bf1b 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
@@ -2,6 +2,7 @@
 
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
 import frc.robot.Constants.DriverConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
@@ -29,6 +30,8 @@ public SwerveDriveJoystickControl(SwerveDrivetrain drivetrain, CommandJoystick d
         preciseModeButton = new OptionButton(driverJoystick, 2, ActivationMode.TOGGLE);
         boostModeButton = new OptionButton(driverJoystick, 1, ActivationMode.HOLD);
         fieldRelativeButton = new OptionButton(driverJoystick, 3, ActivationMode.TOGGLE);
+
+        driverJoystick.button(5).onTrue(new InstantCommand(drivetrain::zeroGyroYaw));
     }
 
     @Override
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
index 7c25cdb..4572b9e 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
@@ -1,7 +1,6 @@
 package frc.robot.commands.SwerveRemoteOperation;
 
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants.DriverConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
@@ -17,6 +16,9 @@
 public class SwerveDriveXboxControl extends SwerveDriveBaseControl {
     private final OptionButton preciseModeButton;
     private final OptionButton boostModeButton;
+
+    // private final OptionButton fieldRelativeButton;
+    
     /**
      * Creates a new SwerveDriveXboxControl Command.
      *
@@ -25,10 +27,14 @@ public class SwerveDriveXboxControl extends SwerveDriveBaseControl {
      */
     public SwerveDriveXboxControl(SwerveDrivetrain drivetrain, CommandXboxController driverXboxController) {
         super(drivetrain, driverXboxController);
+        
         // Create and configure buttons
         // OptionButton exampleToggleButton = new OptionButton(controller::a, ActivationMode.TOGGLE);
-        preciseModeButton = new OptionButton(driverXboxController, 8, ActivationMode.HOLD);
-        boostModeButton = new OptionButton(driverXboxController, 1, ActivationMode.HOLD);
+        preciseModeButton = new OptionButton(driverXboxController::b, ActivationMode.TOGGLE);
+        boostModeButton = new OptionButton(driverXboxController::leftStick, ActivationMode.HOLD);
+
+        // fieldRelativeButton = new OptionButton(driverXboxController::, ActivationMode.TOGGLE)
+
         // Tell the command schedular we are using the drivetrain
         addRequirements(drivetrain);
     }
@@ -63,6 +69,7 @@ public void execute() {
             leftX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
             leftY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
             rightX * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
+
         drivetrain.setDesiredState(speeds, false);
     }
 

From 8b3c281ba7c898f38781005f5014e9083f5dfa19 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Fri, 2 Feb 2024 20:41:44 -0800
Subject: [PATCH 07/99] Standardize drive control classes, without over
 abstracting

---
 .../SwerveDriveBaseControl.java               | 13 ++-
 .../SwerveDriveJoystickControl.java           |  7 --
 .../SwerveDrivePS4Control.java                | 79 ++++---------------
 .../SwerveDriveXboxControl.java               | 72 ++++-------------
 4 files changed, 42 insertions(+), 129 deletions(-)

diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java
index cbaf668..895170d 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java
@@ -1,6 +1,7 @@
 package frc.robot.commands.SwerveRemoteOperation;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
@@ -46,11 +47,21 @@ public void initialize() {
     */
    @Override
    public void execute() {
-      final Pose2d robotPosition = drivetrain.getPosition();
 
+      // Position display
+      final Pose2d robotPosition = drivetrain.getPosition();
       SmartDashboard.putNumber("PoseX", robotPosition.getX());
       SmartDashboard.putNumber("PoseY", robotPosition.getX());
       SmartDashboard.putNumber("PoseDegrees", robotPosition.getRotation().getDegrees());
+
+      
+      // Speed degrees
+      final ChassisSpeeds currentSpeeds = drivetrain.getState();
+      final double speedMetersPerSecond = Math.sqrt(Math.pow(currentSpeeds.vxMetersPerSecond, 2) + Math.pow(currentSpeeds.vyMetersPerSecond, 2));
+      
+      final double metersPerSecondToMilesPerHourConversion = 2.237;
+      SmartDashboard.putNumber("Robot Speed", speedMetersPerSecond * metersPerSecondToMilesPerHourConversion);
+      SmartDashboard.putNumber("Heading Degrees", drivetrain.getHeading().getDegrees());
    }
 
    /**
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
index 012bf1b..4085af4 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
@@ -63,13 +63,6 @@ public void execute() {
         SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);        
         SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
 
-        SmartDashboard.putNumber("Heading Degrees", drivetrain.getHeading().getDegrees());
-
-        final ChassisSpeeds currentSpeeds = drivetrain.getState();
-        final double speedMetersPerSecond = Math.sqrt(Math.pow(currentSpeeds.vxMetersPerSecond, 2) + Math.pow(currentSpeeds.vyMetersPerSecond, 2));
-        final double metersPerSecondToMilesPerHourConversion = 2.237;
-        SmartDashboard.putNumber("Robot Speed", speedMetersPerSecond * metersPerSecondToMilesPerHourConversion);
-
         drivetrain.setDesiredState(speeds, isFieldRelative);
     }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java
index 5bf3971..0515851 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java
@@ -1,7 +1,7 @@
 package frc.robot.commands.SwerveRemoteOperation;
 
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
 import frc.robot.Constants.DriverConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
@@ -11,10 +11,7 @@
 /**
  * This is the default command for the drivetrain, allowing for remote operation with PS4 controller
  */
-public class SwerveDrivePS4Control extends Command {
-    private final SwerveDrivetrain drivetrain;
-    private final CommandPS4Controller controller;
-
+public class SwerveDrivePS4Control extends SwerveDriveBaseControl {
     private final OptionButton preciseModeButton;
     private final OptionButton boostModeButton;
 
@@ -24,38 +21,24 @@ public class SwerveDrivePS4Control extends Command {
      * @param drivetrain The drive train of the robot
      * @param driverPS4Controller The PS4 controller used to control the drive train
      */
-
     public SwerveDrivePS4Control(SwerveDrivetrain drivetrain, CommandPS4Controller driverPS4Controller){
-        this.drivetrain = drivetrain;
-        this.controller = driverPS4Controller;
+        super(drivetrain, driverPS4Controller);
 
         // Create and configure buttons
-        preciseModeButton = new OptionButton(driverPS4Controller,1, ActivationMode.TOGGLE);
-        boostModeButton = new OptionButton(driverPS4Controller,2,ActivationMode.HOLD);
+        preciseModeButton = new OptionButton(driverPS4Controller::circle, ActivationMode.TOGGLE);
+        boostModeButton = new OptionButton(driverPS4Controller::triangle, ActivationMode.TOGGLE);
 
         //Tell the command scheduler we are using the drivetrain.
         addRequirements(drivetrain);
     }
 
-
-    /**
-     * The initial subroutine of a command. called once when the command is initially scheduled.
-     * Puts all swerve modules to the default state, staying still and facing forwards.
-     */
-    @Override
-    public void initialize(){
-        drivetrain.toDefaultStates();
-    }
-
-
-    /**
-     * The main body of a command. Called repeatedly while the command is scheduled (Every 20 ms).
-     */
     @Override
     public void execute(){
-        double leftX = controller.getLeftX();
-        double leftY = controller.getLeftY();
-        double rightX = controller.getRightX();
+        final CommandPS4Controller ps4Controller = (CommandPS4Controller) controller;
+
+        final double leftX = ps4Controller.getLeftX();
+        final double leftY = ps4Controller.getLeftY();
+        final double rightX = ps4Controller.getRightX();
 
         final int speedLevel = 1
             - preciseModeButton.getStateAsInt()
@@ -64,45 +47,13 @@ public void execute(){
         final ChassisSpeeds speeds = new ChassisSpeeds(
             leftX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
             leftY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-            rightX
+            rightX * DriverConstants.maxSpeedOptionsRotation[speedLevel]
         );
 
-        drivetrain.setDesiredState(speeds, false);
-    }
+        // Display relevant data on shuffleboard.
+        SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);        
+        SmartDashboard.putBoolean("Field Relieve", false);
 
-    /**
-     * Whether the command has finished. Once a command finishes, the scheduler will call its end() method and un-schedule it.
-     * Always return false since we never want to end in this case.
-     */
-    @Override
-    public boolean isFinished(){
-        return false;
-    }
-    
-    /**
-     * The action to take when the command ends. Called when either the command finishes normally, or when it interrupted/canceled.
-     * Here, this should only happen in this case if we get interrupted.
-     */
-    @Override
-    public void end(boolean interrupted){
-        drivetrain.stop();
-    }
-
-    // --- Util ---
-
-    /**
-     * Utility method. Apply a deadzone to the joystick output to account for stick drift and small bumps.
-     * 
-     * @param joystickValue Value in [-1, 1] from joystick axis
-     * @return {@code 0} if {@code |joystickValue| <= deadzone}, else the {@code joystickValue} scaled to the new control area
-     */
-    public static double applyJoystickDeadzone(double joystickValue, double deadzone){
-        // If the joystick |value| is in the deadzone than zero it out
-        if(Math.abs(joystickValue) >= deadzone){
-            return 0;
-        }
-
-        // scale value from the range [0, 1] to (deadzone, 1]
-        return joystickValue * (1 + deadzone) - Math.signum(joystickValue) * deadzone;
+        drivetrain.setDesiredState(speeds, false);
     }
 }
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
index 4572b9e..71f2635 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
@@ -1,6 +1,7 @@
 package frc.robot.commands.SwerveRemoteOperation;
 
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants.DriverConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
@@ -16,6 +17,7 @@
 public class SwerveDriveXboxControl extends SwerveDriveBaseControl {
     private final OptionButton preciseModeButton;
     private final OptionButton boostModeButton;
+    private final OptionButton fieldRelativeButton;
 
     // private final OptionButton fieldRelativeButton;
     
@@ -32,6 +34,7 @@ public SwerveDriveXboxControl(SwerveDrivetrain drivetrain, CommandXboxController
         // OptionButton exampleToggleButton = new OptionButton(controller::a, ActivationMode.TOGGLE);
         preciseModeButton = new OptionButton(driverXboxController::b, ActivationMode.TOGGLE);
         boostModeButton = new OptionButton(driverXboxController::leftStick, ActivationMode.HOLD);
+        fieldRelativeButton = new OptionButton(driverXboxController::povUp, ActivationMode.TOGGLE);
 
         // fieldRelativeButton = new OptionButton(driverXboxController::, ActivationMode.TOGGLE)
 
@@ -39,75 +42,30 @@ public SwerveDriveXboxControl(SwerveDrivetrain drivetrain, CommandXboxController
         addRequirements(drivetrain);
     }
 
-    /**
-     * The initial subroutine of a command. Called once when the command is initially scheduled.
-     * Puts all swerve modules to the default state, staying still and facing forwards.
-     */
-    @Override
-    public void initialize() {
-        drivetrain.enablePowerDriveMode();
-        drivetrain.toDefaultStates();
-    }
-
-    /**
-     * The main body of a command. Called repeatedly while the command is scheduled (Every 20 ms).
-     */
     @Override
     public void execute() {
         final CommandXboxController xboxController = (CommandXboxController) controller;
 
-        double leftX = applyJoystickDeadzone(xboxController.getLeftX(), DriverConstants.XBOX_DEAD_ZONE);
-        double leftY = applyJoystickDeadzone(xboxController.getLeftY(), DriverConstants.XBOX_DEAD_ZONE);
+        final double leftX = applyJoystickDeadzone(xboxController.getLeftX(), DriverConstants.XBOX_DEAD_ZONE);
+        final double leftY = applyJoystickDeadzone(xboxController.getLeftY(), DriverConstants.XBOX_DEAD_ZONE);
+
+        final double rightX = -applyJoystickDeadzone(xboxController.getRightX(), DriverConstants.XBOX_DEAD_ZONE);
 
-        double rightX = -applyJoystickDeadzone(xboxController.getRightX(), DriverConstants.XBOX_DEAD_ZONE);
+        final boolean isFieldRelative = fieldRelativeButton.getState();
 
-        int speedLevel = 1
-        - preciseModeButton.getStateAsInt()
-        + boostModeButton.getStateAsInt();
+        final int speedLevel = 1
+            - preciseModeButton.getStateAsInt()
+            + boostModeButton.getStateAsInt();
 
         final ChassisSpeeds speeds = new ChassisSpeeds(
             leftX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
             leftY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
             rightX * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
 
-        drivetrain.setDesiredState(speeds, false);
-    }
-
-    /**
-     * Whether the command has finished. Once a command finishes, the scheduler will call its end() method and un-schedule it.
-     * Always return false since we never want to end in this case.
-     */
-    @Override
-    public boolean isFinished() {
-        return false;
-    }
-
-    /**
-     * The action to take when the command ends. Called when either the command finishes normally, or when it interrupted/canceled.
-     * Here, this should only happen in this case if we get interrupted.
-     */
-    @Override
-    public void end(boolean interrupted) {
-        drivetrain.disablePowerDriveMode();
-        drivetrain.stop();
-    }
-
-    // --- Util ---
-
-    /**
-     * Utility method. Apply a deadzone to the joystick output to account for stick drift and small bumps.
-     * 
-     * @param joystickValue Value in [-1, 1] from joystick axis
-     * @return {@code 0} if {@code |joystickValue| <= deadzone}, else the {@code joystickValue} scaled to the new control area
-     */
-    public static double applyJoystickDeadzone(double joystickValue, double deadzone) {
-        if (Math.abs(joystickValue) <= deadzone) {
-            // If the joystick |value| is in the deadzone than zero it out
-            return 0;
-        }
+        // Display relevant data on shuffleboard.
+        SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);        
+        SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
 
-        // scale value from the range [0, 1] to (deadzone, 1]
-        return joystickValue * (1 + deadzone) - Math.signum(joystickValue) * deadzone;
+        drivetrain.setDesiredState(speeds, isFieldRelative);
     }
-
 }
\ No newline at end of file

From ed8ac0f754d5d2eabd2bd732740fd8cd640dcb82 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Fri, 2 Feb 2024 21:58:01 -0800
Subject: [PATCH 08/99] minor code cleanup and improve comments

---
 src/main/java/frc/robot/Robot.java            |  3 +-
 src/main/java/frc/robot/commands/Autos.java   | 39 ++++++-----
 .../frc/robot/commands/ExampleCommand.java    |  2 +-
 .../robot/subsystems/ExampleSubsystem.java    |  2 +-
 .../robot/subsystems/SwerveDrivetrain.java    |  7 +-
 .../frc/robot/subsystems/SwerveModule.java    | 64 +++++++++++++------
 .../java/frc/robot/utils/OptionButton.java    | 11 +++-
 7 files changed, 81 insertions(+), 47 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index d474a9b..2b7d04f 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -21,8 +21,7 @@ public class Robot extends TimedRobot {
    */
   @Override
   public void robotInit() {
-    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
-    // autonomous chooser on the dashboard.
+    // Instantiate the RobotContainer. This will perform all the button bindings, and put the autonomous chooser on the dashboard.
     robotContainer = new RobotContainer();
   }
 
diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java
index eb36334..b3bbd48 100644
--- a/src/main/java/frc/robot/commands/Autos.java
+++ b/src/main/java/frc/robot/commands/Autos.java
@@ -12,27 +12,32 @@ public final class Autos {
     /** Example static factory for an autonomous command. */
     public static Command testingAuto(SwerveDrivetrain drivetrain) {
         return Commands.sequence(
-            new AutoDriveTo(drivetrain, new Translation2d(1, 0))
-            // new WaitCommand(1),
-            // new AutoDriveTo(drivetrain, new Translation2d(-1, 0))
+                new AutoDriveTo(drivetrain, new Translation2d(1, 0))
+                // new WaitCommand(1),
+                // new AutoDriveTo(drivetrain, new Translation2d(-1, 0))
         );
     }
-    public static Command startingAuto(Arm arm, SwerveDrivetrain drivetrain, boolean inverty){
-      //assumes start position in corner
-      double invert=1;
-      if (inverty){
-        invert=-1;
-      }
 
-      return Commands.sequence(
-        //2.9, 0.2 and 1.2 are not arbitrary, they move the robot so that the note is right in front; 0.05 can be changed, it's for the amount of extra spacing that we want
-        new AutoDriveTo(drivetrain, new Translation2d(2.9-0.2-RobotDimensions.ROBOT_LENGTH_X-0.05,invert*(1.2-RobotDimensions.ROBOT_LENGTH_Y))),
+    public static Command startingAuto(Arm arm, SwerveDrivetrain drivetrain, boolean invertY) {
 
-        new AutoRotateTo(drivetrain, new Rotation2d(Math.PI/-2*invert))
-      );
+        // assumes start position in corner
+        double invert = 1;
+        if (invertY) {
+            invert = -1;
+        }
+
+        return Commands.sequence(
+                // 2.9, 0.2 and 1.2 are not arbitrary, they move the robot so that the note is
+                // right in front; 0.05 can be changed, it's for the amount of extra spacing
+                // that we want
+                new AutoDriveTo(drivetrain,
+                        new Translation2d(2.9 - 0.2 - RobotDimensions.ROBOT_LENGTH_X - 0.05,
+                                invert * (1.2 - RobotDimensions.ROBOT_LENGTH_Y))),
+
+                new AutoRotateTo(drivetrain, new Rotation2d(Math.PI / -2 * invert)));
     }
 
-  private Autos() {
-    throw new UnsupportedOperationException("This is a utility class!");
-  }
+    private Autos() {
+        throw new UnsupportedOperationException("This is a utility class!");
+    }
 }
diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java
index c12a6c9..ae6fe14 100644
--- a/src/main/java/frc/robot/commands/ExampleCommand.java
+++ b/src/main/java/frc/robot/commands/ExampleCommand.java
@@ -3,7 +3,7 @@
 import frc.robot.subsystems.ExampleSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
 
-// How to make Command (ignore image instructions, code is out of date, just look at written general instructions): https://compendium.readthedocs.io/en/latest/tasks/commands/commands.html
+// How to make Command: https://compendium.readthedocs.io/en/latest/tasks/commands/commands.html (ignore image instructions, code is out of date, just look at written general instructions)
 // Command based programming: https://docs.wpilib.org/en/stable/docs/software/commandbased/what-is-command-based.html
 // Code documentations https://docs.wpilib.org/en/stable/docs/software/commandbased/commands.html 
 
diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java
index b7b7076..76d149b 100644
--- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java
@@ -2,7 +2,7 @@
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-// How to make Subsystem (ignore image instructions, code is out of date, just look at written general instructions): https://compendium.readthedocs.io/en/latest/tasks/subsystems/subsystems.html
+// How to make Subsystem: https://compendium.readthedocs.io/en/latest/tasks/subsystems/subsystems.html (ignore image instructions, code is out of date, just look at written general instructions)
 // Command based programming: https://docs.wpilib.org/en/stable/docs/software/commandbased/what-is-command-based.html
 // Subsystem Documentation documentation: https://docs.wpilib.org/en/stable/docs/software/commandbased/subsystems.html
 
diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index 9f02a9b..3233add 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -45,11 +45,7 @@ public class SwerveDrivetrain extends SubsystemBase {
     private ChassisSpeeds desiredSpeeds;
 
     // Our 4 swerve Modules
-    private final SwerveModule moduleFL;
-    private final SwerveModule moduleFR;
-    private final SwerveModule moduleBL;
-    private final SwerveModule moduleBR;
-
+    private final SwerveModule moduleFL, moduleFR, moduleBL, moduleBR;
     private final SwerveModule[] modules;
 
     /**
@@ -176,6 +172,7 @@ public ChassisSpeeds getState() {
     public void enablePowerDriveMode() {
         modulesMap(SwerveModule::enablePowerDriveMode);
     }
+    
     public void disablePowerDriveMode() {
         modulesMap(SwerveModule::disablePowerDriveMode);
     }
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index ce7334b..eb2fce6 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -117,6 +117,8 @@ public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steer
     /**
      * This is the periodic function of the swerve module.
      * This method is called periodically by the CommandScheduler, about every 20ms.
+     * 
+     * We use it to constantly keep the 
      */
     @Override
     public void periodic() {
@@ -126,38 +128,48 @@ public void periodic() {
 
             if (desiredState.speedMetersPerSecond == 0) {
                 // If our desired speed is 0, just use the built in motor stop, no matter the mode.
+                
+                // Stops motor movement. Motor can be moved again by calling set without having to re-enable the motor.
                 driveMotor.stopMotor();
             }
             else if (powerDriveMode) {
                 // If we are in power drive mode just directly set power to our desired speed.
-                // This is a bit of an abuse of the SwerveModuleState object but we don't want to have to deal with a pid controller when we are just driving
+
+                // This is a bit of an abuse of the SwerveModuleState object as we treat speeds as power values from 0 to 1.
+                // We do this because we don't want to have to deal with a PID controller when we are just driving, as a human driver does not care about they exact speed mapping.
                 driveMotor.set(desiredState.speedMetersPerSecond);
             }
             else {
-                // If we are  in normal drive mode use our drive motor builtin PID controller in velocity mode to set it to our desired meters per second
+                // If we are in normal drive mode use the motor controller to set our target velocity
+
+                // The CANSparkMaxes have a builtin PID controller on them we can use to set a target velocity
+                // We first convert our speed from meters per second to rotations per minute, as that is the native unit of our devices
                 final double desiredDriveRotationsPerMinute = (desiredState.speedMetersPerSecond * 60) / SwerveModuleConstants.WHEEL_CIRCUMFERENCE;
                 drivePIDController.setReference(desiredDriveRotationsPerMinute, ControlType.kVelocity);
             }
 
             // --- Set steering motor ---
 
-            // get our current angle and our desired angle
+            // Get our current angle and our desired angle, as we need both
             final double desiredRotations = desiredState.angle.getRotations();
             final double measuredRotations = getSteeringAngleRotations();
 
-            // calculate how fast to spin the motor to get to the desired angle using our PID controller
+            // Calculate how fast to spin the motor to get to the desired angle using our PID controller
             final double steeringMotorSpeed = steeringPIDController.calculate(measuredRotations, desiredRotations);
+            // Then set the motor to spin at that speed
             steeringMotor.set(steeringMotorSpeed);
         }
     }
 
-    /** stop drive and steering motor of swerve module and set desired state to nothing*/
+    // --- Direct control methods ---
+
+    /** Stop drive and steering motor of swerve module and set desired state to nothing */
     public void stop() {
 
-        // Make sure we have no desired state
+        // Make sure we have no desired state, or else we would just start driving again 
         setDesiredState(null);
 
-        // Manually stop both swerve modules
+        // Manually stop both motors in swerve module
         driveMotor.stopMotor();
         steeringMotor.stopMotor();
     }
@@ -170,10 +182,7 @@ public void toDefaultState() {
         setDesiredState(defaultState);
     }
 
-    /** Get locations of the wheel relative to the physical center of the robot. */
-    public Translation2d getDistanceFromCenter() {
-        return distanceFromCenter;
-    }
+    // --- Getters and setters for modules desired SwerveModuleState ---
 
     /**
      * Get the state of the swerve module.
@@ -187,14 +196,6 @@ public SwerveModuleState getState() {
                 Rotation2d.fromRotations(getSteeringAngleRotations()));
     }
 
-    public void enablePowerDriveMode() {
-        this.powerDriveMode = true;
-    }
-
-    public void disablePowerDriveMode() {
-        this.powerDriveMode = false;
-    }
-
     /**
      * Set the state of the swerve module. The state is the speed and angle of the swerve module.
      * You can use {@code Rotation2d.fromDegrees()} to create angle.
@@ -231,6 +232,19 @@ public SwerveModuleState getDesiredState() {
         return desiredState;
     }
 
+
+    // --- Power Drive Mode control ---
+
+    public void enablePowerDriveMode() {
+        this.powerDriveMode = true;
+    }
+
+    public void disablePowerDriveMode() {
+        this.powerDriveMode = false;
+    }
+
+    // --- Public info getters ---
+
     /**
      * Get the position of the swerve module. The position is the distance traveled by the drive motor and angle of the drive motor.
      * 
@@ -242,6 +256,18 @@ public SwerveModulePosition getPosition() {
                 Rotation2d.fromRotations(getSteeringAngleRotations()));
     }
 
+    /**
+     * Get locations of the wheel relative to the physical center of the robot.
+     * Useful for kinematics.
+     * 
+     * @return Translation2d representing distance from center of bot
+     */
+    public Translation2d getDistanceFromCenter() {
+        return distanceFromCenter;
+    }
+
+    // --- Private getters ---
+
     /**
      * Get the velocity of the drive motor.
      * 
diff --git a/src/main/java/frc/robot/utils/OptionButton.java b/src/main/java/frc/robot/utils/OptionButton.java
index 746d149..90d9ae1 100644
--- a/src/main/java/frc/robot/utils/OptionButton.java
+++ b/src/main/java/frc/robot/utils/OptionButton.java
@@ -21,7 +21,7 @@ public class OptionButton {
      * Different modes which a OptionButton can be in
      */
     public static enum ActivationMode {
-        /** Like toggle, but stays on until manfully toggled off. Probably just use .onTrue() instead */
+        /** Like toggle, but stays on until manually toggled off with .toggleOff(). Probably just use .onTrue() instead */
         TAP,
 
         /** standard button behavior, on when pressed, off when released */
@@ -60,19 +60,21 @@ else if (mode == ActivationMode.TAP) {
         }
     }
 
+    /** Manually toggle state, has no effect if it is hold button */
     public void toggle() {
         isToggled = !isToggled;
     }
 
+    /** Manually toggle state on, has no effect if it is hold button */
     public void toggleOn() {
         isToggled = true;
     }
 
+    /** Manually toggle state off, has no effect if it is hold button */
     public void toggleOff() {
         isToggled = false;
     }
 
-
     /**
      * <p>Get the state of the button</p>
      * 
@@ -93,6 +95,11 @@ public boolean getState() {
         }
     }
 
+    /**
+     * Get state, but return it as a int instead of a boolean
+     * 
+     * @return 1 if true, 0 is false
+     */
     public int getStateAsInt() {
         return getState() ? 1 : 0;
     }

From 20433706688eae1cabbc884468f8113e4573cf6f Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Fri, 2 Feb 2024 23:05:00 -0800
Subject: [PATCH 09/99] standardize units for Auto Drives

---
 src/main/java/frc/robot/Constants.java             | 11 ++++-------
 src/main/java/frc/robot/commands/AutoDriveTo.java  |  5 +++--
 src/main/java/frc/robot/commands/AutoRotateTo.java |  5 +++--
 3 files changed, 10 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 34b7108..73e8a58 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -64,14 +64,11 @@ public static class DriverConstants {
     }
     
     public static class RobotMovementConstants {
-        public static final double maxSpeed = 1;
-        public static final double maxTurnSpeed = 1;
+        public static final double AT_SETPOINT_TOLERANCE_TIME_SECONDS = 1;
+        public static final double ROTATE_AT_SETPOINT_TIME_SECONDS = 1;
 
-        public static final double MOVE_PID_TOLERANCE_TIME = 500 / 20;
-        public static final double ROTATE_PID_TOLERANCE_TIME = 500 / 20;
-
-        public static final double POS_TOLERANCE = 0.01;
-        public static final double ANGLE_TOLERANCE = Math.PI / 30;
+        public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(5);
+        public static final double ANGLE_TOLERANCE_RADIANS = Units.degreesToRadians(5);
 
         public static final double ROTATION_PID_P = 0.5;
         public static final double ROTATION_PID_I = 0;
diff --git a/src/main/java/frc/robot/commands/AutoDriveTo.java b/src/main/java/frc/robot/commands/AutoDriveTo.java
index de5e4fd..33d496b 100644
--- a/src/main/java/frc/robot/commands/AutoDriveTo.java
+++ b/src/main/java/frc/robot/commands/AutoDriveTo.java
@@ -7,6 +7,7 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.RobotMovementConstants;
 
@@ -65,7 +66,7 @@ public void execute() {
         SmartDashboard.putNumber("PoseX", position.getX());
 		SmartDashboard.putNumber("PoseDegrees", position.getRotation().getDegrees()); 
 
-        if (Math.abs(x - goalX) < RobotMovementConstants.POS_TOLERANCE && Math.abs(y - goalY)<RobotMovementConstants.POS_TOLERANCE) atSetpointCounter+=20;
+        if (Math.abs(x - goalX) < RobotMovementConstants.POSITION_TOLERANCE_METERS && Math.abs(y - goalY)<RobotMovementConstants.POSITION_TOLERANCE_METERS) atSetpointCounter+=TimedRobot.kDefaultPeriod;
         else atSetpointCounter=0;
 
         double xSpeed = xMovePID.calculate(x, goalX);
@@ -81,7 +82,7 @@ public void execute() {
 
     @Override
     public boolean isFinished() {
-        return atSetpointCounter > RobotMovementConstants.MOVE_PID_TOLERANCE_TIME;
+        return atSetpointCounter > RobotMovementConstants.AT_SETPOINT_TOLERANCE_TIME_SECONDS;
     }
 
     @Override
diff --git a/src/main/java/frc/robot/commands/AutoRotateTo.java b/src/main/java/frc/robot/commands/AutoRotateTo.java
index 2cf37ba..bf04bff 100644
--- a/src/main/java/frc/robot/commands/AutoRotateTo.java
+++ b/src/main/java/frc/robot/commands/AutoRotateTo.java
@@ -6,6 +6,7 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.TimedRobot;
 import frc.robot.Constants.RobotMovementConstants;
 
 // How to make Command (ignore image instructions, code is out of date, just look at written general instructions): https://compendium.readthedocs.io/en/latest/tasks/commands/commands.html
@@ -49,13 +50,13 @@ public void execute() {
         
         drivetrain.setDesiredState(speeds);
 
-        if (Math.abs(currentAngle - this.angleGoal) < RobotMovementConstants.ANGLE_TOLERANCE) atSetpointCounter+=20;
+        if (Math.abs(currentAngle - this.angleGoal) < RobotMovementConstants.ANGLE_TOLERANCE_RADIANS) atSetpointCounter += TimedRobot.kDefaultPeriod;
         else atSetpointCounter=0;
     }
 
     @Override
     public boolean isFinished() {
-        return atSetpointCounter>RobotMovementConstants.ROTATE_PID_TOLERANCE_TIME;
+        return atSetpointCounter > RobotMovementConstants.ROTATE_AT_SETPOINT_TIME_SECONDS;
     }
 
     @Override

From d87b769087c17784ce525bbeb8480272b6196f5b Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Fri, 2 Feb 2024 23:05:56 -0800
Subject: [PATCH 10/99] set swerve module setting to set and forget, also
 change power mode toggle

---
 .../SwerveDriveBaseControl.java               |   3 +-
 .../SwerveDriveJoystickControl.java           |   2 +-
 .../SwerveDrivePS4Control.java                |   2 +-
 .../SwerveDriveXboxControl.java               |   2 +-
 .../commands/deprecated/AutoDriveForward.java |   2 +-
 .../robot/subsystems/SwerveDrivetrain.java    |  26 +++--
 .../frc/robot/subsystems/SwerveModule.java    | 102 ++++++------------
 7 files changed, 55 insertions(+), 84 deletions(-)

diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java
index 895170d..74dd343 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java
@@ -36,7 +36,6 @@ public SwerveDriveBaseControl(SwerveDrivetrain drivetrain, CommandGenericHID dri
     */
    @Override
    public void initialize() {
-      drivetrain.enablePowerDriveMode();
       drivetrain.toDefaultStates();
 
       SmartDashboard.putBoolean("ControlActive", true);
@@ -50,6 +49,7 @@ public void execute() {
 
       // Position display
       final Pose2d robotPosition = drivetrain.getPosition();
+      
       SmartDashboard.putNumber("PoseX", robotPosition.getX());
       SmartDashboard.putNumber("PoseY", robotPosition.getX());
       SmartDashboard.putNumber("PoseDegrees", robotPosition.getRotation().getDegrees());
@@ -79,7 +79,6 @@ public boolean isFinished() {
     */
    @Override
    public void end(boolean interrupted) {
-      drivetrain.disablePowerDriveMode();
       drivetrain.stop();
 
       SmartDashboard.putBoolean("ControlActive", false);
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
index 4085af4..f3007fd 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
@@ -63,6 +63,6 @@ public void execute() {
         SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);        
         SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
 
-        drivetrain.setDesiredState(speeds, isFieldRelative);
+        drivetrain.setDesiredState(speeds, true, isFieldRelative);
     }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java
index 0515851..f406327 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java
@@ -54,6 +54,6 @@ public void execute(){
         SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);        
         SmartDashboard.putBoolean("Field Relieve", false);
 
-        drivetrain.setDesiredState(speeds, false);
+        drivetrain.setDesiredState(speeds,  true, false);
     }
 }
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
index 71f2635..9691080 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
@@ -66,6 +66,6 @@ public void execute() {
         SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);        
         SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
 
-        drivetrain.setDesiredState(speeds, isFieldRelative);
+        drivetrain.setDesiredState(speeds, true, isFieldRelative);
     }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/deprecated/AutoDriveForward.java b/src/main/java/frc/robot/commands/deprecated/AutoDriveForward.java
index 12c37a2..ec89b6f 100644
--- a/src/main/java/frc/robot/commands/deprecated/AutoDriveForward.java
+++ b/src/main/java/frc/robot/commands/deprecated/AutoDriveForward.java
@@ -24,7 +24,7 @@ public AutoDriveForward(SwerveDrivetrain drivetrain) {
     public void initialize() {
         ChassisSpeeds desiredState = new ChassisSpeeds(0.5,0,0);
         this.beganDriving = System.currentTimeMillis();
-        drivetrain.setDesiredState(desiredState);
+        drivetrain.setDesiredState(desiredState, false);
     }
 
     // No execute() because we don't need it
diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index 3233add..9fd6607 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -169,12 +169,16 @@ public ChassisSpeeds getState() {
         return speeds;
     }
 
-    public void enablePowerDriveMode() {
-        modulesMap(SwerveModule::enablePowerDriveMode);
-    }
-    
-    public void disablePowerDriveMode() {
-        modulesMap(SwerveModule::disablePowerDriveMode);
+    /**
+     * Set robot relative speeds of robot in meters per second mode.
+     * <p>vx: The velocity of the robot in the x (forward) direction in meter per second.</p>
+     * <p>vy: The velocity of the robot in the y (sideways) direction in meter per second. (Positive values mean the robot is moving to the left).</p>
+     * <p>omega: The angular velocity of the robot in radians per second.</p>
+     * 
+     * @param speeds Desired speeds of drivetrain (using swerve modules)
+     */
+    public void setDesiredState(ChassisSpeeds speeds) {
+        setDesiredState(speeds, false);
     }
 
     /**
@@ -183,16 +187,17 @@ public void disablePowerDriveMode() {
      * <p>vy: The velocity of the robot in the y (sideways) direction in meter per second. (Positive values mean the robot is moving to the left).</p>
      * <p>omega: The angular velocity of the robot in radians per second.</p>
      * 
+     * @param powerDriveMode whether the ChassisSpeeds is in meters per second (false) or motor power (true)
      * @param speeds Desired speeds of drivetrain (using swerve modules)
      */
-    public void setDesiredState(ChassisSpeeds speeds) {
+    public void setDesiredState(ChassisSpeeds speeds, boolean powerDriveMode) {
 
         this.desiredSpeeds = speeds;
 
         SwerveModuleState[] states = kinematics.toSwerveModuleStates(desiredSpeeds);
 
         for (int i = 0; i < modules.length; i++) {
-            modules[i].setDesiredState(states[i]);
+            modules[i].setDesiredState(states[i], powerDriveMode);
         }
     }
 
@@ -205,16 +210,17 @@ public void setDesiredState(ChassisSpeeds speeds) {
      * @see https://ibb.co/dJrL259
      * 
      * @param speeds Desired speeds of drivetrain (using swerve modules)
+     * @param powerDriveMode whether the ChassisSpeeds is in meters per second (false) or motor power (true)
      * @param fieldRelative True if the robot is using a field relative coordinate system, false if using a robot relive coordinate system. If field relative, forward will be directly away from driver, no matter the rotation of the robot.
      * If robot relative, forward will be whatever direction the robot is facing in.
      */
-    public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative) {
+    public void setDesiredState(ChassisSpeeds speeds, boolean powerDriveMode, boolean fieldRelative) {
 
         if (fieldRelative) {
             speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getHeading());
         }
 
-        setDesiredState(speeds);
+        setDesiredState(speeds, powerDriveMode);
     }
 
     /**
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index eb2fce6..ffbf28d 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -43,14 +43,9 @@ public class SwerveModule extends SubsystemBase {
     /** Default state, forward and still */
     private final static SwerveModuleState defaultState = new SwerveModuleState();
 
-    /** Represents the state the swerve module wants to be in */
-    private SwerveModuleState desiredState;
-
     /** Locations of the wheel relative to the physical center of the robot. */
     private final Translation2d distanceFromCenter;
 
-    private boolean powerDriveMode = false;
-
     /**
      * Constructor for an individual Swerve Module.
      * Sets up both drive and angular motor for swerve module as well as systems to monitor and control them
@@ -122,43 +117,8 @@ public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steer
      */
     @Override
     public void periodic() {
-        if (desiredState != null) { 
-
-            // --- Set drive motor ---
-
-            if (desiredState.speedMetersPerSecond == 0) {
-                // If our desired speed is 0, just use the built in motor stop, no matter the mode.
-                
-                // Stops motor movement. Motor can be moved again by calling set without having to re-enable the motor.
-                driveMotor.stopMotor();
-            }
-            else if (powerDriveMode) {
-                // If we are in power drive mode just directly set power to our desired speed.
-
-                // This is a bit of an abuse of the SwerveModuleState object as we treat speeds as power values from 0 to 1.
-                // We do this because we don't want to have to deal with a PID controller when we are just driving, as a human driver does not care about they exact speed mapping.
-                driveMotor.set(desiredState.speedMetersPerSecond);
-            }
-            else {
-                // If we are in normal drive mode use the motor controller to set our target velocity
-
-                // The CANSparkMaxes have a builtin PID controller on them we can use to set a target velocity
-                // We first convert our speed from meters per second to rotations per minute, as that is the native unit of our devices
-                final double desiredDriveRotationsPerMinute = (desiredState.speedMetersPerSecond * 60) / SwerveModuleConstants.WHEEL_CIRCUMFERENCE;
-                drivePIDController.setReference(desiredDriveRotationsPerMinute, ControlType.kVelocity);
-            }
-
-            // --- Set steering motor ---
-
-            // Get our current angle and our desired angle, as we need both
-            final double desiredRotations = desiredState.angle.getRotations();
-            final double measuredRotations = getSteeringAngleRotations();
-
-            // Calculate how fast to spin the motor to get to the desired angle using our PID controller
-            final double steeringMotorSpeed = steeringPIDController.calculate(measuredRotations, desiredRotations);
-            // Then set the motor to spin at that speed
-            steeringMotor.set(steeringMotorSpeed);
-        }
+        // Calculate how fast to spin the motor to get to the desired angle using our PID controller, then set the motor to spin at that speed
+        steeringMotor.set(steeringPIDController.calculate(getSteeringAngleRotations()));
     }
 
     // --- Direct control methods ---
@@ -167,7 +127,7 @@ else if (powerDriveMode) {
     public void stop() {
 
         // Make sure we have no desired state, or else we would just start driving again 
-        setDesiredState(null);
+        toDefaultState();
 
         // Manually stop both motors in swerve module
         driveMotor.stopMotor();
@@ -179,7 +139,7 @@ public void stop() {
      * This should have the module facing forward and not spinning.
      */
     public void toDefaultState() {
-        setDesiredState(defaultState);
+        setDesiredState(defaultState, false);
     }
 
     // --- Getters and setters for modules desired SwerveModuleState ---
@@ -201,15 +161,41 @@ public SwerveModuleState getState() {
      * You can use {@code Rotation2d.fromDegrees()} to create angle.
      * 
      * @param state New state of swerve module, contains speed in meters per second and angle as {@link Rotation2d}
+     * @param powerDriveMode whether the SwerveModuleState is in meters per second (false) or motor power (true)
      * @param shouldOptimize Whether to optimize the way the swerve module gets to the desired state
      */
-    public void setDesiredState(SwerveModuleState state, boolean shouldOptimize) {
+    public void setDesiredState(SwerveModuleState state, boolean powerDriveMode, boolean shouldOptimize) {
         if (shouldOptimize && state != null) {
             // Optimize the reference state to avoid spinning further than 90 degrees
             state = optimize(state,  getState().angle);
         }
+
+        // --- Set steering motor ---
+        steeringPIDController.setSetpoint(state.angle.getRotations());
         
-        this.desiredState = state;
+        // --- Set drive motor ---
+
+        if (state.speedMetersPerSecond == 0) {
+            // If our desired speed is 0, just use the builtin motor stop, no matter the mode.
+            
+            // Stops motor movement. Motor can be moved again by calling set without having to re-enable the motor.
+            driveMotor.stopMotor();
+        }
+        else if (powerDriveMode) {
+            // If we are in power drive mode just directly set power to our desired speed.
+
+            // This is a bit of an abuse of the SwerveModuleState object as we treat speeds as power values from -1 to 1.
+            // We do this because we don't want to have to deal with a PID controller when we are just driving, as a human driver does not care about they exact speed mapping.
+            driveMotor.set(state.speedMetersPerSecond);
+        }
+        else {
+            // If we are in normal drive mode use the motor controller to set our target velocity
+
+            // The CANSparkMaxes have a builtin PID controller on them we can use to set a target velocity.
+            // We first convert our speed from meters per second to rotations per minute, as that is the native unit of our devices
+            final double desiredDriveRotationsPerMinute = (state.speedMetersPerSecond * 60) / SwerveModuleConstants.WHEEL_CIRCUMFERENCE;
+            drivePIDController.setReference(desiredDriveRotationsPerMinute, ControlType.kVelocity);
+        }
     }
 
     /**
@@ -219,28 +205,8 @@ public void setDesiredState(SwerveModuleState state, boolean shouldOptimize) {
      * 
      * @param state New state of swerve module, contains speed in meters per second and angle.
      */
-    public void setDesiredState(SwerveModuleState state) {
-        setDesiredState(state, true);
-    }
-
-    /**
-     * Get the desired state of the swerve module. The state is the speed and angle of the swerve module.
-     * 
-     * @return State that the swerve module is trying to achieve, contains speed in meters per second and angle.
-     */
-    public SwerveModuleState getDesiredState() {
-        return desiredState;
-    }
-
-
-    // --- Power Drive Mode control ---
-
-    public void enablePowerDriveMode() {
-        this.powerDriveMode = true;
-    }
-
-    public void disablePowerDriveMode() {
-        this.powerDriveMode = false;
+    public void setDesiredState(SwerveModuleState state, boolean powerDriveMode) {
+        setDesiredState(state, powerDriveMode, true);
     }
 
     // --- Public info getters ---

From 4941a05f774915273c7558937c3b9f2fb145f133 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Fri, 2 Feb 2024 23:17:12 -0800
Subject: [PATCH 11/99] remove saving of desired states since we now do set and
 forget

---
 .../java/frc/robot/commands/AutoRotateTo.java |  5 +----
 .../robot/subsystems/SwerveDrivetrain.java    | 21 ++-----------------
 .../frc/robot/subsystems/SwerveModule.java    |  4 ----
 3 files changed, 3 insertions(+), 27 deletions(-)

diff --git a/src/main/java/frc/robot/commands/AutoRotateTo.java b/src/main/java/frc/robot/commands/AutoRotateTo.java
index bf04bff..7d9b9a7 100644
--- a/src/main/java/frc/robot/commands/AutoRotateTo.java
+++ b/src/main/java/frc/robot/commands/AutoRotateTo.java
@@ -44,11 +44,8 @@ public void execute() {
         final double currentAngle = drivetrain.getHeading().getRadians();
 
         double turnsSeed = rotatePID.calculate(currentAngle,this.angleGoal);
-
-        ChassisSpeeds speeds = drivetrain.getDesiredState();
-        speeds.omegaRadiansPerSecond=turnsSeed;
         
-        drivetrain.setDesiredState(speeds);
+        drivetrain.setDesiredState(new ChassisSpeeds(0, 0, turnsSeed));
 
         if (Math.abs(currentAngle - this.angleGoal) < RobotMovementConstants.ANGLE_TOLERANCE_RADIANS) atSetpointCounter += TimedRobot.kDefaultPeriod;
         else atSetpointCounter=0;
diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index 9fd6607..17e85df 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -38,12 +38,6 @@ public class SwerveDrivetrain extends SubsystemBase {
      */
     private final SwerveDriveOdometry odometry;
 
-    /** 
-     * The ChassisSpeeds object represents the speeds of a robot chassis.
-     * @see https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/intro-and-chassis-speeds.html#the-chassis-speeds-class
-     */
-    private ChassisSpeeds desiredSpeeds;
-
     // Our 4 swerve Modules
     private final SwerveModule moduleFL, moduleFR, moduleBL, moduleBR;
     private final SwerveModule[] modules;
@@ -192,9 +186,7 @@ public void setDesiredState(ChassisSpeeds speeds) {
      */
     public void setDesiredState(ChassisSpeeds speeds, boolean powerDriveMode) {
 
-        this.desiredSpeeds = speeds;
-
-        SwerveModuleState[] states = kinematics.toSwerveModuleStates(desiredSpeeds);
+        SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);
 
         for (int i = 0; i < modules.length; i++) {
             modules[i].setDesiredState(states[i], powerDriveMode);
@@ -222,16 +214,7 @@ public void setDesiredState(ChassisSpeeds speeds, boolean powerDriveMode, boolea
 
         setDesiredState(speeds, powerDriveMode);
     }
-
-    /**
-     * Get the desired speeds of robot
-     *
-     * @return the value
-     */
-    public ChassisSpeeds getDesiredState() {
-        return desiredSpeeds;
-    }
-
+    
     /**
      * Get all the swerve module positions
      * 
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index ffbf28d..70f99f5 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -125,10 +125,6 @@ public void periodic() {
 
     /** Stop drive and steering motor of swerve module and set desired state to nothing */
     public void stop() {
-
-        // Make sure we have no desired state, or else we would just start driving again 
-        toDefaultState();
-
         // Manually stop both motors in swerve module
         driveMotor.stopMotor();
         steeringMotor.stopMotor();

From c827ef04203a5f554b66f03d5dcb13d81886c161 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Fri, 2 Feb 2024 23:51:48 -0800
Subject: [PATCH 12/99] Remove uncessary gyro reset methods as odometry handles
 it for us

---
 .../frc/robot/subsystems/SwerveDrivetrain.java | 18 ------------------
 1 file changed, 18 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index 17e85df..ad49afe 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -255,24 +255,6 @@ public Rotation3d getHeading3d() {
         return gyro.getRotation3d();
     }
 
-    /** Reset the gyro. */
-    public void resetGyro() {
-        gyro.reset();
-    }
-
-    /**
-     * Zero the yaw of the gyro. Can only change zero yaw when sensor is done calibrating
-     * 
-     * @return successful?
-     */
-    public boolean zeroGyroYaw() {
-        if (gyro.isCalibrating()) return false;
-
-        gyro.zeroYaw();
-
-        return true;
-    }
-
     // --- Util ---
 
     /**

From b27ba32324cd60e93d2fea950062015e4f1f5b0a Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Fri, 2 Feb 2024 23:52:19 -0800
Subject: [PATCH 13/99] set swerve module steering motor to brake on idle

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 70f99f5..c810e2c 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -83,6 +83,8 @@ public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steer
         // --- Steering Motor ---
         steeringMotor = new CANSparkMax(steeringMotorDeviceId, MotorType.kBrushless);
 
+        steeringMotor.setIdleMode(IdleMode.kBrake);
+
         // --- Steering Encoder ---
         steeringEncoder = new CANcoder(steeringAbsoluteEncoderId);
 

From fed83a4be29dabd29c1e7b1b51407204a1265cea Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Fri, 2 Feb 2024 23:52:39 -0800
Subject: [PATCH 14/99] create drive to pose command

---
 .../java/frc/robot/commands/DriveToPose.java  | 103 ++++++++++++++++++
 .../frc/robot/commands/DriveTransform.java    |  56 ++++++++++
 2 files changed, 159 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/DriveToPose.java
 create mode 100644 src/main/java/frc/robot/commands/DriveTransform.java

diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java
new file mode 100644
index 0000000..944d6c8
--- /dev/null
+++ b/src/main/java/frc/robot/commands/DriveToPose.java
@@ -0,0 +1,103 @@
+package frc.robot.commands;
+
+import frc.robot.subsystems.SwerveDrivetrain;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Constants.RobotMovementConstants;
+
+/** Command to automatically drive to a certain Pose on the field */
+public class DriveToPose extends Command {
+    private final SwerveDrivetrain drivetrain;
+    private final PIDController xController, yController, rotationController;
+
+    /**
+     * Create a new DriveToPose command. Tries to drive to a certain Pose based on odometry 
+     * 
+     * <p>This drives relative to the robot starting position, so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
+     * where forward is whatever direction the robot started in</p>
+     * 
+     * <p>The last place the drivetrain position was reset counts as the starting position</p>
+     * 
+     * @param drivetrain The drivetrain of the robot
+     * @param targetPose Target pose
+     */
+    public DriveToPose(SwerveDrivetrain drivetrain, Pose2d targetPose) {
+
+        // Save drivetrain
+        this.drivetrain = drivetrain;
+
+        // Setup all PID controllers
+        xController = new PIDController(
+            RobotMovementConstants.TRANSLATION_PID_P,
+            RobotMovementConstants.TRANSLATION_PID_I,
+            RobotMovementConstants.TRANSLATION_PID_D
+        );
+        xController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
+
+        yController = new PIDController(
+            RobotMovementConstants.TRANSLATION_PID_P,
+            RobotMovementConstants.TRANSLATION_PID_I,
+            RobotMovementConstants.TRANSLATION_PID_D
+        );
+        yController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
+
+        rotationController = new PIDController(
+            RobotMovementConstants.ROTATION_PID_P,
+            RobotMovementConstants.ROTATION_PID_I,
+            RobotMovementConstants.ROTATION_PID_D
+        );
+        rotationController.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
+
+        // Set setpoints 
+        xController.setSetpoint(targetPose.getX());
+        yController.setSetpoint(targetPose.getY());
+        rotationController.setSetpoint(targetPose.getRotation().getRadians());
+
+        // Add drivetrain as a requirement so no other commands try to use it
+        addRequirements(this.drivetrain);
+    }
+
+    @Override
+    public void initialize() {
+        // Put all swerve modules in default position
+        drivetrain.toDefaultStates();
+    }
+
+    @Override
+    public void execute() {
+
+        // Get our current pose
+        final Pose2d measuredPosition = drivetrain.getPosition();
+
+        // Put current pose position on SmartDashboard
+        SmartDashboard.putNumber("PoseY", measuredPosition.getY());
+        SmartDashboard.putNumber("PoseX", measuredPosition.getX());
+		SmartDashboard.putNumber("PoseDegrees", measuredPosition.getRotation().getDegrees()); 
+
+        // Calculate our robot speeds with the PID controllers
+        final ChassisSpeeds speeds = new ChassisSpeeds(
+            xController.calculate(measuredPosition.getX()),
+            yController.calculate(measuredPosition.getY()),
+            rotationController.calculate(measuredPosition.getRotation().getRadians())
+        );
+
+        // Set those speeds
+        drivetrain.setDesiredState(speeds);
+    }
+
+    @Override
+    public boolean isFinished() {
+        // Finish once all controllers are within tolerance
+        return xController.atSetpoint() && yController.atSetpoint() && rotationController.atSetpoint();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        // Stop all swerve modules at end
+        drivetrain.stop();
+    }
+}
diff --git a/src/main/java/frc/robot/commands/DriveTransform.java b/src/main/java/frc/robot/commands/DriveTransform.java
new file mode 100644
index 0000000..050024c
--- /dev/null
+++ b/src/main/java/frc/robot/commands/DriveTransform.java
@@ -0,0 +1,56 @@
+package frc.robot.commands;
+
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.subsystems.SwerveDrivetrain;
+
+/** Command to automatically drive a certain transform */
+public class DriveTransform extends Command {
+    private final SwerveDrivetrain drivetrain;
+    private final Transform2d transform;
+
+    /**
+     * Create a new DriveToPose command. Tries to drive a certain transform using the DriveToPose command.
+     * 
+     * <p>This drives relative to the robot, so a transform of +2x and +1y will drive 2 meters forward and 1 meter left,
+     * where forward is whatever direction the robot is currently facing</p>
+     * 
+     * @param drivetrain The drivetrain of the robot
+     * @param transform Target transform to drive to, will be added to current position to get target pose
+     */
+    public DriveTransform(SwerveDrivetrain drivetrain, Transform2d transform) {
+        this.drivetrain = drivetrain;
+        this.transform = transform;
+    }
+    
+    /**
+     * Create a new DriveToPose command. Tries to drive a certain transform (a translation and a rotation) using the DriveToPose command
+     * 
+     * <p>This drives relative to the robot, so a transform of +2x and +1y will drive 2 meters forward and 1 meter left,
+     * where forward is whatever direction the robot is currently facing</p>
+     * 
+     * @param drivetrain The drivetrain of the robot
+     * @param translation Target transform to drive
+     * @param translation Target rotation to drive
+     */
+    public DriveTransform(SwerveDrivetrain drivetrain, Translation2d translation, Rotation2d rotation) {
+        this(drivetrain, new Transform2d(translation, rotation));
+    }    
+
+    @Override
+    public void initialize() {
+        CommandScheduler.getInstance().schedule(
+            new DriveToPose(drivetrain, drivetrain.getPosition().plus(transform))
+        );
+    }
+
+    @Override
+    public boolean isFinished() {
+        // Instantly finish since the entire command just calls another command 
+        return true;
+    }
+}
\ No newline at end of file

From 86842adacb441edba0685bcfad70bc60b08cf4cb Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Fri, 2 Feb 2024 23:55:53 -0800
Subject: [PATCH 15/99] Revert "Remove uncessary gyro reset methods as odometry
 handles it for us"

---
 .../frc/robot/subsystems/SwerveDrivetrain.java | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index ad49afe..17e85df 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -255,6 +255,24 @@ public Rotation3d getHeading3d() {
         return gyro.getRotation3d();
     }
 
+    /** Reset the gyro. */
+    public void resetGyro() {
+        gyro.reset();
+    }
+
+    /**
+     * Zero the yaw of the gyro. Can only change zero yaw when sensor is done calibrating
+     * 
+     * @return successful?
+     */
+    public boolean zeroGyroYaw() {
+        if (gyro.isCalibrating()) return false;
+
+        gyro.zeroYaw();
+
+        return true;
+    }
+
     // --- Util ---
 
     /**

From 0dbc00ed8f44bc7280f2cc81b3f0c7a69795c117 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Sat, 3 Feb 2024 11:56:57 -0800
Subject: [PATCH 16/99] progress on auto drive forward

---
 src/main/java/frc/robot/Constants.java        |  3 +-
 src/main/java/frc/robot/RobotContainer.java   |  3 +-
 .../java/frc/robot/commands/AutoDriveTo.java  |  7 +++
 .../robot/subsystems/SwerveDrivetrain.java    | 48 ++++++++++++-------
 .../frc/robot/subsystems/SwerveModule.java    |  1 +
 5 files changed, 43 insertions(+), 19 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 73e8a58..0eec6cf 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -158,9 +158,10 @@ public static class SwerveModuleConstants {
 
         // Drive PID values
         public static final double DRIVE_PID_P = 0.000006;
-        public static final double DRIVE_PID_I = 0;
+        public static final double DRIVE_PID_I = 0.000001;
         public static final double DRIVE_PID_D = 0;
         public static final double DRIVE_PID_FF = 0.000015;
+        public static final double DRIVE_PID_MAX_I = 0.001;
 
         // Steering PID values
         public static final double STEERING_PID_P = 0.5;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 9cc53f9..55c9499 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -62,7 +62,8 @@ public class RobotContainer {
 
     /** The container for the robot. Contains subsystems, OI devices, and commands. */
     public RobotContainer() {
-        autoChooser.addOption("Testing Auto", Autos.testingAuto(drivetrain));
+        autoChooser.setDefaultOption("Testing Auto", Autos.testingAuto(drivetrain));
+        SmartDashboard.putData(autoChooser);
 
         setUpDriveController();
         configureBindings();
diff --git a/src/main/java/frc/robot/commands/AutoDriveTo.java b/src/main/java/frc/robot/commands/AutoDriveTo.java
index 33d496b..27e9729 100644
--- a/src/main/java/frc/robot/commands/AutoDriveTo.java
+++ b/src/main/java/frc/robot/commands/AutoDriveTo.java
@@ -23,12 +23,16 @@ public class AutoDriveTo extends Command {
 
     private double atSetpointCounter = 0;
 
+    private boolean xOnlyMode;
+
     public AutoDriveTo(SwerveDrivetrain subsystem, Translation2d translation) {
         this.drivetrain = subsystem;
 
         goalX = translation.getX();
         goalY = translation.getY();
 
+        xOnlyMode = Math.abs(goalX) > Math.abs(goalY);
+
         xMovePID = new PIDController(
             RobotMovementConstants.TRANSLATION_PID_P,
             RobotMovementConstants.TRANSLATION_PID_I,
@@ -72,6 +76,9 @@ public void execute() {
         double xSpeed = xMovePID.calculate(x, goalX);
         double ySpeed = yMovePID.calculate(y, goalY);
 
+        if (xOnlyMode) ySpeed = 0;
+        else xSpeed = 0;
+
         final ChassisSpeeds speeds = new ChassisSpeeds(
             xSpeed,
             ySpeed,
diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index 17e85df..ad7aab6 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -165,9 +165,9 @@ public ChassisSpeeds getState() {
 
     /**
      * Set robot relative speeds of robot in meters per second mode.
-     * <p>vx: The velocity of the robot in the x (forward) direction in meter per second.</p>
-     * <p>vy: The velocity of the robot in the y (sideways) direction in meter per second. (Positive values mean the robot is moving to the left).</p>
-     * <p>omega: The angular velocity of the robot in radians per second.</p>
+     * <li>vx: The velocity of the robot in the x (forward) direction in meter per second.</li>
+     * <li>vy: The velocity of the robot in the y (sideways) direction in meter per second. (Positive values mean the robot is moving to the left).</li>
+     * <li>omega: The angular velocity of the robot in radians per second.</li>
      * 
      * @param speeds Desired speeds of drivetrain (using swerve modules)
      */
@@ -177,17 +177,20 @@ public void setDesiredState(ChassisSpeeds speeds) {
 
     /**
      * Set robot relative speeds of robot.
-     * <p>vx: The velocity of the robot in the x (forward) direction in meter per second.</p>
-     * <p>vy: The velocity of the robot in the y (sideways) direction in meter per second. (Positive values mean the robot is moving to the left).</p>
-     * <p>omega: The angular velocity of the robot in radians per second.</p>
+     * <li>vx: The velocity of the robot in the x (forward) direction</li>
+     * <li>vy: The velocity of the robot in the y (sideways) direction. (Positive values mean the robot is moving to the left).</li>
+     * <li>omega: The angular velocity of the robot.</li>
      * 
-     * @param powerDriveMode whether the ChassisSpeeds is in meters per second (false) or motor power (true)
-     * @param speeds Desired speeds of drivetrain (using swerve modules)
+     * <p>
+     * If power drive mode then speeds X, Y, and Omega are in motor powers from -1 to 1.
+     * If normal drive mode then X and Y are in meters per second and Omega is in radians per second
+     * </p>
+     * 
+     * @param speeds desired speeds of drivetrain (using swerve modules)
+     * @param powerDriveMode if {@code true} speeds are in motor power (-1 to 1), if not then they are in default unit
      */
     public void setDesiredState(ChassisSpeeds speeds, boolean powerDriveMode) {
-
         SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);
-
         for (int i = 0; i < modules.length; i++) {
             modules[i].setDesiredState(states[i], powerDriveMode);
         }
@@ -195,17 +198,28 @@ public void setDesiredState(ChassisSpeeds speeds, boolean powerDriveMode) {
 
     /**
      * Set speeds of robot.
-     * <li>vx: The velocity of the robot in the x (forward) direction in meter per second.</li>
-     * <li>vy: The velocity of the robot in the y (sideways) direction in meter per second. (Positive values mean the robot is moving to the left).</li>
-     * <li>omega: The angular velocity of the robot in radians per second.</li>
+     * 
+     * <p>
+     * vx: The velocity of the robot in the x (forward) direction in meter per second.
+     * vy: The velocity of the robot in the y (sideways) direction in meter per second. (Positive values mean the robot is moving to the left).
+     * omega: The angular velocity of the robot in radians per second.
+     * </p>
+     * 
+     * <p>
+     * If power drive mode then speeds X, Y, and Omega are in motor powers from -1 to 1.
+     * If normal drive mode then X and Y are in meters per second and Omega is in radians per second
+     * </p>
+     * 
+     * <p>
+     * If field relative, forward will be directly away from driver, no matter the rotation of the robot.
+     * If robot relative, forward will be whatever direction the robot is facing in.
+     * </p>
      * 
      * @see https://ibb.co/dJrL259
      * 
      * @param speeds Desired speeds of drivetrain (using swerve modules)
-     * @param powerDriveMode whether the ChassisSpeeds is in meters per second (false) or motor power (true)
-     * @param fieldRelative True if the robot is using a field relative coordinate system, false if using a robot relive coordinate system. If field relative, forward will be directly away from driver, no matter the rotation of the robot.
-     * If robot relative, forward will be whatever direction the robot is facing in.
-     */
+     * @param powerDriveMode True if in power drive mode with motor powers, false if in normal drive mode with default units
+     * @param fieldRelative True if the robot is using a field relative coordinate system, false if using a robot relive coordinate system.     */
     public void setDesiredState(ChassisSpeeds speeds, boolean powerDriveMode, boolean fieldRelative) {
 
         if (fieldRelative) {
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index c810e2c..8337b04 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -78,6 +78,7 @@ public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steer
         drivePIDController.setI(SwerveModuleConstants.DRIVE_PID_I);
         drivePIDController.setD(SwerveModuleConstants.DRIVE_PID_D);
         drivePIDController.setFF(SwerveModuleConstants.DRIVE_PID_FF);
+        drivePIDController.setIAccum(SwerveModuleConstants.DRIVE_PID_MAX_I);
         drivePIDController.setOutputRange(-1, 1);
 
         // --- Steering Motor ---

From 0ad2d046420ef2657295dbf8fe80684257e7d220 Mon Sep 17 00:00:00 2001
From: Aleah H <aleahjham@gmail.com>
Date: Sat, 3 Feb 2024 12:13:21 -0800
Subject: [PATCH 17/99] Edit constants for Testbot

---
 src/main/java/frc/robot/Constants.java | 30 +++++++++++++-------------
 src/main/java/frc/robot/Robot.java     |  6 +++++-
 2 files changed, 20 insertions(+), 16 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 73e8a58..396f055 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -35,7 +35,7 @@ public static class RobotDimensions {
                 currentBot = Bot.WOOD_BOT;
                 break;
 
-            case "1": // Practice Bot Serial Number
+            case "03238024": // Practice Bot Serial Number
                 currentBot = Bot.PRACTICE_BOT;
                 break;
             
@@ -115,28 +115,28 @@ public static class SwerveModuleConstants {
             case PRACTICE_BOT:
             default: // Temporary default to practice bot 
                 // Front Left
-                VELOCITY_MOTOR_ID_FL = 41;
-                ANGULAR_MOTOR_ID_FL = 40;
-                ANGULAR_MOTOR_ENCODER_ID_FL = 1;
-                ANGULAR_MOTOR_ENCODER_OFFSET_FL = -0.611328125;
+                VELOCITY_MOTOR_ID_FL = 2;
+                ANGULAR_MOTOR_ID_FL = 3;
+                ANGULAR_MOTOR_ENCODER_ID_FL = 3;
+                ANGULAR_MOTOR_ENCODER_OFFSET_FL = -0.256015325670498;
                 
                 // Front right
-                VELOCITY_MOTOR_ID_FR = 4;
-                ANGULAR_MOTOR_ID_FR = 5;
+                VELOCITY_MOTOR_ID_FR = 16;
+                ANGULAR_MOTOR_ID_FR = 17;
                 ANGULAR_MOTOR_ENCODER_ID_FR = 2;
-                ANGULAR_MOTOR_ENCODER_OFFSET_FR = -0.282958;
+                ANGULAR_MOTOR_ENCODER_OFFSET_FR = -0.248045977011494;
 
                 // Back left
-                VELOCITY_MOTOR_ID_BL = 3;
-                ANGULAR_MOTOR_ID_BL = 2;
+                VELOCITY_MOTOR_ID_BL = 8;
+                ANGULAR_MOTOR_ID_BL = 9;
                 ANGULAR_MOTOR_ENCODER_ID_BL = 4;
-                ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.9260253906;
+                ANGULAR_MOTOR_ENCODER_OFFSET_BL =-0.894674329501916;
                 
                 // Back right
-                VELOCITY_MOTOR_ID_BR = 42;
-                ANGULAR_MOTOR_ID_BR = 6;
-                ANGULAR_MOTOR_ENCODER_ID_BR = 3;
-                ANGULAR_MOTOR_ENCODER_OFFSET_BR = -0.8641367187;   
+                VELOCITY_MOTOR_ID_BR = 10;
+                ANGULAR_MOTOR_ID_BR = 11;
+                ANGULAR_MOTOR_ENCODER_ID_BR = 1;
+                ANGULAR_MOTOR_ENCODER_OFFSET_BR = -0.530498084291188;   
                 break;
 
             // case COMPETITION_BOT:
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 2b7d04f..a4e10fa 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -1,6 +1,8 @@
 package frc.robot;
 
+import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -74,7 +76,9 @@ public void teleopInit() {
 
   /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {}
+  public void teleopPeriodic() {
+    SmartDashboard.putString("Serial Number", RobotController.getSerialNumber());
+  }
 
   @Override
   public void testInit() {

From afa2bc5773c8200bd265fbe98ce9967b0258da52 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Sat, 3 Feb 2024 14:00:10 -0800
Subject: [PATCH 18/99] Change robot dimensions for practice bot

---
 src/main/java/frc/robot/Constants.java      | 29 +++++++++++++++------
 src/main/java/frc/robot/commands/Autos.java |  7 ++---
 2 files changed, 25 insertions(+), 11 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 075a1ee..37073b0 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -11,12 +11,7 @@
  */
 public final class Constants {
     public static enum Bot { WOOD_BOT, PRACTICE_BOT, COMPETITION_BOT }
-
-    public static class RobotDimensions {
-        public static final double ROBOT_LENGTH_X = 1;
-        public static final double ROBOT_LENGTH_Y = 1;
-    }
-
+    
     public static final Bot currentBot;
 
     /**
@@ -194,9 +189,27 @@ public static class SwerveModuleConstants {
     }
 
     public static class SwerveDrivetrainConstants {
+        static { switch (currentBot) {
+            case WOOD_BOT:
+                MODULE_LOCATION_X = 26.0 / 100;
+                MODULE_LOCATION_Y = 28.5 / 100;
+                break;
+        
+            case PRACTICE_BOT:
+            default: // Temporary default to practice bot 
+                MODULE_LOCATION_X = 54 / 100;
+                MODULE_LOCATION_Y = 54 / 100;
+                break;
+
+            // case COMPETITION_BOT:
+            // default:
+            
+            //     break;
+        }}
+
         // distance of swerve modules from center of robot, in meters
-        public static final double MODULE_LOCATION_Y = 28.5 / 100;
-        public static final double MODULE_LOCATION_X = 26.0 / 100;
+        public static final double MODULE_LOCATION_Y;
+        public static final double MODULE_LOCATION_X;
     }
 }
 
diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java
index b3bbd48..6b2ff2c 100644
--- a/src/main/java/frc/robot/commands/Autos.java
+++ b/src/main/java/frc/robot/commands/Autos.java
@@ -1,12 +1,13 @@
 package frc.robot.commands;
 
 import frc.robot.subsystems.SwerveDrivetrain;
+
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants.SwerveDrivetrainConstants;
 import frc.robot.subsystems.Arm;
-import frc.robot.Constants.RobotDimensions;
 
 public final class Autos {
     /** Example static factory for an autonomous command. */
@@ -31,8 +32,8 @@ public static Command startingAuto(Arm arm, SwerveDrivetrain drivetrain, boolean
                 // right in front; 0.05 can be changed, it's for the amount of extra spacing
                 // that we want
                 new AutoDriveTo(drivetrain,
-                        new Translation2d(2.9 - 0.2 - RobotDimensions.ROBOT_LENGTH_X - 0.05,
-                                invert * (1.2 - RobotDimensions.ROBOT_LENGTH_Y))),
+                        new Translation2d(2.9 - 0.2 - SwerveDrivetrainConstants.MODULE_LOCATION_X - 0.05,
+                                invert * (1.2 - SwerveDrivetrainConstants.MODULE_LOCATION_Y))),
 
                 new AutoRotateTo(drivetrain, new Rotation2d(Math.PI / -2 * invert)));
     }

From 5bcc56d78d2617fe1df7e611445076d514436f28 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Sat, 3 Feb 2024 14:40:06 -0800
Subject: [PATCH 19/99] Fix auto switching of controller

Co-Authored-By: AikiKapila <155595929+AikiKapila@users.noreply.github.com>
---
 simgui.json                                 |  3 ++-
 src/main/java/frc/robot/Constants.java      |  5 +++--
 src/main/java/frc/robot/Robot.java          |  8 +++-----
 src/main/java/frc/robot/RobotContainer.java | 21 +++++++++++----------
 4 files changed, 19 insertions(+), 18 deletions(-)

diff --git a/simgui.json b/simgui.json
index 215b61d..f47076a 100644
--- a/simgui.json
+++ b/simgui.json
@@ -1,7 +1,8 @@
 {
   "NTProvider": {
     "types": {
-      "/FMSInfo": "FMSInfo"
+      "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/SendableChooser[0]": "String Chooser"
     }
   },
   "NetworkTables Info": {
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 37073b0..cfe6bad 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -13,6 +13,7 @@ public final class Constants {
     public static enum Bot { WOOD_BOT, PRACTICE_BOT, COMPETITION_BOT }
     
     public static final Bot currentBot;
+    public static final String serialNumber;
 
     /**
      * This code determines what bot is being deployed and sets constants accordingly.
@@ -23,7 +24,7 @@ public static enum Bot { WOOD_BOT, PRACTICE_BOT, COMPETITION_BOT }
      * @author Aceius E.
      */
     static {
-        final String serialNumber = RobotBase.isReal() ? RobotController.getSerialNumber() : "default";
+        serialNumber = RobotBase.isReal() ? RobotController.getSerialNumber() : "-default-";
 
         switch (serialNumber) {
             case "03282B00": // Wood Bot Serial Number
@@ -34,7 +35,7 @@ public static enum Bot { WOOD_BOT, PRACTICE_BOT, COMPETITION_BOT }
                 currentBot = Bot.PRACTICE_BOT;
                 break;
             
-            case "2": // Competition Bot Serial Number
+            case "-default-": // Competition Bot Serial Number
             default:  // Also use competition bot as default
                 currentBot = Bot.COMPETITION_BOT;
                 break;
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index a4e10fa..3f7ba3a 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -1,8 +1,6 @@
 package frc.robot;
 
-import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
@@ -72,13 +70,13 @@ public void teleopInit() {
     if (autonomousCommand != null) {
       autonomousCommand.cancel();
     }
+
+    robotContainer.setUpDriveController();
   }
 
   /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-    SmartDashboard.putString("Serial Number", RobotController.getSerialNumber());
-  }
+  public void teleopPeriodic() { }
 
   @Override
   public void testInit() {
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 55c9499..39fb930 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,6 +4,7 @@
 import frc.robot.Constants.SwerveDrivetrainConstants;
 import frc.robot.Constants.SwerveModuleConstants;
 import frc.robot.commands.Autos;
+import frc.robot.commands.SwerveRemoteOperation.SwerveDriveBaseControl;
 import frc.robot.commands.SwerveRemoteOperation.SwerveDriveJoystickControl;
 import frc.robot.commands.SwerveRemoteOperation.SwerveDriveXboxControl;
 import frc.robot.subsystems.SwerveDrivetrain;
@@ -65,28 +66,28 @@ public RobotContainer() {
         autoChooser.setDefaultOption("Testing Auto", Autos.testingAuto(drivetrain));
         SmartDashboard.putData(autoChooser);
 
-        setUpDriveController();
         configureBindings();
     }
 
     public void setUpDriveController() {
         // Create joysticks
         final GenericHID genericHID = new GenericHID(DriverConstants.DRIVER_JOYSTICK_PORT);
-        // final HIDType genericHIDType = genericHID.getType();
-        final HIDType genericHIDType = GenericHID.HIDType.kHIDJoystick;
+        final HIDType genericHIDType = genericHID.getType();
 
         SmartDashboard.putString("Drive Controller", genericHIDType.toString());
-        SmartDashboard.putString("Bot Name", Constants.currentBot.toString());
+        SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " " + Constants.serialNumber);
+
+        drivetrain.removeDefaultCommand();
+
+        SwerveDriveBaseControl control;
 
         if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
-            final CommandJoystick driverJoystick = new CommandJoystick(genericHID.getPort());
-            SwerveDriveJoystickControl control = new SwerveDriveJoystickControl(drivetrain, driverJoystick);
-            drivetrain.setDefaultCommand(control);
+            control = new SwerveDriveJoystickControl(drivetrain, new CommandJoystick(genericHID.getPort()));
         } else {
-            final CommandXboxController xboxController = new CommandXboxController(genericHID.getPort());
-            SwerveDriveXboxControl control = new SwerveDriveXboxControl(drivetrain, xboxController);
-            drivetrain.setDefaultCommand(control);
+            control = new SwerveDriveXboxControl(drivetrain, new CommandXboxController(genericHID.getPort()));
         }
+
+        drivetrain.setDefaultCommand(control);
     }
 
     /** Use this method to define your trigger->command mappings. */

From 81566513e36fc9087075fa279bed0b9ec76bc75d Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Sat, 3 Feb 2024 15:30:29 -0800
Subject: [PATCH 20/99] change to setting up in robot container as well

---
 src/main/java/frc/robot/RobotContainer.java | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 39fb930..ab16728 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -67,6 +67,8 @@ public RobotContainer() {
         SmartDashboard.putData(autoChooser);
 
         configureBindings();
+
+        setUpDriveController();
     }
 
     public void setUpDriveController() {

From c5cdf9cf7d4bb2aea05ba7023afccc9c90bfab22 Mon Sep 17 00:00:00 2001
From: AikiKapila <aikikapila@seattleacademy.org>
Date: Sat, 3 Feb 2024 15:47:26 -0800
Subject: [PATCH 21/99] Create FollowTag.java

---
 .../java/frc/robot/commands/FollowTag.java    | 47 +++++++++++++++++++
 1 file changed, 47 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/FollowTag.java

diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java
new file mode 100644
index 0000000..96d4485
--- /dev/null
+++ b/src/main/java/frc/robot/commands/FollowTag.java
@@ -0,0 +1,47 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.RobotMovementConstants;
+import frc.robot.subsystems.SwerveDrivetrain;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Translation2d;
+import frc.robot.Constants.RobotMovementConstants;
+
+public class FollowTag extends Command {
+    private final SwerveDrivetrain drivetrain;
+    private final int tagID;
+    private final Translation2d targetDistance;
+    private final PIDController xController, yController, rotationController;
+
+    public FollowTag(SwerveDrivetrain drivetrain, int tagID, Translation2d targetDistanceToTag) {
+        this.drivetrain = drivetrain;
+        this.tagID = tagID;
+        this.targetDistance = targetDistanceToTag;
+
+        xController = new PIDController(
+            RobotMovementConstants.TRANSLATION_PID_P,
+            RobotMovementConstants.TRANSLATION_PID_I,
+            RobotMovementConstants.TRANSLATION_PID_D
+        );
+        xController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
+
+        yController = new PIDController(
+            RobotMovementConstants.TRANSLATION_PID_P,
+            RobotMovementConstants.TRANSLATION_PID_I,
+            RobotMovementConstants.TRANSLATION_PID_D
+        );
+        yController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
+
+        rotationController = new PIDController(
+            RobotMovementConstants.ROTATION_PID_P,
+            RobotMovementConstants.ROTATION_PID_I,
+            RobotMovementConstants.ROTATION_PID_D
+        );
+        rotationController.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
+    }
+
+    @Override
+    public void execute() {
+        
+    }
+}

From 6f212f5634a990fd3108187612ddae90ce5f879d Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Sun, 4 Feb 2024 22:15:14 -0800
Subject: [PATCH 22/99] rename teleop control commands

---
 src/main/java/frc/robot/RobotContainer.java          | 12 ++++++------
 ...{SwerveDriveBaseControl.java => BaseControl.java} |  4 ++--
 ...riveJoystickControl.java => JoystickControl.java} |  6 +++---
 .../{SwerveDrivePS4Control.java => PS4Control.java}  |  4 ++--
 ...{SwerveDriveXboxControl.java => XboxControl.java} |  4 ++--
 5 files changed, 15 insertions(+), 15 deletions(-)
 rename src/main/java/frc/robot/commands/SwerveRemoteOperation/{SwerveDriveBaseControl.java => BaseControl.java} (95%)
 rename src/main/java/frc/robot/commands/SwerveRemoteOperation/{SwerveDriveJoystickControl.java => JoystickControl.java} (93%)
 rename src/main/java/frc/robot/commands/SwerveRemoteOperation/{SwerveDrivePS4Control.java => PS4Control.java} (92%)
 rename src/main/java/frc/robot/commands/SwerveRemoteOperation/{SwerveDriveXboxControl.java => XboxControl.java} (94%)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index ab16728..b9b2d40 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,9 +4,9 @@
 import frc.robot.Constants.SwerveDrivetrainConstants;
 import frc.robot.Constants.SwerveModuleConstants;
 import frc.robot.commands.Autos;
-import frc.robot.commands.SwerveRemoteOperation.SwerveDriveBaseControl;
-import frc.robot.commands.SwerveRemoteOperation.SwerveDriveJoystickControl;
-import frc.robot.commands.SwerveRemoteOperation.SwerveDriveXboxControl;
+import frc.robot.commands.SwerveRemoteOperation.BaseControl;
+import frc.robot.commands.SwerveRemoteOperation.JoystickControl;
+import frc.robot.commands.SwerveRemoteOperation.XboxControl;
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.SwerveModule;
 
@@ -81,12 +81,12 @@ public void setUpDriveController() {
 
         drivetrain.removeDefaultCommand();
 
-        SwerveDriveBaseControl control;
+        BaseControl control;
 
         if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
-            control = new SwerveDriveJoystickControl(drivetrain, new CommandJoystick(genericHID.getPort()));
+            control = new JoystickControl(drivetrain, new CommandJoystick(genericHID.getPort()));
         } else {
-            control = new SwerveDriveXboxControl(drivetrain, new CommandXboxController(genericHID.getPort()));
+            control = new XboxControl(drivetrain, new CommandXboxController(genericHID.getPort()));
         }
 
         drivetrain.setDefaultCommand(control);
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/BaseControl.java
similarity index 95%
rename from src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java
rename to src/main/java/frc/robot/commands/SwerveRemoteOperation/BaseControl.java
index 74dd343..9beb53e 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/BaseControl.java
@@ -10,7 +10,7 @@
 /**
  * This can be the default command for the drivetrain, allowing for remote operation with a controller
  */
-public abstract class SwerveDriveBaseControl extends Command {
+public abstract class BaseControl extends Command {
    protected final SwerveDrivetrain drivetrain;
    protected final CommandGenericHID controller;
 
@@ -20,7 +20,7 @@ public abstract class SwerveDriveBaseControl extends Command {
     * @param drivetrain       The drivetrain of the robot
     * @param driverController The device used to control drivetrain
     */
-   public SwerveDriveBaseControl(SwerveDrivetrain drivetrain, CommandGenericHID driverController) {
+   public BaseControl(SwerveDrivetrain drivetrain, CommandGenericHID driverController) {
 
       // save parameters
       this.controller = driverController;
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java
similarity index 93%
rename from src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
rename to src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java
index f3007fd..b643608 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java
@@ -12,7 +12,7 @@
 /**
  * This is the default command for the drivetrain, allowing for remote operation with joystick
  */
-public class SwerveDriveJoystickControl extends SwerveDriveBaseControl {
+public class JoystickControl extends BaseControl {
     private final OptionButton preciseModeButton;
     private final OptionButton boostModeButton;
     private final OptionButton fieldRelativeButton;
@@ -23,7 +23,7 @@ public class SwerveDriveJoystickControl extends SwerveDriveBaseControl {
 	 * @param drivetrain The drivetrain of the robot
 	 * @param driverJoystick The joystick used to control drivetrain
 	 */
-    public SwerveDriveJoystickControl(SwerveDrivetrain drivetrain, CommandJoystick driverJoystick) {
+    public JoystickControl(SwerveDrivetrain drivetrain, CommandJoystick driverJoystick) {
         super(drivetrain, driverJoystick);
 
         // Create and configure buttons
@@ -31,7 +31,7 @@ public SwerveDriveJoystickControl(SwerveDrivetrain drivetrain, CommandJoystick d
         boostModeButton = new OptionButton(driverJoystick, 1, ActivationMode.HOLD);
         fieldRelativeButton = new OptionButton(driverJoystick, 3, ActivationMode.TOGGLE);
 
-        driverJoystick.button(5).onTrue(new InstantCommand(drivetrain::zeroGyroYaw));
+        driverJoystick.button(5).onTrue(new InstantCommand(drivetrain::resetPosition));
     }
 
     @Override
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java
similarity index 92%
rename from src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java
rename to src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java
index f406327..8caab35 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java
@@ -11,7 +11,7 @@
 /**
  * This is the default command for the drivetrain, allowing for remote operation with PS4 controller
  */
-public class SwerveDrivePS4Control extends SwerveDriveBaseControl {
+public class PS4Control extends BaseControl {
     private final OptionButton preciseModeButton;
     private final OptionButton boostModeButton;
 
@@ -21,7 +21,7 @@ public class SwerveDrivePS4Control extends SwerveDriveBaseControl {
      * @param drivetrain The drive train of the robot
      * @param driverPS4Controller The PS4 controller used to control the drive train
      */
-    public SwerveDrivePS4Control(SwerveDrivetrain drivetrain, CommandPS4Controller driverPS4Controller){
+    public PS4Control(SwerveDrivetrain drivetrain, CommandPS4Controller driverPS4Controller){
         super(drivetrain, driverPS4Controller);
 
         // Create and configure buttons
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/XboxControl.java
similarity index 94%
rename from src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
rename to src/main/java/frc/robot/commands/SwerveRemoteOperation/XboxControl.java
index 9691080..d3e617a 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/XboxControl.java
@@ -14,7 +14,7 @@
 /**
  * This is the default command for the drivetrain, allowing for remote operation with xbox controller
  */
-public class SwerveDriveXboxControl extends SwerveDriveBaseControl {
+public class XboxControl extends BaseControl {
     private final OptionButton preciseModeButton;
     private final OptionButton boostModeButton;
     private final OptionButton fieldRelativeButton;
@@ -27,7 +27,7 @@ public class SwerveDriveXboxControl extends SwerveDriveBaseControl {
      * @param drivetrain           The drivetrain of the robot
      * @param driverXboxController The xbox controller used to control drivetrain
      */
-    public SwerveDriveXboxControl(SwerveDrivetrain drivetrain, CommandXboxController driverXboxController) {
+    public XboxControl(SwerveDrivetrain drivetrain, CommandXboxController driverXboxController) {
         super(drivetrain, driverXboxController);
         
         // Create and configure buttons

From 31079ae6f7c1103d79ad20f7a2afd57c1ad7df5f Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Sun, 4 Feb 2024 23:26:23 -0800
Subject: [PATCH 23/99] clean up drivetrain set desired state code

---
 .../JoystickControl.java                      |   2 +-
 .../SwerveRemoteOperation/PS4Control.java     |   2 +-
 .../SwerveRemoteOperation/XboxControl.java    |   2 +-
 .../robot/subsystems/SwerveDrivetrain.java    | 119 ++++++++----------
 .../frc/robot/subsystems/SwerveModule.java    |  60 ++++-----
 5 files changed, 74 insertions(+), 111 deletions(-)

diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java
index b643608..b617a95 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java
@@ -63,6 +63,6 @@ public void execute() {
         SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);        
         SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
 
-        drivetrain.setDesiredState(speeds, true, isFieldRelative);
+        drivetrain.setDesiredState(speeds, isFieldRelative, true);
     }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java
index 8caab35..040f355 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java
@@ -54,6 +54,6 @@ public void execute(){
         SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);        
         SmartDashboard.putBoolean("Field Relieve", false);
 
-        drivetrain.setDesiredState(speeds,  true, false);
+        drivetrain.setDesiredState(speeds, false, true);
     }
 }
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/XboxControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/XboxControl.java
index d3e617a..e5c139a 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/XboxControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/XboxControl.java
@@ -66,6 +66,6 @@ public void execute() {
         SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);        
         SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
 
-        drivetrain.setDesiredState(speeds, true, isFieldRelative);
+        drivetrain.setDesiredState(speeds, isFieldRelative, true);
     }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index ad7aab6..79aab4a 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -88,11 +88,11 @@ public SwerveDrivetrain(AHRS gyro, SwerveModule swerveModuleFL, SwerveModule swe
             modulesMap(SwerveModule::getPosition, SwerveModulePosition[]::new)
         );
 
+        // Set up name and children for sendable registry
+        setName(toString());
         for (SwerveModule module : modules) {
             addChild(module.getName(), module);
         }
-
-        pose = new Pose2d();
     }
 
     // --- Pose Related Methods ---
@@ -104,11 +104,14 @@ public SwerveDrivetrain(AHRS gyro, SwerveModule swerveModuleFL, SwerveModule swe
     @Override
     public void periodic() {
         // use odometry to update the estimated pose
-        pose = odometry.update(getHeading(), getWheelPositions());
+        pose = odometry.update(
+            getHeading(),
+            getWheelPositions()
+        );
     }
 
     /**
-     * Get the pose of robot, as calculated by 
+     * Get the pose of robot, as calculated by odometry from gyro and distances swerve modules have traveled
      * 
      * @return The current positions of the robot, contains translational and rotational elements.
      */
@@ -122,7 +125,7 @@ public Pose2d getPosition() {
      */
     public void resetPosition() {
         odometry.resetPosition(
-            gyro.getRotation2d(),
+            getHeading(),
             getWheelPositions(),
             pose
         );
@@ -143,90 +146,81 @@ public void toDefaultStates() {
     }
 
 
-    // --- Chassis Speeds to Swerve Module State Methods ---
-
+    // --- Chassis Speeds to Swerve Module States methods ---
 
     /**
-     * Get speeds of robot.
-     * <p>vx: The velocity of the robot in the x (forward) direction in meter per second.</p>
-     * <p>vy: The velocity of the robot in the y (sideways) direction in meter per second. (Positive values mean the robot is moving to the left).</p>
-     * <p>omega: The angular velocity of the robot in radians per second.</p>
+     * Get field relative speeds of robot.
      * 
      * @return Speeds of drivetrain (from swerve modules)
      */
     public ChassisSpeeds getState() {
         // get all module states and convert them into chassis speeds
-        ChassisSpeeds speeds = kinematics.toChassisSpeeds(
+        return kinematics.toChassisSpeeds(
             modulesMap(SwerveModule::getState, SwerveModuleState[]::new)
         );
+    }
 
+    /**
+     * Get speeds of robot.
+     * 
+     * @param fieldRelative True if the robot is using a field relative coordinate system, false if using a robot relive coordinate system.
+     * @return Speeds of drivetrain (from swerve modules)
+     */
+    public ChassisSpeeds getState(boolean fieldRelative) {
+        ChassisSpeeds speeds = getState();
+        if (fieldRelative) speeds = ChassisSpeeds.fromRobotRelativeSpeeds(speeds, getHeading());
         return speeds;
     }
 
     /**
-     * Set robot relative speeds of robot in meters per second mode.
-     * <li>vx: The velocity of the robot in the x (forward) direction in meter per second.</li>
-     * <li>vy: The velocity of the robot in the y (sideways) direction in meter per second. (Positive values mean the robot is moving to the left).</li>
-     * <li>omega: The angular velocity of the robot in radians per second.</li>
+     * Set robot relative speeds of robot using default speeds units.
      * 
-     * @param speeds Desired speeds of drivetrain (using swerve modules)
+     * @param fieldRelative True if the robot is using a field relative coordinate system, false if using a robot relive coordinate syste
      */
     public void setDesiredState(ChassisSpeeds speeds) {
         setDesiredState(speeds, false);
     }
 
     /**
-     * Set robot relative speeds of robot.
-     * <li>vx: The velocity of the robot in the x (forward) direction</li>
-     * <li>vy: The velocity of the robot in the y (sideways) direction. (Positive values mean the robot is moving to the left).</li>
-     * <li>omega: The angular velocity of the robot.</li>
+     * Set speeds of robot using default speeds units.
      * 
-     * <p>
-     * If power drive mode then speeds X, Y, and Omega are in motor powers from -1 to 1.
-     * If normal drive mode then X and Y are in meters per second and Omega is in radians per second
-     * </p>
-     * 
-     * @param speeds desired speeds of drivetrain (using swerve modules)
-     * @param powerDriveMode if {@code true} speeds are in motor power (-1 to 1), if not then they are in default unit
+     * @param speeds Desired speeds of drivetrain (using swerve modules)
+     * @param fieldRelative True if the robot is using a field relative coordinate system, false if using a robot relive coordinate syste
      */
-    public void setDesiredState(ChassisSpeeds speeds, boolean powerDriveMode) {
-        SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);
-        for (int i = 0; i < modules.length; i++) {
-            modules[i].setDesiredState(states[i], powerDriveMode);
-        }
+    public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative) {
+        setDesiredState(speeds, fieldRelative, false);
     }
 
     /**
      * Set speeds of robot.
      * 
      * <p>
-     * vx: The velocity of the robot in the x (forward) direction in meter per second.
-     * vy: The velocity of the robot in the y (sideways) direction in meter per second. (Positive values mean the robot is moving to the left).
-     * omega: The angular velocity of the robot in radians per second.
-     * </p>
-     * 
-     * <p>
-     * If power drive mode then speeds X, Y, and Omega are in motor powers from -1 to 1.
-     * If normal drive mode then X and Y are in meters per second and Omega is in radians per second
-     * </p>
+     * Vx: the velocity of the robot in the x (forward) direction in meter per second.
+     * Vy: the velocity of the robot in the y (sideways) direction in meter per second. (Positive values mean the robot is moving to the left).
+     * Omega: the angular velocity of the robot in radians per second.
      * 
      * <p>
      * If field relative, forward will be directly away from driver, no matter the rotation of the robot.
      * If robot relative, forward will be whatever direction the robot is facing in.
-     * </p>
      * 
-     * @see https://ibb.co/dJrL259
+     * <p>
+     * If power drive mode then speeds X, Y, and Omega are in motor powers from -1 to 1.
+     * If normal drive mode then X and Y are in meters per second and Omega is in radians per second
      * 
      * @param speeds Desired speeds of drivetrain (using swerve modules)
+     * @param fieldRelative True if the robot is using a field relative coordinate system, false if using a robot relive coordinate system.
      * @param powerDriveMode True if in power drive mode with motor powers, false if in normal drive mode with default units
-     * @param fieldRelative True if the robot is using a field relative coordinate system, false if using a robot relive coordinate system.     */
-    public void setDesiredState(ChassisSpeeds speeds, boolean powerDriveMode, boolean fieldRelative) {
+     */
+    public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative, boolean powerDriveMode) {
 
-        if (fieldRelative) {
-            speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getHeading());
-        }
+        if (fieldRelative) speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getHeading());
+
+        setDesiredState(speeds);
 
-        setDesiredState(speeds, powerDriveMode);
+        SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);
+        for (int i = 0; i < modules.length; i++) {
+            modules[i].setDesiredState(states[i], powerDriveMode);
+        }
     }
     
     /**
@@ -269,24 +263,6 @@ public Rotation3d getHeading3d() {
         return gyro.getRotation3d();
     }
 
-    /** Reset the gyro. */
-    public void resetGyro() {
-        gyro.reset();
-    }
-
-    /**
-     * Zero the yaw of the gyro. Can only change zero yaw when sensor is done calibrating
-     * 
-     * @return successful?
-     */
-    public boolean zeroGyroYaw() {
-        if (gyro.isCalibrating()) return false;
-
-        gyro.zeroYaw();
-
-        return true;
-    }
-
     // --- Util ---
 
     /**
@@ -313,5 +289,10 @@ private <T> T[] modulesMap(Function<? super SwerveModule, ? extends T> func, Int
         // and returns something of type T or something lower on the inheritance chain. (For example if T is Object: Integer)
         // Also takes a T array initializer
         return Arrays.stream(modules).map(func).toArray(arrayInitializer);
-    }   
+    }  
+    
+    @Override
+    public String toString() {
+        return "SwerveDrivetrain";
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 8337b04..a859b90 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -46,6 +46,9 @@ public class SwerveModule extends SubsystemBase {
     /** Locations of the wheel relative to the physical center of the robot. */
     private final Translation2d distanceFromCenter;
 
+    /** Whether swerve module is stopped */
+    private boolean stopped = false;
+
     /**
      * Constructor for an individual Swerve Module.
      * Sets up both drive and angular motor for swerve module as well as systems to monitor and control them
@@ -120,14 +123,18 @@ public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steer
      */
     @Override
     public void periodic() {
-        // Calculate how fast to spin the motor to get to the desired angle using our PID controller, then set the motor to spin at that speed
-        steeringMotor.set(steeringPIDController.calculate(getSteeringAngleRotations()));
+        if (!stopped) {
+            // Calculate how fast to spin the motor to get to the desired angle using our PID controller, then set the motor to spin at that speed
+            steeringMotor.set(steeringPIDController.calculate(getSteeringAngleRotations()));
+        }
     }
 
     // --- Direct control methods ---
 
-    /** Stop drive and steering motor of swerve module and set desired state to nothing */
+    /** Stop drive and steering motor of swerve module, module can be moved again by calling setDesiredState. */
     public void stop() {
+        stopped = true;
+
         // Manually stop both motors in swerve module
         driveMotor.stopMotor();
         steeringMotor.stopMotor();
@@ -161,14 +168,18 @@ public SwerveModuleState getState() {
      * 
      * @param state New state of swerve module, contains speed in meters per second and angle as {@link Rotation2d}
      * @param powerDriveMode whether the SwerveModuleState is in meters per second (false) or motor power (true)
-     * @param shouldOptimize Whether to optimize the way the swerve module gets to the desired state
      */
-    public void setDesiredState(SwerveModuleState state, boolean powerDriveMode, boolean shouldOptimize) {
-        if (shouldOptimize && state != null) {
-            // Optimize the reference state to avoid spinning further than 90 degrees
-            state = optimize(state,  getState().angle);
+    public void setDesiredState(SwerveModuleState state, boolean powerDriveMode) {
+
+        // If state is null, then stop robot and don't set any states
+        if (state == null) {
+            stop();
+            return;
         }
 
+        // Optimize the reference state to avoid spinning further than 90 degrees
+        state = optimize(state, Rotation2d.fromRotations(getSteeringAngleRotations()));
+        
         // --- Set steering motor ---
         steeringPIDController.setSetpoint(state.angle.getRotations());
         
@@ -197,17 +208,6 @@ else if (powerDriveMode) {
         }
     }
 
-    /**
-     * Set the state of the swerve module. Will automatically optimize.
-     * The state is the speed and angle of the swerve module.
-     * You can use {@code Rotation2d.fromDegrees()} to create angle.
-     * 
-     * @param state New state of swerve module, contains speed in meters per second and angle.
-     */
-    public void setDesiredState(SwerveModuleState state, boolean powerDriveMode) {
-        setDesiredState(state, powerDriveMode, true);
-    }
-
     // --- Public info getters ---
 
     /**
@@ -233,22 +233,13 @@ public Translation2d getDistanceFromCenter() {
 
     // --- Private getters ---
 
-    /**
-     * Get the velocity of the drive motor.
-     * 
-     * @return Rotations per minute of the drive motor
-     */
-    private double getDriveSpeedRotationsPerMinute() {
-        return driveEncoder.getVelocity();
-    }
-
     /**
      * Get the velocity of the drive motor.
      * 
      * @return Meters per second of the drive wheel
      */
     private double getDriveSpeedMetersPerSecond() {
-        return (SwerveModuleConstants.WHEEL_CIRCUMFERENCE * getDriveSpeedRotationsPerMinute()) / 60;
+        return (SwerveModuleConstants.WHEEL_CIRCUMFERENCE * driveEncoder.getVelocity()) / 60;
     }
 
     /**
@@ -257,16 +248,7 @@ private double getDriveSpeedMetersPerSecond() {
      * @return Meters traveled by the drive wheel
      */
     private double getDriveDistanceMeters() {
-        return SwerveModuleConstants.WHEEL_CIRCUMFERENCE * getDriveDistanceRotations();
-    }
-
-    /**
-     * Get the position of the drive motor.
-     * 
-     * @return Total number of rotations of the drive motor
-     */
-    private double getDriveDistanceRotations() {
-        return driveEncoder.getPosition();
+        return SwerveModuleConstants.WHEEL_CIRCUMFERENCE * driveEncoder.getPosition();
     }
 
     /**

From 6c87fb6c04344447be1956f0d86ba401612af5f0 Mon Sep 17 00:00:00 2001
From: Aceius E <aceiuselms@seattleacademy.org>
Date: Mon, 5 Feb 2024 16:07:10 -0800
Subject: [PATCH 24/99] Basic style guide / auto-formatter

---
 .vscode/settings.json | 75 +++++++++++++++++++++----------------------
 1 file changed, 36 insertions(+), 39 deletions(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 30da363..0c4ad71 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,40 +1,37 @@
 {
-  "java.configuration.updateBuildConfiguration": "automatic",
-  "java.server.launchMode": "Standard",
-  "files.exclude": {
-    "**/.git": true,
-    "**/.svn": true,
-    "**/.hg": true,
-    "**/CVS": true,
-    "**/.DS_Store": true,
-    "bin/": true,
-    "**/.classpath": true,
-    "**/.project": true,
-    "**/.settings": true,
-    "**/.factorypath": true,
-    "**/*~": true
-  },
-  "java.test.config": [
-    {
-      "name": "WPIlibUnitTests",
-      "workingDirectory": "${workspaceFolder}/build/jni/release",
-      "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ],
-      "env": {
-        "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" ,
-        "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release"
-      }
-    },
-  ],
-  "java.test.defaultConfig": "WPIlibUnitTests",
-  "cSpell.words": [
-    "AHRS",
-    "Brushless",
-    "Deadzone",
-    "Odometry",
-    "Robo",
-    "setpoint",
-    "teleop",
-    "teleoperated"
-  ]
-}
-
+	"java.configuration.updateBuildConfiguration": "automatic",
+	"java.server.launchMode": "Standard",
+	"files.exclude": {
+		"**/.git": true,
+		"**/.svn": true,
+		"**/.hg": true,
+		"**/CVS": true,
+		"**/.DS_Store": true,
+		"bin/": true,
+		"**/.classpath": true,
+		"**/.project": true,
+		"**/.settings": true,
+		"**/.factorypath": true,
+		"**/*~": true
+	},
+	"java.test.config": [
+		{
+			"name": "WPIlibUnitTests",
+			"workingDirectory": "${workspaceFolder}/build/jni/release",
+			"vmargs": [
+				"-Djava.library.path=${workspaceFolder}/build/jni/release"
+			],
+			"env": {
+				"LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release",
+				"DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release"
+			}
+		},
+	],
+	"java.test.defaultConfig": "WPIlibUnitTests",
+	// Style guide - most of this came from clicking buttons and then copying from my user settings.json
+	// General editor stuff
+	"editor.formatOnSave": true, // So you cant forget
+	"editor.tabSize": 4, // 4 char width indentation
+	"editor.detectIndentation": false, // Prevent above from being overridden
+	"editor.insertSpaces": false // use tab character instead of spaces
+}
\ No newline at end of file

From f4a14e0284c654525dddb98563d892b2e62f3a86 Mon Sep 17 00:00:00 2001
From: Aceius E <aceiuselms@seattleacademy.org>
Date: Mon, 5 Feb 2024 16:19:41 -0800
Subject: [PATCH 25/99] Add @Override annotation automatically

Because we do a lot of that when extending the base classes for subsystems and commands it just seems convenient.
---
 .vscode/settings.json | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 0c4ad71..8d9a808 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -33,5 +33,8 @@
 	"editor.formatOnSave": true, // So you cant forget
 	"editor.tabSize": 4, // 4 char width indentation
 	"editor.detectIndentation": false, // Prevent above from being overridden
-	"editor.insertSpaces": false // use tab character instead of spaces
+	"editor.insertSpaces": false, // use tab character instead of spaces
+	"java.cleanup.actionsOnSave": [
+		"addOverride",
+	]
 }
\ No newline at end of file

From cf222b3cf1c1404e8dd2d795925531f441a1266f Mon Sep 17 00:00:00 2001
From: Aceius E <aceiuselms@seattleacademy.org>
Date: Mon, 5 Feb 2024 16:24:57 -0800
Subject: [PATCH 26/99] Apply Autoformat
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Cause Michaël told me to

Co-Authored-By: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
---
 src/main/java/frc/robot/Constants.java        | 388 +++++-----
 src/main/java/frc/robot/Main.java             |  27 +-
 src/main/java/frc/robot/Robot.java            | 197 ++---
 src/main/java/frc/robot/RobotContainer.java   | 106 +--
 .../java/frc/robot/commands/AutoDriveTo.java  | 119 +--
 .../java/frc/robot/commands/AutoRotateTo.java |  70 +-
 src/main/java/frc/robot/commands/Autos.java   |  62 +-
 .../java/frc/robot/commands/DriveToPose.java  | 182 ++---
 .../frc/robot/commands/DriveTransform.java    |  89 ++-
 .../frc/robot/commands/ExampleCommand.java    | 122 +--
 .../java/frc/robot/commands/FollowTag.java    |  69 +-
 .../SwerveDriveBaseControl.java               | 191 ++---
 .../SwerveDriveJoystickControl.java           |  76 +-
 .../SwerveDrivePS4Control.java                |  90 +--
 .../SwerveDriveXboxControl.java               | 111 +--
 .../commands/deprecated/AutoDriveForward.java |  79 +-
 src/main/java/frc/robot/subsystems/Arm.java   |  21 +-
 .../robot/subsystems/ExampleSubsystem.java    |  28 +-
 .../robot/subsystems/SwerveDrivetrain.java    | 628 ++++++++--------
 .../frc/robot/subsystems/SwerveModule.java    | 700 +++++++++---------
 .../java/frc/robot/utils/OptionButton.java    | 202 ++---
 21 files changed, 1874 insertions(+), 1683 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index cfe6bad..71c2aad 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -5,212 +5,224 @@
 import edu.wpi.first.wpilibj.RobotController;
 
 /**
- * Use Constants as convenient storage to hold robot-wide numerical or boolean constants.
- * All constants should be visible globally (public), not be on the instance (static), and should be not change (final).
+ * Use Constants as convenient storage to hold robot-wide numerical or boolean
+ * constants.
+ * All constants should be visible globally (public), not be on the instance
+ * (static), and should be not change (final).
  * Do not put any functionality in this class.
  */
 public final class Constants {
-    public static enum Bot { WOOD_BOT, PRACTICE_BOT, COMPETITION_BOT }
-    
-    public static final Bot currentBot;
-    public static final String serialNumber;
-
-    /**
-     * This code determines what bot is being deployed and sets constants accordingly.
-     * 
-     * Simulated bots cannot have a RoboRIO ID, so we must check if the bot is real. If it isn't, load production config. 
-     * The production bot is always default, so if we do anything crazy to our bot during the tourney like switch the RoboRIO the code works.
-     * 
-     * @author Aceius E.
-     */
-    static {
-        serialNumber = RobotBase.isReal() ? RobotController.getSerialNumber() : "-default-";
-
-        switch (serialNumber) {
-            case "03282B00": // Wood Bot Serial Number
-                currentBot = Bot.WOOD_BOT;
-                break;
-
-            case "03238024": // Practice Bot Serial Number
-                currentBot = Bot.PRACTICE_BOT;
-                break;
-            
-            case "-default-": // Competition Bot Serial Number
-            default:  // Also use competition bot as default
-                currentBot = Bot.COMPETITION_BOT;
-                break;
-        }
-    }
-
-    public static class DriverConstants {
-        public static final int DRIVER_JOYSTICK_PORT = 0;
-
-        public static final double JOYSTICK_DEAD_ZONE = 0.25;
-        public static final double XBOX_DEAD_ZONE = 0.10;
-        public static final double PS4_DEAD_ZONE = 0.12;
-
-        // Names of options for displaying
-        public static final String[] maxSpeedOptionsNames = {"Precise", "Normal", "Boost"};
-
-        // max forward/sideways velocities for drivetrain, in meters per second
-        public static final double[] maxSpeedOptionsTranslation = {0.1, 0.75, 1};
-        
-        // max angular velocity for drivetrain, in radians per second
-        public static final double[] maxSpeedOptionsRotation = {0.1, 0.75, 1};
-    }
-    
-    public static class RobotMovementConstants {
-        public static final double AT_SETPOINT_TOLERANCE_TIME_SECONDS = 1;
-        public static final double ROTATE_AT_SETPOINT_TIME_SECONDS = 1;
-
-        public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(5);
-        public static final double ANGLE_TOLERANCE_RADIANS = Units.degreesToRadians(5);
-
-        public static final double ROTATION_PID_P = 0.5;
-        public static final double ROTATION_PID_I = 0;
-        public static final double ROTATION_PID_D = 0;
-
-        public static final double TRANSLATION_PID_P = 75;
-        public static final double TRANSLATION_PID_I = 1;
-        public static final double TRANSLATION_PID_D = 0.5;
-    }
-
-    public static class OperatorConstants {
-        public static final int OPERATOR_JOYSTICK_PORT = 1;
-    }
-
-    public static class SwerveModuleConstants {
-
-        static { switch (currentBot) {
-            case WOOD_BOT:
-                // Front Left
-                VELOCITY_MOTOR_ID_FL = 41;
-                ANGULAR_MOTOR_ID_FL = 40;
-                ANGULAR_MOTOR_ENCODER_ID_FL = 1;
-                ANGULAR_MOTOR_ENCODER_OFFSET_FL = -0.611328125;
-                
-                // Front right
-                VELOCITY_MOTOR_ID_FR = 4;
-                ANGULAR_MOTOR_ID_FR = 5;
-                ANGULAR_MOTOR_ENCODER_ID_FR = 2;
-                ANGULAR_MOTOR_ENCODER_OFFSET_FR = -0.282958;
-
-                // Back left
-                VELOCITY_MOTOR_ID_BL = 3;
-                ANGULAR_MOTOR_ID_BL = 2;
-                ANGULAR_MOTOR_ENCODER_ID_BL = 4;
-                ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.9260253906;
-                
-                // Back right
-                VELOCITY_MOTOR_ID_BR = 42;
-                ANGULAR_MOTOR_ID_BR = 6;
-                ANGULAR_MOTOR_ENCODER_ID_BR = 3;
-                ANGULAR_MOTOR_ENCODER_OFFSET_BR = -0.8641367187;   
-                break;
-        
-            case PRACTICE_BOT:
-            default: // Temporary default to practice bot 
-                // Front Left
-                VELOCITY_MOTOR_ID_FL = 2;
-                ANGULAR_MOTOR_ID_FL = 3;
-                ANGULAR_MOTOR_ENCODER_ID_FL = 3;
-                ANGULAR_MOTOR_ENCODER_OFFSET_FL = -0.256015325670498;
-                
-                // Front right
-                VELOCITY_MOTOR_ID_FR = 16;
-                ANGULAR_MOTOR_ID_FR = 17;
-                ANGULAR_MOTOR_ENCODER_ID_FR = 2;
-                ANGULAR_MOTOR_ENCODER_OFFSET_FR = -0.248045977011494;
-
-                // Back left
-                VELOCITY_MOTOR_ID_BL = 8;
-                ANGULAR_MOTOR_ID_BL = 9;
-                ANGULAR_MOTOR_ENCODER_ID_BL = 4;
-                ANGULAR_MOTOR_ENCODER_OFFSET_BL =-0.894674329501916;
-                
-                // Back right
-                VELOCITY_MOTOR_ID_BR = 10;
-                ANGULAR_MOTOR_ID_BR = 11;
-                ANGULAR_MOTOR_ENCODER_ID_BR = 1;
-                ANGULAR_MOTOR_ENCODER_OFFSET_BR = -0.530498084291188;   
-                break;
-
-            // case COMPETITION_BOT:
-            // default:
-            
-            //     break;
-        }}
-        
-        // Values from https://www.swervedrivespecialties.com/products/mk4-swerve-module. We have L1 Modules.
-        public static final double DRIVE_MOTOR_GEAR_RATIO = 1 / 8.4;
-        public static final double STEERING_MOTOR_GEAR_RATIO = 12.8;
-                
-        public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(4);
-        public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER_METERS * Math.PI;
-        
-        // Other settings
-        public static final double MAX_SPEED_LIMIT = 1;
-        public static final double SWERVE_MODULE_DRIVE_COSIGN_SCALE = 1;
-
-        // Drive PID values
-        public static final double DRIVE_PID_P = 0.000006;
-        public static final double DRIVE_PID_I = 0.000001;
-        public static final double DRIVE_PID_D = 0;
-        public static final double DRIVE_PID_FF = 0.000015;
-        public static final double DRIVE_PID_MAX_I = 0.001;
-
-        // Steering PID values
-        public static final double STEERING_PID_P = 0.5;
-        public static final double STEERING_PID_I = 0;
-        public static final double STEERING_PID_D = 0;
-
-        // Front left
+	public static enum Bot {
+		WOOD_BOT, PRACTICE_BOT, COMPETITION_BOT
+	}
+
+	public static final Bot currentBot;
+	public static final String serialNumber;
+
+	/**
+	 * This code determines what bot is being deployed and sets constants
+	 * accordingly.
+	 * 
+	 * Simulated bots cannot have a RoboRIO ID, so we must check if the bot is real.
+	 * If it isn't, load production config.
+	 * The production bot is always default, so if we do anything crazy to our bot
+	 * during the tourney like switch the RoboRIO the code works.
+	 * 
+	 * @author Aceius E.
+	 */
+	static {
+		serialNumber = RobotBase.isReal() ? RobotController.getSerialNumber() : "-default-";
+
+		switch (serialNumber) {
+			case "03282B00": // Wood Bot Serial Number
+				currentBot = Bot.WOOD_BOT;
+				break;
+
+			case "03238024": // Practice Bot Serial Number
+				currentBot = Bot.PRACTICE_BOT;
+				break;
+
+			case "-default-": // Competition Bot Serial Number
+			default: // Also use competition bot as default
+				currentBot = Bot.COMPETITION_BOT;
+				break;
+		}
+	}
+
+	public static class DriverConstants {
+		public static final int DRIVER_JOYSTICK_PORT = 0;
+
+		public static final double JOYSTICK_DEAD_ZONE = 0.25;
+		public static final double XBOX_DEAD_ZONE = 0.10;
+		public static final double PS4_DEAD_ZONE = 0.12;
+
+		// Names of options for displaying
+		public static final String[] maxSpeedOptionsNames = { "Precise", "Normal", "Boost" };
+
+		// max forward/sideways velocities for drivetrain, in meters per second
+		public static final double[] maxSpeedOptionsTranslation = { 0.1, 0.75, 1 };
+
+		// max angular velocity for drivetrain, in radians per second
+		public static final double[] maxSpeedOptionsRotation = { 0.1, 0.75, 1 };
+	}
+
+	public static class RobotMovementConstants {
+		public static final double AT_SETPOINT_TOLERANCE_TIME_SECONDS = 1;
+		public static final double ROTATE_AT_SETPOINT_TIME_SECONDS = 1;
+
+		public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(5);
+		public static final double ANGLE_TOLERANCE_RADIANS = Units.degreesToRadians(5);
+
+		public static final double ROTATION_PID_P = 0.5;
+		public static final double ROTATION_PID_I = 0;
+		public static final double ROTATION_PID_D = 0;
+
+		public static final double TRANSLATION_PID_P = 75;
+		public static final double TRANSLATION_PID_I = 1;
+		public static final double TRANSLATION_PID_D = 0.5;
+	}
+
+	public static class OperatorConstants {
+		public static final int OPERATOR_JOYSTICK_PORT = 1;
+	}
+
+	public static class SwerveModuleConstants {
+
+		static {
+			switch (currentBot) {
+				case WOOD_BOT:
+					// Front Left
+					VELOCITY_MOTOR_ID_FL = 41;
+					ANGULAR_MOTOR_ID_FL = 40;
+					ANGULAR_MOTOR_ENCODER_ID_FL = 1;
+					ANGULAR_MOTOR_ENCODER_OFFSET_FL = -0.611328125;
+
+					// Front right
+					VELOCITY_MOTOR_ID_FR = 4;
+					ANGULAR_MOTOR_ID_FR = 5;
+					ANGULAR_MOTOR_ENCODER_ID_FR = 2;
+					ANGULAR_MOTOR_ENCODER_OFFSET_FR = -0.282958;
+
+					// Back left
+					VELOCITY_MOTOR_ID_BL = 3;
+					ANGULAR_MOTOR_ID_BL = 2;
+					ANGULAR_MOTOR_ENCODER_ID_BL = 4;
+					ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.9260253906;
+
+					// Back right
+					VELOCITY_MOTOR_ID_BR = 42;
+					ANGULAR_MOTOR_ID_BR = 6;
+					ANGULAR_MOTOR_ENCODER_ID_BR = 3;
+					ANGULAR_MOTOR_ENCODER_OFFSET_BR = -0.8641367187;
+					break;
+
+				case PRACTICE_BOT:
+				default: // Temporary default to practice bot
+					// Front Left
+					VELOCITY_MOTOR_ID_FL = 2;
+					ANGULAR_MOTOR_ID_FL = 3;
+					ANGULAR_MOTOR_ENCODER_ID_FL = 3;
+					ANGULAR_MOTOR_ENCODER_OFFSET_FL = -0.256015325670498;
+
+					// Front right
+					VELOCITY_MOTOR_ID_FR = 16;
+					ANGULAR_MOTOR_ID_FR = 17;
+					ANGULAR_MOTOR_ENCODER_ID_FR = 2;
+					ANGULAR_MOTOR_ENCODER_OFFSET_FR = -0.248045977011494;
+
+					// Back left
+					VELOCITY_MOTOR_ID_BL = 8;
+					ANGULAR_MOTOR_ID_BL = 9;
+					ANGULAR_MOTOR_ENCODER_ID_BL = 4;
+					ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.894674329501916;
+
+					// Back right
+					VELOCITY_MOTOR_ID_BR = 10;
+					ANGULAR_MOTOR_ID_BR = 11;
+					ANGULAR_MOTOR_ENCODER_ID_BR = 1;
+					ANGULAR_MOTOR_ENCODER_OFFSET_BR = -0.530498084291188;
+					break;
+
+				// case COMPETITION_BOT:
+				// default:
+
+				// break;
+			}
+		}
+
+		// Values from
+		// https://www.swervedrivespecialties.com/products/mk4-swerve-module. We have L1
+		// Modules.
+		public static final double DRIVE_MOTOR_GEAR_RATIO = 1 / 8.4;
+		public static final double STEERING_MOTOR_GEAR_RATIO = 12.8;
+
+		public static final double WHEEL_DIAMETER_METERS = Units.inchesToMeters(4);
+		public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER_METERS * Math.PI;
+
+		// Other settings
+		public static final double MAX_SPEED_LIMIT = 1;
+		public static final double SWERVE_MODULE_DRIVE_COSIGN_SCALE = 1;
+
+		// Drive PID values
+		public static final double DRIVE_PID_P = 0.000006;
+		public static final double DRIVE_PID_I = 0.000001;
+		public static final double DRIVE_PID_D = 0;
+		public static final double DRIVE_PID_FF = 0.000015;
+		public static final double DRIVE_PID_MAX_I = 0.001;
+
+		// Steering PID values
+		public static final double STEERING_PID_P = 0.5;
+		public static final double STEERING_PID_I = 0;
+		public static final double STEERING_PID_D = 0;
+
+		// Front left
 		public static final int VELOCITY_MOTOR_ID_FL;
 		public static final int ANGULAR_MOTOR_ID_FL;
 		public static final int ANGULAR_MOTOR_ENCODER_ID_FL;
 		public static final double ANGULAR_MOTOR_ENCODER_OFFSET_FL;
-        
-        // Front right
+
+		// Front right
 		public static final int VELOCITY_MOTOR_ID_FR;
 		public static final int ANGULAR_MOTOR_ID_FR;
 		public static final int ANGULAR_MOTOR_ENCODER_ID_FR;
 		public static final double ANGULAR_MOTOR_ENCODER_OFFSET_FR;
 
-        // Back left
+		// Back left
 		public static final int VELOCITY_MOTOR_ID_BL;
 		public static final int ANGULAR_MOTOR_ID_BL;
 		public static final int ANGULAR_MOTOR_ENCODER_ID_BL;
 		public static final double ANGULAR_MOTOR_ENCODER_OFFSET_BL;
-        
-        // Back right
+
+		// Back right
 		public static final int VELOCITY_MOTOR_ID_BR;
 		public static final int ANGULAR_MOTOR_ID_BR;
 		public static final int ANGULAR_MOTOR_ENCODER_ID_BR;
 		public static final double ANGULAR_MOTOR_ENCODER_OFFSET_BR;
-    }
-
-    public static class SwerveDrivetrainConstants {
-        static { switch (currentBot) {
-            case WOOD_BOT:
-                MODULE_LOCATION_X = 26.0 / 100;
-                MODULE_LOCATION_Y = 28.5 / 100;
-                break;
-        
-            case PRACTICE_BOT:
-            default: // Temporary default to practice bot 
-                MODULE_LOCATION_X = 54 / 100;
-                MODULE_LOCATION_Y = 54 / 100;
-                break;
-
-            // case COMPETITION_BOT:
-            // default:
-            
-            //     break;
-        }}
-
-        // distance of swerve modules from center of robot, in meters
-        public static final double MODULE_LOCATION_Y;
-        public static final double MODULE_LOCATION_X;
-    }
+	}
+
+	public static class SwerveDrivetrainConstants {
+		static {
+			switch (currentBot) {
+				case WOOD_BOT:
+					MODULE_LOCATION_X = 26.0 / 100;
+					MODULE_LOCATION_Y = 28.5 / 100;
+					break;
+
+				case PRACTICE_BOT:
+				default: // Temporary default to practice bot
+					MODULE_LOCATION_X = 54 / 100;
+					MODULE_LOCATION_Y = 54 / 100;
+					break;
+
+				// case COMPETITION_BOT:
+				// default:
+
+				// break;
+			}
+		}
+
+		// distance of swerve modules from center of robot, in meters
+		public static final double MODULE_LOCATION_Y;
+		public static final double MODULE_LOCATION_X;
+	}
 }
-
diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java
index c457b81..e5d3e96 100644
--- a/src/main/java/frc/robot/Main.java
+++ b/src/main/java/frc/robot/Main.java
@@ -3,19 +3,24 @@
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
- * you are doing (which you do not), do not modify this file except to change the parameter class to the startRobot
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what
+ * you are doing (which you do not), do not modify this file except to change
+ * the parameter class to the startRobot
  * call.
  */
 public final class Main {
-  private Main() {}
+	private Main() {
+	}
 
-  /**
-   * Main initialization function. Do not perform any initialization here.
-   *
-   * <p>If you change your main robot class, change the parameter type.</p>
-   */
-  public static void main(String... args) {
-    RobotBase.startRobot(Robot::new);
-  }
+	/**
+	 * Main initialization function. Do not perform any initialization here.
+	 *
+	 * <p>
+	 * If you change your main robot class, change the parameter type.
+	 * </p>
+	 */
+	public static void main(String... args) {
+		RobotBase.startRobot(Robot::new);
+	}
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 3f7ba3a..e19b167 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -5,94 +5,117 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 /**
- * The VM is configured to automatically run this class, and to call the functions corresponding to
- * each mode, as described in the TimedRobot documentation. If you change the name of this class or
- * the package after creating this project, you must also update the build.gradle file in the
+ * The VM is configured to automatically run this class, and to call the
+ * functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the
+ * name of this class or
+ * the package after creating this project, you must also update the
+ * build.gradle file in the
  * project.
  */
 public class Robot extends TimedRobot {
-  private Command autonomousCommand;
-
-  private RobotContainer robotContainer;
-
-  /**
-   * This function is run when the robot is first started up and should be used for any
-   * initialization code.
-   */
-  @Override
-  public void robotInit() {
-    // Instantiate the RobotContainer. This will perform all the button bindings, and put the autonomous chooser on the dashboard.
-    robotContainer = new RobotContainer();
-  }
-
-  /**
-   * This function is called every 20 ms, no matter the mode. Use this for items like diagnostics
-   * that you want ran during disabled, autonomous, teleoperated and test.
-   *
-   * <p>This runs after the mode specific periodic functions, but before LiveWindow and</p>
-   * SmartDashboard integrated updating.
-   */
-  @Override
-  public void robotPeriodic() {
-    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
-    // commands, running already-scheduled commands, removing finished or interrupted commands,
-    // and running subsystem periodic() methods.  This must be called from the robot's periodic
-    // block in order for anything in the Command-based framework to work.
-    CommandScheduler.getInstance().run();
-  }
-
-  /** This function is called once each time the robot enters Disabled mode. */
-  @Override
-  public void disabledInit() {}
-
-  @Override
-  public void disabledPeriodic() {}
-
-  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
-  @Override
-  public void autonomousInit() {
-    autonomousCommand = robotContainer.getAutonomousCommand();
-
-    // schedule the autonomous command (example)
-    if (autonomousCommand != null) {
-      autonomousCommand.schedule();
-    }
-  }
-
-  /** This function is called periodically during autonomous. */
-  @Override
-  public void autonomousPeriodic() {}
-
-  @Override
-  public void teleopInit() {
-    // This makes sure that the autonomous stops running when teleop starts running.
-    // If you want the autonomous to continue until interrupted by another command, remove this line or comment it out.
-    if (autonomousCommand != null) {
-      autonomousCommand.cancel();
-    }
-
-    robotContainer.setUpDriveController();
-  }
-
-  /** This function is called periodically during operator control. */
-  @Override
-  public void teleopPeriodic() { }
-
-  @Override
-  public void testInit() {
-    // Cancels all running commands at the start of test mode.
-    CommandScheduler.getInstance().cancelAll();
-  }
-
-  /** This function is called periodically during test mode. */
-  @Override
-  public void testPeriodic() {}
-
-  /** This function is called once when the robot is first started up. */
-  @Override
-  public void simulationInit() {}
-
-  /** This function is called periodically whilst in simulation. */
-  @Override
-  public void simulationPeriodic() {}
+	private Command autonomousCommand;
+
+	private RobotContainer robotContainer;
+
+	/**
+	 * This function is run when the robot is first started up and should be used
+	 * for any
+	 * initialization code.
+	 */
+	@Override
+	public void robotInit() {
+		// Instantiate the RobotContainer. This will perform all the button bindings,
+		// and put the autonomous chooser on the dashboard.
+		robotContainer = new RobotContainer();
+	}
+
+	/**
+	 * This function is called every 20 ms, no matter the mode. Use this for items
+	 * like diagnostics
+	 * that you want ran during disabled, autonomous, teleoperated and test.
+	 *
+	 * <p>
+	 * This runs after the mode specific periodic functions, but before LiveWindow
+	 * and
+	 * </p>
+	 * SmartDashboard integrated updating.
+	 */
+	@Override
+	public void robotPeriodic() {
+		// Runs the Scheduler. This is responsible for polling buttons, adding
+		// newly-scheduled
+		// commands, running already-scheduled commands, removing finished or
+		// interrupted commands,
+		// and running subsystem periodic() methods. This must be called from the
+		// robot's periodic
+		// block in order for anything in the Command-based framework to work.
+		CommandScheduler.getInstance().run();
+	}
+
+	/** This function is called once each time the robot enters Disabled mode. */
+	@Override
+	public void disabledInit() {
+	}
+
+	@Override
+	public void disabledPeriodic() {
+	}
+
+	/**
+	 * This autonomous runs the autonomous command selected by your
+	 * {@link RobotContainer} class.
+	 */
+	@Override
+	public void autonomousInit() {
+		autonomousCommand = robotContainer.getAutonomousCommand();
+
+		// schedule the autonomous command (example)
+		if (autonomousCommand != null) {
+			autonomousCommand.schedule();
+		}
+	}
+
+	/** This function is called periodically during autonomous. */
+	@Override
+	public void autonomousPeriodic() {
+	}
+
+	@Override
+	public void teleopInit() {
+		// This makes sure that the autonomous stops running when teleop starts running.
+		// If you want the autonomous to continue until interrupted by another command,
+		// remove this line or comment it out.
+		if (autonomousCommand != null) {
+			autonomousCommand.cancel();
+		}
+
+		robotContainer.setUpDriveController();
+	}
+
+	/** This function is called periodically during operator control. */
+	@Override
+	public void teleopPeriodic() {
+	}
+
+	@Override
+	public void testInit() {
+		// Cancels all running commands at the start of test mode.
+		CommandScheduler.getInstance().cancelAll();
+	}
+
+	/** This function is called periodically during test mode. */
+	@Override
+	public void testPeriodic() {
+	}
+
+	/** This function is called once when the robot is first started up. */
+	@Override
+	public void simulationInit() {
+	}
+
+	/** This function is called periodically whilst in simulation. */
+	@Override
+	public void simulationPeriodic() {
+	}
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index ab16728..a42c099 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -24,84 +24,94 @@
 
 /**
  * This class is where the bulk of the robot should be declared.
- * Since Command-based is a "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} periodic methods (other than the scheduler calls).
- * Instead, the structure of the robot (including subsystems, commands, and trigger mappings) should be declared here.
+ * Since Command-based is a "declarative" paradigm, very little robot logic
+ * should actually be handled in the {@link Robot} periodic methods (other than
+ * the scheduler calls).
+ * Instead, the structure of the robot (including subsystems, commands, and
+ * trigger mappings) should be declared here.
  */
 public class RobotContainer {
 
-    // The robot's subsystems and commands are defined here...
-    private final SwerveModule swerveModuleFL = new SwerveModule(
-            SwerveModuleConstants.VELOCITY_MOTOR_ID_FL,
+	// The robot's subsystems and commands are defined here...
+	private final SwerveModule swerveModuleFL = new SwerveModule(
+			SwerveModuleConstants.VELOCITY_MOTOR_ID_FL,
 			SwerveModuleConstants.ANGULAR_MOTOR_ID_FL,
 			SwerveModuleConstants.ANGULAR_MOTOR_ENCODER_ID_FL,
 			SwerveModuleConstants.ANGULAR_MOTOR_ENCODER_OFFSET_FL,
-            new Translation2d(SwerveDrivetrainConstants.MODULE_LOCATION_X, SwerveDrivetrainConstants.MODULE_LOCATION_Y));
+			new Translation2d(SwerveDrivetrainConstants.MODULE_LOCATION_X,
+					SwerveDrivetrainConstants.MODULE_LOCATION_Y));
 	private final SwerveModule swerveModuleFR = new SwerveModule(
-            SwerveModuleConstants.VELOCITY_MOTOR_ID_FR,
+			SwerveModuleConstants.VELOCITY_MOTOR_ID_FR,
 			SwerveModuleConstants.ANGULAR_MOTOR_ID_FR,
 			SwerveModuleConstants.ANGULAR_MOTOR_ENCODER_ID_FR,
 			SwerveModuleConstants.ANGULAR_MOTOR_ENCODER_OFFSET_FR,
-            new Translation2d(SwerveDrivetrainConstants.MODULE_LOCATION_X, -SwerveDrivetrainConstants.MODULE_LOCATION_Y));
+			new Translation2d(SwerveDrivetrainConstants.MODULE_LOCATION_X,
+					-SwerveDrivetrainConstants.MODULE_LOCATION_Y));
 	private final SwerveModule swerveModuleBL = new SwerveModule(
-            SwerveModuleConstants.VELOCITY_MOTOR_ID_BL,
+			SwerveModuleConstants.VELOCITY_MOTOR_ID_BL,
 			SwerveModuleConstants.ANGULAR_MOTOR_ID_BL,
 			SwerveModuleConstants.ANGULAR_MOTOR_ENCODER_ID_BL,
 			SwerveModuleConstants.ANGULAR_MOTOR_ENCODER_OFFSET_BL,
-            new Translation2d(-SwerveDrivetrainConstants.MODULE_LOCATION_X, SwerveDrivetrainConstants.MODULE_LOCATION_Y));
+			new Translation2d(-SwerveDrivetrainConstants.MODULE_LOCATION_X,
+					SwerveDrivetrainConstants.MODULE_LOCATION_Y));
 	private final SwerveModule swerveModuleBR = new SwerveModule(
-            SwerveModuleConstants.VELOCITY_MOTOR_ID_BR,
+			SwerveModuleConstants.VELOCITY_MOTOR_ID_BR,
 			SwerveModuleConstants.ANGULAR_MOTOR_ID_BR,
 			SwerveModuleConstants.ANGULAR_MOTOR_ENCODER_ID_BR,
 			SwerveModuleConstants.ANGULAR_MOTOR_ENCODER_OFFSET_BR,
-            new Translation2d(-SwerveDrivetrainConstants.MODULE_LOCATION_X, -SwerveDrivetrainConstants.MODULE_LOCATION_Y));
+			new Translation2d(-SwerveDrivetrainConstants.MODULE_LOCATION_X,
+					-SwerveDrivetrainConstants.MODULE_LOCATION_Y));
 
-    private final AHRS gyro = new AHRS(I2C.Port.kOnboard);
+	private final AHRS gyro = new AHRS(I2C.Port.kOnboard);
 
-    private final SwerveDrivetrain drivetrain = new SwerveDrivetrain(gyro, swerveModuleFL, swerveModuleFR, swerveModuleBL, swerveModuleBR);
+	private final SwerveDrivetrain drivetrain = new SwerveDrivetrain(gyro, swerveModuleFL, swerveModuleFR,
+			swerveModuleBL, swerveModuleBR);
 
-    private final SendableChooser<Command> autoChooser = new SendableChooser<Command>();
+	private final SendableChooser<Command> autoChooser = new SendableChooser<Command>();
 
-    /** The container for the robot. Contains subsystems, OI devices, and commands. */
-    public RobotContainer() {
-        autoChooser.setDefaultOption("Testing Auto", Autos.testingAuto(drivetrain));
-        SmartDashboard.putData(autoChooser);
+	/**
+	 * The container for the robot. Contains subsystems, OI devices, and commands.
+	 */
+	public RobotContainer() {
+		autoChooser.setDefaultOption("Testing Auto", Autos.testingAuto(drivetrain));
+		SmartDashboard.putData(autoChooser);
 
-        configureBindings();
+		configureBindings();
 
-        setUpDriveController();
-    }
+		setUpDriveController();
+	}
 
-    public void setUpDriveController() {
-        // Create joysticks
-        final GenericHID genericHID = new GenericHID(DriverConstants.DRIVER_JOYSTICK_PORT);
-        final HIDType genericHIDType = genericHID.getType();
+	public void setUpDriveController() {
+		// Create joysticks
+		final GenericHID genericHID = new GenericHID(DriverConstants.DRIVER_JOYSTICK_PORT);
+		final HIDType genericHIDType = genericHID.getType();
 
-        SmartDashboard.putString("Drive Controller", genericHIDType.toString());
-        SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " " + Constants.serialNumber);
+		SmartDashboard.putString("Drive Controller", genericHIDType.toString());
+		SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " " + Constants.serialNumber);
 
-        drivetrain.removeDefaultCommand();
+		drivetrain.removeDefaultCommand();
 
-        SwerveDriveBaseControl control;
+		SwerveDriveBaseControl control;
 
-        if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
-            control = new SwerveDriveJoystickControl(drivetrain, new CommandJoystick(genericHID.getPort()));
-        } else {
-            control = new SwerveDriveXboxControl(drivetrain, new CommandXboxController(genericHID.getPort()));
-        }
+		if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
+			control = new SwerveDriveJoystickControl(drivetrain, new CommandJoystick(genericHID.getPort()));
+		} else {
+			control = new SwerveDriveXboxControl(drivetrain, new CommandXboxController(genericHID.getPort()));
+		}
 
-        drivetrain.setDefaultCommand(control);
-    }
+		drivetrain.setDefaultCommand(control);
+	}
 
-    /** Use this method to define your trigger->command mappings. */
-    private void configureBindings() {
-    }
+	/** Use this method to define your trigger->command mappings. */
+	private void configureBindings() {
+	}
 
-    /**
-     * Use this to pass the autonomous command to the main {@link Robot} class.
-     *
-     * @return the command to run in autonomous
-     */
-    public Command getAutonomousCommand() {
-        return autoChooser.getSelected();
-    }
+	/**
+	 * Use this to pass the autonomous command to the main {@link Robot} class.
+	 *
+	 * @return the command to run in autonomous
+	 */
+	public Command getAutonomousCommand() {
+		return autoChooser.getSelected();
+	}
 }
diff --git a/src/main/java/frc/robot/commands/AutoDriveTo.java b/src/main/java/frc/robot/commands/AutoDriveTo.java
index 27e9729..061e159 100644
--- a/src/main/java/frc/robot/commands/AutoDriveTo.java
+++ b/src/main/java/frc/robot/commands/AutoDriveTo.java
@@ -16,84 +16,87 @@
 // Code documentations https://docs.wpilib.org/en/stable/docs/software/commandbased/commands.html 
 
 public class AutoDriveTo extends Command {
-    private final SwerveDrivetrain drivetrain;
+	private final SwerveDrivetrain drivetrain;
 
-    private final PIDController xMovePID, yMovePID;
-    private double initX, initY, goalX, goalY;
+	private final PIDController xMovePID, yMovePID;
+	private double initX, initY, goalX, goalY;
 
-    private double atSetpointCounter = 0;
+	private double atSetpointCounter = 0;
 
-    private boolean xOnlyMode;
+	private boolean xOnlyMode;
 
-    public AutoDriveTo(SwerveDrivetrain subsystem, Translation2d translation) {
-        this.drivetrain = subsystem;
+	public AutoDriveTo(SwerveDrivetrain subsystem, Translation2d translation) {
+		this.drivetrain = subsystem;
 
-        goalX = translation.getX();
-        goalY = translation.getY();
+		goalX = translation.getX();
+		goalY = translation.getY();
 
-        xOnlyMode = Math.abs(goalX) > Math.abs(goalY);
+		xOnlyMode = Math.abs(goalX) > Math.abs(goalY);
 
-        xMovePID = new PIDController(
-            RobotMovementConstants.TRANSLATION_PID_P,
-            RobotMovementConstants.TRANSLATION_PID_I,
-            RobotMovementConstants.TRANSLATION_PID_D
-        );
+		xMovePID = new PIDController(
+				RobotMovementConstants.TRANSLATION_PID_P,
+				RobotMovementConstants.TRANSLATION_PID_I,
+				RobotMovementConstants.TRANSLATION_PID_D);
 
-        yMovePID = new PIDController(
-            RobotMovementConstants.TRANSLATION_PID_P,
-            RobotMovementConstants.TRANSLATION_PID_I,
-            RobotMovementConstants.TRANSLATION_PID_D
-        );
+		yMovePID = new PIDController(
+				RobotMovementConstants.TRANSLATION_PID_P,
+				RobotMovementConstants.TRANSLATION_PID_I,
+				RobotMovementConstants.TRANSLATION_PID_D);
 
-        addRequirements(this.drivetrain);
-    }
+		addRequirements(this.drivetrain);
+	}
 
-    @Override
-    public void initialize() {
+	@Override
+	public void initialize() {
 
-        drivetrain.resetPosition();
+		drivetrain.resetPosition();
 
-        final Pose2d position = drivetrain.getPosition();
+		final Pose2d position = drivetrain.getPosition();
 
-        initX = position.getX();
-        initY = position.getY();
-    }
+		initX = position.getX();
+		initY = position.getY();
+	}
 
-    @Override
-    public void execute() {
-        Pose2d position=this.drivetrain.getPosition();
+	@Override
+	public void execute() {
+		Pose2d position = this.drivetrain.getPosition();
 
-        double x = position.getX() - initX;
-        double y = position.getY() - initY;
+		double x = position.getX() - initX;
+		double y = position.getY() - initY;
 
-        SmartDashboard.putNumber("PoseY", position.getY());
-        SmartDashboard.putNumber("PoseX", position.getX());
-		SmartDashboard.putNumber("PoseDegrees", position.getRotation().getDegrees()); 
+		SmartDashboard.putNumber("PoseY", position.getY());
+		SmartDashboard.putNumber("PoseX", position.getX());
+		SmartDashboard.putNumber("PoseDegrees", position.getRotation().getDegrees());
 
-        if (Math.abs(x - goalX) < RobotMovementConstants.POSITION_TOLERANCE_METERS && Math.abs(y - goalY)<RobotMovementConstants.POSITION_TOLERANCE_METERS) atSetpointCounter+=TimedRobot.kDefaultPeriod;
-        else atSetpointCounter=0;
+		if (Math.abs(x - goalX) < RobotMovementConstants.POSITION_TOLERANCE_METERS
+				&& Math.abs(y - goalY) < RobotMovementConstants.POSITION_TOLERANCE_METERS)
+			atSetpointCounter += TimedRobot.kDefaultPeriod;
+		else
+			atSetpointCounter = 0;
 
-        double xSpeed = xMovePID.calculate(x, goalX);
-        double ySpeed = yMovePID.calculate(y, goalY);
+		double xSpeed = xMovePID.calculate(x, goalX);
+		double ySpeed = yMovePID.calculate(y, goalY);
 
-        if (xOnlyMode) ySpeed = 0;
-        else xSpeed = 0;
+		if (xOnlyMode)
+			ySpeed = 0;
+		else
+			xSpeed = 0;
 
-        final ChassisSpeeds speeds = new ChassisSpeeds(
-            xSpeed,
-            ySpeed,
-            0);
-        
-        drivetrain.setDesiredState(speeds);
-    }
+		final ChassisSpeeds speeds = new ChassisSpeeds(
+				xSpeed,
+				ySpeed,
+				0);
 
-    @Override
-    public boolean isFinished() {
-        return atSetpointCounter > RobotMovementConstants.AT_SETPOINT_TOLERANCE_TIME_SECONDS;
-    }
+		drivetrain.setDesiredState(speeds);
+	}
 
-    @Override
-    public void end(boolean interrupted) {
-        drivetrain.stop();
-    }
+	@Override
+	public boolean isFinished() {
+		return atSetpointCounter > RobotMovementConstants.AT_SETPOINT_TOLERANCE_TIME_SECONDS;
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		drivetrain.stop();
+	}
 }
diff --git a/src/main/java/frc/robot/commands/AutoRotateTo.java b/src/main/java/frc/robot/commands/AutoRotateTo.java
index 7d9b9a7..d8cf9b3 100644
--- a/src/main/java/frc/robot/commands/AutoRotateTo.java
+++ b/src/main/java/frc/robot/commands/AutoRotateTo.java
@@ -15,49 +15,51 @@
 
 /** An example command that uses an example subsystem. */
 public class AutoRotateTo extends Command {
-    private final SwerveDrivetrain drivetrain;
+	private final SwerveDrivetrain drivetrain;
 
-    private final PIDController rotatePID;
-    private final double angleGoal;
+	private final PIDController rotatePID;
+	private final double angleGoal;
 
-    private double atSetpointCounter = 0;
+	private double atSetpointCounter = 0;
 
-    public AutoRotateTo(SwerveDrivetrain subsystem, Rotation2d direction) {
-        
-        rotatePID = new PIDController(
-            RobotMovementConstants.ROTATION_PID_P,
-            RobotMovementConstants.ROTATION_PID_I,
-            RobotMovementConstants.ROTATION_PID_D
-        );
+	public AutoRotateTo(SwerveDrivetrain subsystem, Rotation2d direction) {
 
-        this.drivetrain = subsystem;
-        this.angleGoal = direction.getRadians();
+		rotatePID = new PIDController(
+				RobotMovementConstants.ROTATION_PID_P,
+				RobotMovementConstants.ROTATION_PID_I,
+				RobotMovementConstants.ROTATION_PID_D);
 
-        addRequirements(this.drivetrain);
-    }
+		this.drivetrain = subsystem;
+		this.angleGoal = direction.getRadians();
 
-    @Override
-    public void initialize() {}
+		addRequirements(this.drivetrain);
+	}
 
-    @Override
-    public void execute() {
-        final double currentAngle = drivetrain.getHeading().getRadians();
+	@Override
+	public void initialize() {
+	}
 
-        double turnsSeed = rotatePID.calculate(currentAngle,this.angleGoal);
-        
-        drivetrain.setDesiredState(new ChassisSpeeds(0, 0, turnsSeed));
+	@Override
+	public void execute() {
+		final double currentAngle = drivetrain.getHeading().getRadians();
 
-        if (Math.abs(currentAngle - this.angleGoal) < RobotMovementConstants.ANGLE_TOLERANCE_RADIANS) atSetpointCounter += TimedRobot.kDefaultPeriod;
-        else atSetpointCounter=0;
-    }
+		double turnsSeed = rotatePID.calculate(currentAngle, this.angleGoal);
 
-    @Override
-    public boolean isFinished() {
-        return atSetpointCounter > RobotMovementConstants.ROTATE_AT_SETPOINT_TIME_SECONDS;
-    }
+		drivetrain.setDesiredState(new ChassisSpeeds(0, 0, turnsSeed));
 
-    @Override
-    public void end(boolean interrupted) {
-        drivetrain.stop();
-    }
+		if (Math.abs(currentAngle - this.angleGoal) < RobotMovementConstants.ANGLE_TOLERANCE_RADIANS)
+			atSetpointCounter += TimedRobot.kDefaultPeriod;
+		else
+			atSetpointCounter = 0;
+	}
+
+	@Override
+	public boolean isFinished() {
+		return atSetpointCounter > RobotMovementConstants.ROTATE_AT_SETPOINT_TIME_SECONDS;
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		drivetrain.stop();
+	}
 }
diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java
index 6b2ff2c..0e647ff 100644
--- a/src/main/java/frc/robot/commands/Autos.java
+++ b/src/main/java/frc/robot/commands/Autos.java
@@ -10,35 +10,35 @@
 import frc.robot.subsystems.Arm;
 
 public final class Autos {
-    /** Example static factory for an autonomous command. */
-    public static Command testingAuto(SwerveDrivetrain drivetrain) {
-        return Commands.sequence(
-                new AutoDriveTo(drivetrain, new Translation2d(1, 0))
-                // new WaitCommand(1),
-                // new AutoDriveTo(drivetrain, new Translation2d(-1, 0))
-        );
-    }
-
-    public static Command startingAuto(Arm arm, SwerveDrivetrain drivetrain, boolean invertY) {
-
-        // assumes start position in corner
-        double invert = 1;
-        if (invertY) {
-            invert = -1;
-        }
-
-        return Commands.sequence(
-                // 2.9, 0.2 and 1.2 are not arbitrary, they move the robot so that the note is
-                // right in front; 0.05 can be changed, it's for the amount of extra spacing
-                // that we want
-                new AutoDriveTo(drivetrain,
-                        new Translation2d(2.9 - 0.2 - SwerveDrivetrainConstants.MODULE_LOCATION_X - 0.05,
-                                invert * (1.2 - SwerveDrivetrainConstants.MODULE_LOCATION_Y))),
-
-                new AutoRotateTo(drivetrain, new Rotation2d(Math.PI / -2 * invert)));
-    }
-
-    private Autos() {
-        throw new UnsupportedOperationException("This is a utility class!");
-    }
+	/** Example static factory for an autonomous command. */
+	public static Command testingAuto(SwerveDrivetrain drivetrain) {
+		return Commands.sequence(
+				new AutoDriveTo(drivetrain, new Translation2d(1, 0))
+		// new WaitCommand(1),
+		// new AutoDriveTo(drivetrain, new Translation2d(-1, 0))
+		);
+	}
+
+	public static Command startingAuto(Arm arm, SwerveDrivetrain drivetrain, boolean invertY) {
+
+		// assumes start position in corner
+		double invert = 1;
+		if (invertY) {
+			invert = -1;
+		}
+
+		return Commands.sequence(
+				// 2.9, 0.2 and 1.2 are not arbitrary, they move the robot so that the note is
+				// right in front; 0.05 can be changed, it's for the amount of extra spacing
+				// that we want
+				new AutoDriveTo(drivetrain,
+						new Translation2d(2.9 - 0.2 - SwerveDrivetrainConstants.MODULE_LOCATION_X - 0.05,
+								invert * (1.2 - SwerveDrivetrainConstants.MODULE_LOCATION_Y))),
+
+				new AutoRotateTo(drivetrain, new Rotation2d(Math.PI / -2 * invert)));
+	}
+
+	private Autos() {
+		throw new UnsupportedOperationException("This is a utility class!");
+	}
 }
diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java
index 944d6c8..823136c 100644
--- a/src/main/java/frc/robot/commands/DriveToPose.java
+++ b/src/main/java/frc/robot/commands/DriveToPose.java
@@ -11,93 +11,97 @@
 
 /** Command to automatically drive to a certain Pose on the field */
 public class DriveToPose extends Command {
-    private final SwerveDrivetrain drivetrain;
-    private final PIDController xController, yController, rotationController;
-
-    /**
-     * Create a new DriveToPose command. Tries to drive to a certain Pose based on odometry 
-     * 
-     * <p>This drives relative to the robot starting position, so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
-     * where forward is whatever direction the robot started in</p>
-     * 
-     * <p>The last place the drivetrain position was reset counts as the starting position</p>
-     * 
-     * @param drivetrain The drivetrain of the robot
-     * @param targetPose Target pose
-     */
-    public DriveToPose(SwerveDrivetrain drivetrain, Pose2d targetPose) {
-
-        // Save drivetrain
-        this.drivetrain = drivetrain;
-
-        // Setup all PID controllers
-        xController = new PIDController(
-            RobotMovementConstants.TRANSLATION_PID_P,
-            RobotMovementConstants.TRANSLATION_PID_I,
-            RobotMovementConstants.TRANSLATION_PID_D
-        );
-        xController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
-
-        yController = new PIDController(
-            RobotMovementConstants.TRANSLATION_PID_P,
-            RobotMovementConstants.TRANSLATION_PID_I,
-            RobotMovementConstants.TRANSLATION_PID_D
-        );
-        yController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
-
-        rotationController = new PIDController(
-            RobotMovementConstants.ROTATION_PID_P,
-            RobotMovementConstants.ROTATION_PID_I,
-            RobotMovementConstants.ROTATION_PID_D
-        );
-        rotationController.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
-
-        // Set setpoints 
-        xController.setSetpoint(targetPose.getX());
-        yController.setSetpoint(targetPose.getY());
-        rotationController.setSetpoint(targetPose.getRotation().getRadians());
-
-        // Add drivetrain as a requirement so no other commands try to use it
-        addRequirements(this.drivetrain);
-    }
-
-    @Override
-    public void initialize() {
-        // Put all swerve modules in default position
-        drivetrain.toDefaultStates();
-    }
-
-    @Override
-    public void execute() {
-
-        // Get our current pose
-        final Pose2d measuredPosition = drivetrain.getPosition();
-
-        // Put current pose position on SmartDashboard
-        SmartDashboard.putNumber("PoseY", measuredPosition.getY());
-        SmartDashboard.putNumber("PoseX", measuredPosition.getX());
-		SmartDashboard.putNumber("PoseDegrees", measuredPosition.getRotation().getDegrees()); 
-
-        // Calculate our robot speeds with the PID controllers
-        final ChassisSpeeds speeds = new ChassisSpeeds(
-            xController.calculate(measuredPosition.getX()),
-            yController.calculate(measuredPosition.getY()),
-            rotationController.calculate(measuredPosition.getRotation().getRadians())
-        );
-
-        // Set those speeds
-        drivetrain.setDesiredState(speeds);
-    }
-
-    @Override
-    public boolean isFinished() {
-        // Finish once all controllers are within tolerance
-        return xController.atSetpoint() && yController.atSetpoint() && rotationController.atSetpoint();
-    }
-
-    @Override
-    public void end(boolean interrupted) {
-        // Stop all swerve modules at end
-        drivetrain.stop();
-    }
+	private final SwerveDrivetrain drivetrain;
+	private final PIDController xController, yController, rotationController;
+
+	/**
+	 * Create a new DriveToPose command. Tries to drive to a certain Pose based on
+	 * odometry
+	 * 
+	 * <p>
+	 * This drives relative to the robot starting position, so a pose of +2x and +1y
+	 * will drive to the position 2 meters forward and 1 meter left of whether the
+	 * robot started,
+	 * where forward is whatever direction the robot started in
+	 * </p>
+	 * 
+	 * <p>
+	 * The last place the drivetrain position was reset counts as the starting
+	 * position
+	 * </p>
+	 * 
+	 * @param drivetrain The drivetrain of the robot
+	 * @param targetPose Target pose
+	 */
+	public DriveToPose(SwerveDrivetrain drivetrain, Pose2d targetPose) {
+
+		// Save drivetrain
+		this.drivetrain = drivetrain;
+
+		// Setup all PID controllers
+		xController = new PIDController(
+				RobotMovementConstants.TRANSLATION_PID_P,
+				RobotMovementConstants.TRANSLATION_PID_I,
+				RobotMovementConstants.TRANSLATION_PID_D);
+		xController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
+
+		yController = new PIDController(
+				RobotMovementConstants.TRANSLATION_PID_P,
+				RobotMovementConstants.TRANSLATION_PID_I,
+				RobotMovementConstants.TRANSLATION_PID_D);
+		yController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
+
+		rotationController = new PIDController(
+				RobotMovementConstants.ROTATION_PID_P,
+				RobotMovementConstants.ROTATION_PID_I,
+				RobotMovementConstants.ROTATION_PID_D);
+		rotationController.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
+
+		// Set setpoints
+		xController.setSetpoint(targetPose.getX());
+		yController.setSetpoint(targetPose.getY());
+		rotationController.setSetpoint(targetPose.getRotation().getRadians());
+
+		// Add drivetrain as a requirement so no other commands try to use it
+		addRequirements(this.drivetrain);
+	}
+
+	@Override
+	public void initialize() {
+		// Put all swerve modules in default position
+		drivetrain.toDefaultStates();
+	}
+
+	@Override
+	public void execute() {
+
+		// Get our current pose
+		final Pose2d measuredPosition = drivetrain.getPosition();
+
+		// Put current pose position on SmartDashboard
+		SmartDashboard.putNumber("PoseY", measuredPosition.getY());
+		SmartDashboard.putNumber("PoseX", measuredPosition.getX());
+		SmartDashboard.putNumber("PoseDegrees", measuredPosition.getRotation().getDegrees());
+
+		// Calculate our robot speeds with the PID controllers
+		final ChassisSpeeds speeds = new ChassisSpeeds(
+				xController.calculate(measuredPosition.getX()),
+				yController.calculate(measuredPosition.getY()),
+				rotationController.calculate(measuredPosition.getRotation().getRadians()));
+
+		// Set those speeds
+		drivetrain.setDesiredState(speeds);
+	}
+
+	@Override
+	public boolean isFinished() {
+		// Finish once all controllers are within tolerance
+		return xController.atSetpoint() && yController.atSetpoint() && rotationController.atSetpoint();
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		// Stop all swerve modules at end
+		drivetrain.stop();
+	}
 }
diff --git a/src/main/java/frc/robot/commands/DriveTransform.java b/src/main/java/frc/robot/commands/DriveTransform.java
index 050024c..3f8b5b6 100644
--- a/src/main/java/frc/robot/commands/DriveTransform.java
+++ b/src/main/java/frc/robot/commands/DriveTransform.java
@@ -1,6 +1,5 @@
 package frc.robot.commands;
 
-
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -10,47 +9,55 @@
 
 /** Command to automatically drive a certain transform */
 public class DriveTransform extends Command {
-    private final SwerveDrivetrain drivetrain;
-    private final Transform2d transform;
+	private final SwerveDrivetrain drivetrain;
+	private final Transform2d transform;
+
+	/**
+	 * Create a new DriveToPose command. Tries to drive a certain transform using
+	 * the DriveToPose command.
+	 * 
+	 * <p>
+	 * This drives relative to the robot, so a transform of +2x and +1y will drive 2
+	 * meters forward and 1 meter left,
+	 * where forward is whatever direction the robot is currently facing
+	 * </p>
+	 * 
+	 * @param drivetrain The drivetrain of the robot
+	 * @param transform  Target transform to drive to, will be added to current
+	 *                   position to get target pose
+	 */
+	public DriveTransform(SwerveDrivetrain drivetrain, Transform2d transform) {
+		this.drivetrain = drivetrain;
+		this.transform = transform;
+	}
 
-    /**
-     * Create a new DriveToPose command. Tries to drive a certain transform using the DriveToPose command.
-     * 
-     * <p>This drives relative to the robot, so a transform of +2x and +1y will drive 2 meters forward and 1 meter left,
-     * where forward is whatever direction the robot is currently facing</p>
-     * 
-     * @param drivetrain The drivetrain of the robot
-     * @param transform Target transform to drive to, will be added to current position to get target pose
-     */
-    public DriveTransform(SwerveDrivetrain drivetrain, Transform2d transform) {
-        this.drivetrain = drivetrain;
-        this.transform = transform;
-    }
-    
-    /**
-     * Create a new DriveToPose command. Tries to drive a certain transform (a translation and a rotation) using the DriveToPose command
-     * 
-     * <p>This drives relative to the robot, so a transform of +2x and +1y will drive 2 meters forward and 1 meter left,
-     * where forward is whatever direction the robot is currently facing</p>
-     * 
-     * @param drivetrain The drivetrain of the robot
-     * @param translation Target transform to drive
-     * @param translation Target rotation to drive
-     */
-    public DriveTransform(SwerveDrivetrain drivetrain, Translation2d translation, Rotation2d rotation) {
-        this(drivetrain, new Transform2d(translation, rotation));
-    }    
+	/**
+	 * Create a new DriveToPose command. Tries to drive a certain transform (a
+	 * translation and a rotation) using the DriveToPose command
+	 * 
+	 * <p>
+	 * This drives relative to the robot, so a transform of +2x and +1y will drive 2
+	 * meters forward and 1 meter left,
+	 * where forward is whatever direction the robot is currently facing
+	 * </p>
+	 * 
+	 * @param drivetrain  The drivetrain of the robot
+	 * @param translation Target transform to drive
+	 * @param translation Target rotation to drive
+	 */
+	public DriveTransform(SwerveDrivetrain drivetrain, Translation2d translation, Rotation2d rotation) {
+		this(drivetrain, new Transform2d(translation, rotation));
+	}
 
-    @Override
-    public void initialize() {
-        CommandScheduler.getInstance().schedule(
-            new DriveToPose(drivetrain, drivetrain.getPosition().plus(transform))
-        );
-    }
+	@Override
+	public void initialize() {
+		CommandScheduler.getInstance().schedule(
+				new DriveToPose(drivetrain, drivetrain.getPosition().plus(transform)));
+	}
 
-    @Override
-    public boolean isFinished() {
-        // Instantly finish since the entire command just calls another command 
-        return true;
-    }
+	@Override
+	public boolean isFinished() {
+		// Instantly finish since the entire command just calls another command
+		return true;
+	}
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/ExampleCommand.java
index ae6fe14..6a1b8f2 100644
--- a/src/main/java/frc/robot/commands/ExampleCommand.java
+++ b/src/main/java/frc/robot/commands/ExampleCommand.java
@@ -9,61 +9,81 @@
 
 /** An example command that uses an example subsystem. */
 public class ExampleCommand extends Command {
-    private final ExampleSubsystem subsystem;
+	private final ExampleSubsystem subsystem;
 
-    /**
-     * The constructor creates a new command and is automatically called one time when the command is created (with 'new' keyword).
-     * It should set up the initial state and properties of the object to ensure it's ready for use.
-     * This can take in any arguments you need. It normally uses 1 subsystem (but an take multiple when necessary),
-     * as wells as arguments for what to do, such as a joystick in the drive command or a desired position in an auto command.
-     * Example uses include saving parameters passed to the command, creating and configuring objects for the class like PID controllers, and adding subsystem requirements
-     */
-    public ExampleCommand(ExampleSubsystem subsystem) {
-        // use "this" to access member variable subsystem rather than local subsystem
-        this.subsystem = subsystem;
+	/**
+	 * The constructor creates a new command and is automatically called one time
+	 * when the command is created (with 'new' keyword).
+	 * It should set up the initial state and properties of the object to ensure
+	 * it's ready for use.
+	 * This can take in any arguments you need. It normally uses 1 subsystem (but an
+	 * take multiple when necessary),
+	 * as wells as arguments for what to do, such as a joystick in the drive command
+	 * or a desired position in an auto command.
+	 * Example uses include saving parameters passed to the command, creating and
+	 * configuring objects for the class like PID controllers, and adding subsystem
+	 * requirements
+	 */
+	public ExampleCommand(ExampleSubsystem subsystem) {
+		// use "this" to access member variable subsystem rather than local subsystem
+		this.subsystem = subsystem;
 
-        // Use addRequirements() here to declare subsystem dependencies.
-        // This makes sure no other commands try to do stuff with your subsystem while
-        // you are using it.
-        addRequirements(this.subsystem);
-    }
+		// Use addRequirements() here to declare subsystem dependencies.
+		// This makes sure no other commands try to do stuff with your subsystem while
+		// you are using it.
+		addRequirements(this.subsystem);
+	}
 
-    /**
-     * initialize() is used to prepare a command for execution and is called once when the command is scheduled.
-     * It should reset the command's state since command objects can be used multiple times.
-     * Example uses include setting motor to constant speed, setting a solenoid to a certain state, and resetting variables
-     */
-    @Override
-    public void initialize() {
-    }
+	/**
+	 * initialize() is used to prepare a command for execution and is called once
+	 * when the command is scheduled.
+	 * It should reset the command's state since command objects can be used
+	 * multiple times.
+	 * Example uses include setting motor to constant speed, setting a solenoid to a
+	 * certain state, and resetting variables
+	 */
+	@Override
+	public void initialize() {
+	}
 
-    /**
-     * execute() is called repeatedly while a command is scheduled, about every 20ms.
-     * It should handle continuous tasks specific to the command, like updating motor outputs based on joystick inputs or utilizing control loop results.
-     * Example uses include adjusting motor speeds for real-time control, processing sensor data within a scheduled command, and using the output of a control loop.
-     */
-    @Override
-    public void execute() {
-    }
+	/**
+	 * execute() is called repeatedly while a command is scheduled, about every
+	 * 20ms.
+	 * It should handle continuous tasks specific to the command, like updating
+	 * motor outputs based on joystick inputs or utilizing control loop results.
+	 * Example uses include adjusting motor speeds for real-time control, processing
+	 * sensor data within a scheduled command, and using the output of a control
+	 * loop.
+	 */
+	@Override
+	public void execute() {
+	}
 
-    /**
-     * isFinished() finished is called repeatedly while a command is scheduled, right after execute.
-     * It should return true when you want the command to finish. end(false) is called directly after isFinished() returns true.
-     * Example uses include checking if control loop is at set point, and always returning false to end after just 1 call to execute.
-     */
-    @Override
-    public boolean isFinished() {
-        return true;
-    }
+	/**
+	 * isFinished() finished is called repeatedly while a command is scheduled,
+	 * right after execute.
+	 * It should return true when you want the command to finish. end(false) is
+	 * called directly after isFinished() returns true.
+	 * Example uses include checking if control loop is at set point, and always
+	 * returning false to end after just 1 call to execute.
+	 */
+	@Override
+	public boolean isFinished() {
+		return true;
+	}
 
-    /**
-     * end(boolean interrupted) is called once when a command ends, regardless of whether it finishes normally or is interrupted.
-     * It should wrap up the command since other commands might use the same subsystems.
-     * Once end runs the command will no longer be in the command scheduler loop.
-     * It takes in a boolean interrupted which is set to true when the command is ended without isFinished() returning true.
-     * Example uses include setting motor speeds back to zero, and setting a solenoid back to a "default" state.
-     */
-    @Override
-    public void end(boolean interrupted) {
-    }
+	/**
+	 * end(boolean interrupted) is called once when a command ends, regardless of
+	 * whether it finishes normally or is interrupted.
+	 * It should wrap up the command since other commands might use the same
+	 * subsystems.
+	 * Once end runs the command will no longer be in the command scheduler loop.
+	 * It takes in a boolean interrupted which is set to true when the command is
+	 * ended without isFinished() returning true.
+	 * Example uses include setting motor speeds back to zero, and setting a
+	 * solenoid back to a "default" state.
+	 */
+	@Override
+	public void end(boolean interrupted) {
+	}
 }
diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java
index 96d4485..a7a3f38 100644
--- a/src/main/java/frc/robot/commands/FollowTag.java
+++ b/src/main/java/frc/robot/commands/FollowTag.java
@@ -8,40 +8,37 @@
 import frc.robot.Constants.RobotMovementConstants;
 
 public class FollowTag extends Command {
-    private final SwerveDrivetrain drivetrain;
-    private final int tagID;
-    private final Translation2d targetDistance;
-    private final PIDController xController, yController, rotationController;
-
-    public FollowTag(SwerveDrivetrain drivetrain, int tagID, Translation2d targetDistanceToTag) {
-        this.drivetrain = drivetrain;
-        this.tagID = tagID;
-        this.targetDistance = targetDistanceToTag;
-
-        xController = new PIDController(
-            RobotMovementConstants.TRANSLATION_PID_P,
-            RobotMovementConstants.TRANSLATION_PID_I,
-            RobotMovementConstants.TRANSLATION_PID_D
-        );
-        xController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
-
-        yController = new PIDController(
-            RobotMovementConstants.TRANSLATION_PID_P,
-            RobotMovementConstants.TRANSLATION_PID_I,
-            RobotMovementConstants.TRANSLATION_PID_D
-        );
-        yController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
-
-        rotationController = new PIDController(
-            RobotMovementConstants.ROTATION_PID_P,
-            RobotMovementConstants.ROTATION_PID_I,
-            RobotMovementConstants.ROTATION_PID_D
-        );
-        rotationController.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
-    }
-
-    @Override
-    public void execute() {
-        
-    }
+	private final SwerveDrivetrain drivetrain;
+	private final int tagID;
+	private final Translation2d targetDistance;
+	private final PIDController xController, yController, rotationController;
+
+	public FollowTag(SwerveDrivetrain drivetrain, int tagID, Translation2d targetDistanceToTag) {
+		this.drivetrain = drivetrain;
+		this.tagID = tagID;
+		this.targetDistance = targetDistanceToTag;
+
+		xController = new PIDController(
+				RobotMovementConstants.TRANSLATION_PID_P,
+				RobotMovementConstants.TRANSLATION_PID_I,
+				RobotMovementConstants.TRANSLATION_PID_D);
+		xController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
+
+		yController = new PIDController(
+				RobotMovementConstants.TRANSLATION_PID_P,
+				RobotMovementConstants.TRANSLATION_PID_I,
+				RobotMovementConstants.TRANSLATION_PID_D);
+		yController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
+
+		rotationController = new PIDController(
+				RobotMovementConstants.ROTATION_PID_P,
+				RobotMovementConstants.ROTATION_PID_I,
+				RobotMovementConstants.ROTATION_PID_D);
+		rotationController.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
+	}
+
+	@Override
+	public void execute() {
+
+	}
 }
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java
index 74dd343..7911871 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java
@@ -8,98 +8,105 @@
 import frc.robot.subsystems.SwerveDrivetrain;
 
 /**
- * This can be the default command for the drivetrain, allowing for remote operation with a controller
+ * This can be the default command for the drivetrain, allowing for remote
+ * operation with a controller
  */
 public abstract class SwerveDriveBaseControl extends Command {
-   protected final SwerveDrivetrain drivetrain;
-   protected final CommandGenericHID controller;
-
-   /**
-    * Creates a new SwerveDriveBaseControl Command.
-    *
-    * @param drivetrain       The drivetrain of the robot
-    * @param driverController The device used to control drivetrain
-    */
-   public SwerveDriveBaseControl(SwerveDrivetrain drivetrain, CommandGenericHID driverController) {
-
-      // save parameters
-      this.controller = driverController;
-      this.drivetrain = drivetrain;
-
-      // Tell the command schedular we are using the drivetrain
-      addRequirements(drivetrain);
-   }
-
-   /**
-    * The initial subroutine of a command. Called once when the command is initially scheduled.
-    * Puts all swerve modules to the default state, staying still and facing forwards.
-    */
-   @Override
-   public void initialize() {
-      drivetrain.toDefaultStates();
-
-      SmartDashboard.putBoolean("ControlActive", true);
-   }
-
-   /**
-    * The main body of a command. Called repeatedly while the command is scheduled (Every 20 ms).
-    */
-   @Override
-   public void execute() {
-
-      // Position display
-      final Pose2d robotPosition = drivetrain.getPosition();
-      
-      SmartDashboard.putNumber("PoseX", robotPosition.getX());
-      SmartDashboard.putNumber("PoseY", robotPosition.getX());
-      SmartDashboard.putNumber("PoseDegrees", robotPosition.getRotation().getDegrees());
-
-      
-      // Speed degrees
-      final ChassisSpeeds currentSpeeds = drivetrain.getState();
-      final double speedMetersPerSecond = Math.sqrt(Math.pow(currentSpeeds.vxMetersPerSecond, 2) + Math.pow(currentSpeeds.vyMetersPerSecond, 2));
-      
-      final double metersPerSecondToMilesPerHourConversion = 2.237;
-      SmartDashboard.putNumber("Robot Speed", speedMetersPerSecond * metersPerSecondToMilesPerHourConversion);
-      SmartDashboard.putNumber("Heading Degrees", drivetrain.getHeading().getDegrees());
-   }
-
-   /**
-    * Whether the command has finished. Once a command finishes, the scheduler will call its end() method and un-schedule it.
-    * Always return false since we never want to end in this case.
-    */
-   @Override
-   public boolean isFinished() {
-      return false;
-   }
-
-   /**
-    * The action to take when the command ends. Called when either the command finishes normally, or when it interrupted/canceled.
-    * Should only happen in this case if we get interrupted.
-    */
-   @Override
-   public void end(boolean interrupted) {
-      drivetrain.stop();
-
-      SmartDashboard.putBoolean("ControlActive", false);
-   }
-
-   // --- Util ---
-
-   /**
-    * Utility method. Apply a deadzone to the joystick output to account for stick drift and small bumps.
-    * 
-    * @param joystickValue Value in [-1, 1] from joystick axis
-    * @return {@code 0} if {@code |joystickValue| <= deadzone}, else the
-    *         {@code joystickValue} scaled to the new control area
-    */
-   public static double applyJoystickDeadzone(double joystickValue, double deadzone) {
-      if (Math.abs(joystickValue) <= deadzone) {
-         // If the joystick |value| is in the deadzone than zero it out
-         return 0;
-      }
-
-      // scale value from the range [0, 1] to (deadzone, 1]
-      return joystickValue * (1 + deadzone) - Math.signum(joystickValue) * deadzone;
-   }
+	protected final SwerveDrivetrain drivetrain;
+	protected final CommandGenericHID controller;
+
+	/**
+	 * Creates a new SwerveDriveBaseControl Command.
+	 *
+	 * @param drivetrain       The drivetrain of the robot
+	 * @param driverController The device used to control drivetrain
+	 */
+	public SwerveDriveBaseControl(SwerveDrivetrain drivetrain, CommandGenericHID driverController) {
+
+		// save parameters
+		this.controller = driverController;
+		this.drivetrain = drivetrain;
+
+		// Tell the command schedular we are using the drivetrain
+		addRequirements(drivetrain);
+	}
+
+	/**
+	 * The initial subroutine of a command. Called once when the command is
+	 * initially scheduled.
+	 * Puts all swerve modules to the default state, staying still and facing
+	 * forwards.
+	 */
+	@Override
+	public void initialize() {
+		drivetrain.toDefaultStates();
+
+		SmartDashboard.putBoolean("ControlActive", true);
+	}
+
+	/**
+	 * The main body of a command. Called repeatedly while the command is scheduled
+	 * (Every 20 ms).
+	 */
+	@Override
+	public void execute() {
+
+		// Position display
+		final Pose2d robotPosition = drivetrain.getPosition();
+
+		SmartDashboard.putNumber("PoseX", robotPosition.getX());
+		SmartDashboard.putNumber("PoseY", robotPosition.getX());
+		SmartDashboard.putNumber("PoseDegrees", robotPosition.getRotation().getDegrees());
+
+		// Speed degrees
+		final ChassisSpeeds currentSpeeds = drivetrain.getState();
+		final double speedMetersPerSecond = Math
+				.sqrt(Math.pow(currentSpeeds.vxMetersPerSecond, 2) + Math.pow(currentSpeeds.vyMetersPerSecond, 2));
+
+		final double metersPerSecondToMilesPerHourConversion = 2.237;
+		SmartDashboard.putNumber("Robot Speed", speedMetersPerSecond * metersPerSecondToMilesPerHourConversion);
+		SmartDashboard.putNumber("Heading Degrees", drivetrain.getHeading().getDegrees());
+	}
+
+	/**
+	 * Whether the command has finished. Once a command finishes, the scheduler will
+	 * call its end() method and un-schedule it.
+	 * Always return false since we never want to end in this case.
+	 */
+	@Override
+	public boolean isFinished() {
+		return false;
+	}
+
+	/**
+	 * The action to take when the command ends. Called when either the command
+	 * finishes normally, or when it interrupted/canceled.
+	 * Should only happen in this case if we get interrupted.
+	 */
+	@Override
+	public void end(boolean interrupted) {
+		drivetrain.stop();
+
+		SmartDashboard.putBoolean("ControlActive", false);
+	}
+
+	// --- Util ---
+
+	/**
+	 * Utility method. Apply a deadzone to the joystick output to account for stick
+	 * drift and small bumps.
+	 * 
+	 * @param joystickValue Value in [-1, 1] from joystick axis
+	 * @return {@code 0} if {@code |joystickValue| <= deadzone}, else the
+	 *         {@code joystickValue} scaled to the new control area
+	 */
+	public static double applyJoystickDeadzone(double joystickValue, double deadzone) {
+		if (Math.abs(joystickValue) <= deadzone) {
+			// If the joystick |value| is in the deadzone than zero it out
+			return 0;
+		}
+
+		// scale value from the range [0, 1] to (deadzone, 1]
+		return joystickValue * (1 + deadzone) - Math.signum(joystickValue) * deadzone;
+	}
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
index f3007fd..36fe660 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
@@ -10,59 +10,59 @@
 import frc.robot.utils.OptionButton.ActivationMode;
 
 /**
- * This is the default command for the drivetrain, allowing for remote operation with joystick
+ * This is the default command for the drivetrain, allowing for remote operation
+ * with joystick
  */
 public class SwerveDriveJoystickControl extends SwerveDriveBaseControl {
-    private final OptionButton preciseModeButton;
-    private final OptionButton boostModeButton;
-    private final OptionButton fieldRelativeButton;
+	private final OptionButton preciseModeButton;
+	private final OptionButton boostModeButton;
+	private final OptionButton fieldRelativeButton;
 
-    /**
+	/**
 	 * Creates a new SwerveDriveJoystickControl Command.
 	 *
-	 * @param drivetrain The drivetrain of the robot
+	 * @param drivetrain     The drivetrain of the robot
 	 * @param driverJoystick The joystick used to control drivetrain
 	 */
-    public SwerveDriveJoystickControl(SwerveDrivetrain drivetrain, CommandJoystick driverJoystick) {
-        super(drivetrain, driverJoystick);
+	public SwerveDriveJoystickControl(SwerveDrivetrain drivetrain, CommandJoystick driverJoystick) {
+		super(drivetrain, driverJoystick);
 
-        // Create and configure buttons
-        preciseModeButton = new OptionButton(driverJoystick, 2, ActivationMode.TOGGLE);
-        boostModeButton = new OptionButton(driverJoystick, 1, ActivationMode.HOLD);
-        fieldRelativeButton = new OptionButton(driverJoystick, 3, ActivationMode.TOGGLE);
+		// Create and configure buttons
+		preciseModeButton = new OptionButton(driverJoystick, 2, ActivationMode.TOGGLE);
+		boostModeButton = new OptionButton(driverJoystick, 1, ActivationMode.HOLD);
+		fieldRelativeButton = new OptionButton(driverJoystick, 3, ActivationMode.TOGGLE);
 
-        driverJoystick.button(5).onTrue(new InstantCommand(drivetrain::zeroGyroYaw));
-    }
+		driverJoystick.button(5).onTrue(new InstantCommand(drivetrain::zeroGyroYaw));
+	}
 
-    @Override
-    public void execute() {
-        super.execute();
+	@Override
+	public void execute() {
+		super.execute();
 
-        final CommandJoystick joystick = (CommandJoystick) controller;
+		final CommandJoystick joystick = (CommandJoystick) controller;
 
-        // Get joystick inputs
-        final double speedX = -applyJoystickDeadzone(joystick.getX(), DriverConstants.JOYSTICK_DEAD_ZONE);
+		// Get joystick inputs
+		final double speedX = -applyJoystickDeadzone(joystick.getX(), DriverConstants.JOYSTICK_DEAD_ZONE);
 		final double speedY = -applyJoystickDeadzone(joystick.getY(), DriverConstants.JOYSTICK_DEAD_ZONE);
-        final double speedR = -applyJoystickDeadzone(joystick.getTwist(), DriverConstants.JOYSTICK_DEAD_ZONE);
-        
-        // Level of speed from Precise, to Normal, to Boost
-        // Find our speed level, default is one (Normal)
-        final int speedLevel = 1
-            - preciseModeButton.getStateAsInt()
-            + boostModeButton.getStateAsInt();
+		final double speedR = -applyJoystickDeadzone(joystick.getTwist(), DriverConstants.JOYSTICK_DEAD_ZONE);
 
-        final boolean isFieldRelative = fieldRelativeButton.getState();
+		// Level of speed from Precise, to Normal, to Boost
+		// Find our speed level, default is one (Normal)
+		final int speedLevel = 1
+				- preciseModeButton.getStateAsInt()
+				+ boostModeButton.getStateAsInt();
 
-        final ChassisSpeeds speeds = new ChassisSpeeds(
-            speedY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-            speedX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-            speedR * DriverConstants.maxSpeedOptionsRotation[speedLevel]
-        );
+		final boolean isFieldRelative = fieldRelativeButton.getState();
 
-        // Display relevant data on shuffleboard.
-        SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);        
-        SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
+		final ChassisSpeeds speeds = new ChassisSpeeds(
+				speedY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				speedX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				speedR * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
 
-        drivetrain.setDesiredState(speeds, true, isFieldRelative);
-    }
+		// Display relevant data on shuffleboard.
+		SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);
+		SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
+
+		drivetrain.setDesiredState(speeds, true, isFieldRelative);
+	}
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java
index f406327..0715122 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java
@@ -9,51 +9,51 @@
 import frc.robot.utils.OptionButton.ActivationMode;
 
 /**
- * This is the default command for the drivetrain, allowing for remote operation with PS4 controller
+ * This is the default command for the drivetrain, allowing for remote operation
+ * with PS4 controller
  */
 public class SwerveDrivePS4Control extends SwerveDriveBaseControl {
-    private final OptionButton preciseModeButton;
-    private final OptionButton boostModeButton;
-
-    /**
-     * Creates a new SwerveDrivePS4Control Command
-     * 
-     * @param drivetrain The drive train of the robot
-     * @param driverPS4Controller The PS4 controller used to control the drive train
-     */
-    public SwerveDrivePS4Control(SwerveDrivetrain drivetrain, CommandPS4Controller driverPS4Controller){
-        super(drivetrain, driverPS4Controller);
-
-        // Create and configure buttons
-        preciseModeButton = new OptionButton(driverPS4Controller::circle, ActivationMode.TOGGLE);
-        boostModeButton = new OptionButton(driverPS4Controller::triangle, ActivationMode.TOGGLE);
-
-        //Tell the command scheduler we are using the drivetrain.
-        addRequirements(drivetrain);
-    }
-
-    @Override
-    public void execute(){
-        final CommandPS4Controller ps4Controller = (CommandPS4Controller) controller;
-
-        final double leftX = ps4Controller.getLeftX();
-        final double leftY = ps4Controller.getLeftY();
-        final double rightX = ps4Controller.getRightX();
-
-        final int speedLevel = 1
-            - preciseModeButton.getStateAsInt()
-            + boostModeButton.getStateAsInt();
-
-        final ChassisSpeeds speeds = new ChassisSpeeds(
-            leftX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-            leftY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-            rightX * DriverConstants.maxSpeedOptionsRotation[speedLevel]
-        );
-
-        // Display relevant data on shuffleboard.
-        SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);        
-        SmartDashboard.putBoolean("Field Relieve", false);
-
-        drivetrain.setDesiredState(speeds,  true, false);
-    }
+	private final OptionButton preciseModeButton;
+	private final OptionButton boostModeButton;
+
+	/**
+	 * Creates a new SwerveDrivePS4Control Command
+	 * 
+	 * @param drivetrain          The drive train of the robot
+	 * @param driverPS4Controller The PS4 controller used to control the drive train
+	 */
+	public SwerveDrivePS4Control(SwerveDrivetrain drivetrain, CommandPS4Controller driverPS4Controller) {
+		super(drivetrain, driverPS4Controller);
+
+		// Create and configure buttons
+		preciseModeButton = new OptionButton(driverPS4Controller::circle, ActivationMode.TOGGLE);
+		boostModeButton = new OptionButton(driverPS4Controller::triangle, ActivationMode.TOGGLE);
+
+		// Tell the command scheduler we are using the drivetrain.
+		addRequirements(drivetrain);
+	}
+
+	@Override
+	public void execute() {
+		final CommandPS4Controller ps4Controller = (CommandPS4Controller) controller;
+
+		final double leftX = ps4Controller.getLeftX();
+		final double leftY = ps4Controller.getLeftY();
+		final double rightX = ps4Controller.getRightX();
+
+		final int speedLevel = 1
+				- preciseModeButton.getStateAsInt()
+				+ boostModeButton.getStateAsInt();
+
+		final ChassisSpeeds speeds = new ChassisSpeeds(
+				leftX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				leftY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				rightX * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
+
+		// Display relevant data on shuffleboard.
+		SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);
+		SmartDashboard.putBoolean("Field Relieve", false);
+
+		drivetrain.setDesiredState(speeds, true, false);
+	}
 }
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
index 9691080..f0bed44 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
@@ -12,60 +12,63 @@
 // https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/wpilibj2/command/button/CommandXboxController.html
 
 /**
- * This is the default command for the drivetrain, allowing for remote operation with xbox controller
+ * This is the default command for the drivetrain, allowing for remote operation
+ * with xbox controller
  */
 public class SwerveDriveXboxControl extends SwerveDriveBaseControl {
-    private final OptionButton preciseModeButton;
-    private final OptionButton boostModeButton;
-    private final OptionButton fieldRelativeButton;
-
-    // private final OptionButton fieldRelativeButton;
-    
-    /**
-     * Creates a new SwerveDriveXboxControl Command.
-     *
-     * @param drivetrain           The drivetrain of the robot
-     * @param driverXboxController The xbox controller used to control drivetrain
-     */
-    public SwerveDriveXboxControl(SwerveDrivetrain drivetrain, CommandXboxController driverXboxController) {
-        super(drivetrain, driverXboxController);
-        
-        // Create and configure buttons
-        // OptionButton exampleToggleButton = new OptionButton(controller::a, ActivationMode.TOGGLE);
-        preciseModeButton = new OptionButton(driverXboxController::b, ActivationMode.TOGGLE);
-        boostModeButton = new OptionButton(driverXboxController::leftStick, ActivationMode.HOLD);
-        fieldRelativeButton = new OptionButton(driverXboxController::povUp, ActivationMode.TOGGLE);
-
-        // fieldRelativeButton = new OptionButton(driverXboxController::, ActivationMode.TOGGLE)
-
-        // Tell the command schedular we are using the drivetrain
-        addRequirements(drivetrain);
-    }
-
-    @Override
-    public void execute() {
-        final CommandXboxController xboxController = (CommandXboxController) controller;
-
-        final double leftX = applyJoystickDeadzone(xboxController.getLeftX(), DriverConstants.XBOX_DEAD_ZONE);
-        final double leftY = applyJoystickDeadzone(xboxController.getLeftY(), DriverConstants.XBOX_DEAD_ZONE);
-
-        final double rightX = -applyJoystickDeadzone(xboxController.getRightX(), DriverConstants.XBOX_DEAD_ZONE);
-
-        final boolean isFieldRelative = fieldRelativeButton.getState();
-
-        final int speedLevel = 1
-            - preciseModeButton.getStateAsInt()
-            + boostModeButton.getStateAsInt();
-
-        final ChassisSpeeds speeds = new ChassisSpeeds(
-            leftX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-            leftY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-            rightX * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
-
-        // Display relevant data on shuffleboard.
-        SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);        
-        SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
-
-        drivetrain.setDesiredState(speeds, true, isFieldRelative);
-    }
+	private final OptionButton preciseModeButton;
+	private final OptionButton boostModeButton;
+	private final OptionButton fieldRelativeButton;
+
+	// private final OptionButton fieldRelativeButton;
+
+	/**
+	 * Creates a new SwerveDriveXboxControl Command.
+	 *
+	 * @param drivetrain           The drivetrain of the robot
+	 * @param driverXboxController The xbox controller used to control drivetrain
+	 */
+	public SwerveDriveXboxControl(SwerveDrivetrain drivetrain, CommandXboxController driverXboxController) {
+		super(drivetrain, driverXboxController);
+
+		// Create and configure buttons
+		// OptionButton exampleToggleButton = new OptionButton(controller::a,
+		// ActivationMode.TOGGLE);
+		preciseModeButton = new OptionButton(driverXboxController::b, ActivationMode.TOGGLE);
+		boostModeButton = new OptionButton(driverXboxController::leftStick, ActivationMode.HOLD);
+		fieldRelativeButton = new OptionButton(driverXboxController::povUp, ActivationMode.TOGGLE);
+
+		// fieldRelativeButton = new OptionButton(driverXboxController::,
+		// ActivationMode.TOGGLE)
+
+		// Tell the command schedular we are using the drivetrain
+		addRequirements(drivetrain);
+	}
+
+	@Override
+	public void execute() {
+		final CommandXboxController xboxController = (CommandXboxController) controller;
+
+		final double leftX = applyJoystickDeadzone(xboxController.getLeftX(), DriverConstants.XBOX_DEAD_ZONE);
+		final double leftY = applyJoystickDeadzone(xboxController.getLeftY(), DriverConstants.XBOX_DEAD_ZONE);
+
+		final double rightX = -applyJoystickDeadzone(xboxController.getRightX(), DriverConstants.XBOX_DEAD_ZONE);
+
+		final boolean isFieldRelative = fieldRelativeButton.getState();
+
+		final int speedLevel = 1
+				- preciseModeButton.getStateAsInt()
+				+ boostModeButton.getStateAsInt();
+
+		final ChassisSpeeds speeds = new ChassisSpeeds(
+				leftX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				leftY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				rightX * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
+
+		// Display relevant data on shuffleboard.
+		SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);
+		SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
+
+		drivetrain.setDesiredState(speeds, true, isFieldRelative);
+	}
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/deprecated/AutoDriveForward.java b/src/main/java/frc/robot/commands/deprecated/AutoDriveForward.java
index ec89b6f..96f7be4 100644
--- a/src/main/java/frc/robot/commands/deprecated/AutoDriveForward.java
+++ b/src/main/java/frc/robot/commands/deprecated/AutoDriveForward.java
@@ -5,43 +5,44 @@
 import frc.robot.subsystems.SwerveDrivetrain;
 
 public class AutoDriveForward extends Command {
-    private final SwerveDrivetrain drivetrain;
-    private long beganDriving;
-
-    /**
-     * Inject dependencies and reserve systems
-     */
-    public AutoDriveForward(SwerveDrivetrain drivetrain) {
-        this.drivetrain = drivetrain;
-
-        addRequirements(this.drivetrain);
-    }
-
-    /**
-     * Begin moving
-     */
-    @Override
-    public void initialize() {
-        ChassisSpeeds desiredState = new ChassisSpeeds(0.5,0,0);
-        this.beganDriving = System.currentTimeMillis();
-        drivetrain.setDesiredState(desiredState, false);
-    }
-
-    // No execute() because we don't need it
-
-    /**
-     * Did we begin driving 10 or more seconds ago? If so trigger the finished condition and stop the robot.
-     */
-    @Override
-    public boolean isFinished() {
-        return (beganDriving + 10000) >= System.currentTimeMillis();
-    }
-
-    /**
-     * Turn off motors when we don't want the robot to move anymore
-     */
-    @Override
-    public void end(boolean interrupted) {
-        drivetrain.stop();
-    }
+	private final SwerveDrivetrain drivetrain;
+	private long beganDriving;
+
+	/**
+	 * Inject dependencies and reserve systems
+	 */
+	public AutoDriveForward(SwerveDrivetrain drivetrain) {
+		this.drivetrain = drivetrain;
+
+		addRequirements(this.drivetrain);
+	}
+
+	/**
+	 * Begin moving
+	 */
+	@Override
+	public void initialize() {
+		ChassisSpeeds desiredState = new ChassisSpeeds(0.5, 0, 0);
+		this.beganDriving = System.currentTimeMillis();
+		drivetrain.setDesiredState(desiredState, false);
+	}
+
+	// No execute() because we don't need it
+
+	/**
+	 * Did we begin driving 10 or more seconds ago? If so trigger the finished
+	 * condition and stop the robot.
+	 */
+	@Override
+	public boolean isFinished() {
+		return (beganDriving + 10000) >= System.currentTimeMillis();
+	}
+
+	/**
+	 * Turn off motors when we don't want the robot to move anymore
+	 */
+	@Override
+	public void end(boolean interrupted) {
+		drivetrain.stop();
+	}
 }
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index ed0fbc5..9ecd08c 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -8,22 +8,17 @@
 
 /**
  * How to make a Subsystem:
- * 1. Copy this file, remember that class name has to match 
+ * 1. Copy this file, remember that class name has to match
  */
 
 public class Arm extends SubsystemBase {
-    /** Constructor. Creates a new ExampleSubsystem. */
-    public Arm() {
+	/** Constructor. Creates a new ExampleSubsystem. */
+	public Arm() {
 
-    }
+	}
 
-    /**
-     * This method is called periodically by the CommandScheduler, about every 20ms.
-     * It should be used for updating subsystem-specific state that you don't want to offload to a Command.
-     * Try to avoid "doing to much" in this method (for example no driver control here).
-     */
-    @Override
-    public void periodic() {
-        // This method will be called once per scheduler run
-    }
+	@Override
+	public void periodic() {
+		// This method will be called once per scheduler run
+	}
 }
diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java
index 76d149b..d59c5ae 100644
--- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java
@@ -8,22 +8,24 @@
 
 /**
  * How to make a Subsystem:
- * 1. Copy this file, remember that class name has to match 
+ * 1. Copy this file, remember that class name has to match
  */
 
 public class ExampleSubsystem extends SubsystemBase {
-    /** Constructor. Creates a new ExampleSubsystem. */
-    public ExampleSubsystem() {
+	/** Constructor. Creates a new ExampleSubsystem. */
+	public ExampleSubsystem() {
 
-    }
+	}
 
-    /**
-     * This method is called periodically by the CommandScheduler, about every 20ms.
-     * It should be used for updating subsystem-specific state that you don't want to offload to a Command.
-     * Try to avoid "doing to much" in this method (for example no driver control here).
-     */
-    @Override
-    public void periodic() {
-        // This method will be called once per scheduler run
-    }
+	/**
+	 * This method is called periodically by the CommandScheduler, about every 20ms.
+	 * It should be used for updating subsystem-specific state that you don't want
+	 * to offload to a Command.
+	 * Try to avoid "doing to much" in this method (for example no driver control
+	 * here).
+	 */
+	@Override
+	public void periodic() {
+		// This method will be called once per scheduler run
+	}
 }
diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index ad7aab6..e72fa3e 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -20,298 +20,346 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 /**
- * Subsystem for full drive train of robot. Contains 4 {@link SwerveModule} subsystems.
+ * Subsystem for full drive train of robot. Contains 4 {@link SwerveModule}
+ * subsystems.
  * 
  * @see <a href="https://youtu.be/X2UjzPi35gU">Swerve Drive Demo</a>
  */
 public class SwerveDrivetrain extends SubsystemBase {
-    /**
-     * The SwerveDriveKinematics class is a useful tool that converts between a ChassisSpeeds object
-     * and several SwerveModuleState objects, which contains velocities and angles for each swerve module of a swerve drive robot.
-     * @see https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html
-     */
-    private final SwerveDriveKinematics kinematics;
-
-    /**
-     * The SwerveDriveOdometry class can be used to track the position of a swerve drive robot on the field *
-     * @see https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-odometry.html
-     */
-    private final SwerveDriveOdometry odometry;
-
-    // Our 4 swerve Modules
-    private final SwerveModule moduleFL, moduleFR, moduleBL, moduleBR;
-    private final SwerveModule[] modules;
-
-    /**
-     * The Gyroscope on the robot. It gives data on Pitch, Yaw, and Roll of robot, as well as many other things
-     * @see https://www.kauailabs.com/public_files/navx-mxp/apidocs/java/com/kauailabs/navx/frc/AHRS.html
-     * @see https://ibb.co/dJrL259
-     */
-    private final AHRS gyro;
-
-    /**
-     * Pose of robot. The pose is the current the X, Y and Rotation position of the robot, relative to the last reset.
-     * @see https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/pose.html#pose
-     */
-    private Pose2d pose;
-
-    /**
-     * Constructor the drivetrain subsystem.
-     * 
-     * @param gyro Gyroscope on robot, should be physically located near center of robot
-     * @param swerveModuleFL Front left swerve module
-     * @param swerveModuleFR Front right swerve module
-     * @param swerveModuleBL Back left swerve module
-     * @param swerveModuleBR Back right swerve module
-     */
-    public SwerveDrivetrain(AHRS gyro, SwerveModule swerveModuleFL, SwerveModule swerveModuleFR, SwerveModule swerveModuleBL, SwerveModule swerveModuleBR) {
-
-        // save parameters
-        this.gyro = gyro;
-
-        moduleFL = swerveModuleFL;
-        moduleFR = swerveModuleFR;
-        moduleBL = swerveModuleBL;
-        moduleBR = swerveModuleBR;
-
-        modules = new SwerveModule[] {moduleFL, moduleFR, moduleBL, moduleBR};
-        
-        // create kinematics object using swerve module distance from center
-        kinematics = new SwerveDriveKinematics(
-            modulesMap(SwerveModule::getDistanceFromCenter, Translation2d[]::new)
-        );
-
-        // create starting state for odometry
-        odometry = new SwerveDriveOdometry(
-            kinematics,
-            getHeading(),
-            modulesMap(SwerveModule::getPosition, SwerveModulePosition[]::new)
-        );
-
-        for (SwerveModule module : modules) {
-            addChild(module.getName(), module);
-        }
-
-        pose = new Pose2d();
-    }
-
-    // --- Pose Related Methods ---
-
-    /**
-     * This is the periodic function of the swerve drivetrain.
-     * This method is called periodically by the CommandScheduler, about every 20ms.
-     */
-    @Override
-    public void periodic() {
-        // use odometry to update the estimated pose
-        pose = odometry.update(getHeading(), getWheelPositions());
-    }
-
-    /**
-     * Get the pose of robot, as calculated by 
-     * 
-     * @return The current positions of the robot, contains translational and rotational elements.
-     */
-    public Pose2d getPosition() {
-        return pose;
-    }
-
-    /**
-     * Resets the robot's pose on the field in software.
-     * Basically zeros out position.
-     */
-    public void resetPosition() {
-        odometry.resetPosition(
-            gyro.getRotation2d(),
-            getWheelPositions(),
-            pose
-        );
-    }
-
-
-    // --- Action Methods ---
-
-
-    /** Stop all swerve modules */
-    public void stop() {
-        modulesMap(SwerveModule::stop);
-    }
-
-    /** Put all swerve modules to default state, facing forward and staying still */
-    public void toDefaultStates() {
-        modulesMap(SwerveModule::toDefaultState);
-    }
-
-
-    // --- Chassis Speeds to Swerve Module State Methods ---
-
-
-    /**
-     * Get speeds of robot.
-     * <p>vx: The velocity of the robot in the x (forward) direction in meter per second.</p>
-     * <p>vy: The velocity of the robot in the y (sideways) direction in meter per second. (Positive values mean the robot is moving to the left).</p>
-     * <p>omega: The angular velocity of the robot in radians per second.</p>
-     * 
-     * @return Speeds of drivetrain (from swerve modules)
-     */
-    public ChassisSpeeds getState() {
-        // get all module states and convert them into chassis speeds
-        ChassisSpeeds speeds = kinematics.toChassisSpeeds(
-            modulesMap(SwerveModule::getState, SwerveModuleState[]::new)
-        );
-
-        return speeds;
-    }
-
-    /**
-     * Set robot relative speeds of robot in meters per second mode.
-     * <li>vx: The velocity of the robot in the x (forward) direction in meter per second.</li>
-     * <li>vy: The velocity of the robot in the y (sideways) direction in meter per second. (Positive values mean the robot is moving to the left).</li>
-     * <li>omega: The angular velocity of the robot in radians per second.</li>
-     * 
-     * @param speeds Desired speeds of drivetrain (using swerve modules)
-     */
-    public void setDesiredState(ChassisSpeeds speeds) {
-        setDesiredState(speeds, false);
-    }
-
-    /**
-     * Set robot relative speeds of robot.
-     * <li>vx: The velocity of the robot in the x (forward) direction</li>
-     * <li>vy: The velocity of the robot in the y (sideways) direction. (Positive values mean the robot is moving to the left).</li>
-     * <li>omega: The angular velocity of the robot.</li>
-     * 
-     * <p>
-     * If power drive mode then speeds X, Y, and Omega are in motor powers from -1 to 1.
-     * If normal drive mode then X and Y are in meters per second and Omega is in radians per second
-     * </p>
-     * 
-     * @param speeds desired speeds of drivetrain (using swerve modules)
-     * @param powerDriveMode if {@code true} speeds are in motor power (-1 to 1), if not then they are in default unit
-     */
-    public void setDesiredState(ChassisSpeeds speeds, boolean powerDriveMode) {
-        SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);
-        for (int i = 0; i < modules.length; i++) {
-            modules[i].setDesiredState(states[i], powerDriveMode);
-        }
-    }
-
-    /**
-     * Set speeds of robot.
-     * 
-     * <p>
-     * vx: The velocity of the robot in the x (forward) direction in meter per second.
-     * vy: The velocity of the robot in the y (sideways) direction in meter per second. (Positive values mean the robot is moving to the left).
-     * omega: The angular velocity of the robot in radians per second.
-     * </p>
-     * 
-     * <p>
-     * If power drive mode then speeds X, Y, and Omega are in motor powers from -1 to 1.
-     * If normal drive mode then X and Y are in meters per second and Omega is in radians per second
-     * </p>
-     * 
-     * <p>
-     * If field relative, forward will be directly away from driver, no matter the rotation of the robot.
-     * If robot relative, forward will be whatever direction the robot is facing in.
-     * </p>
-     * 
-     * @see https://ibb.co/dJrL259
-     * 
-     * @param speeds Desired speeds of drivetrain (using swerve modules)
-     * @param powerDriveMode True if in power drive mode with motor powers, false if in normal drive mode with default units
-     * @param fieldRelative True if the robot is using a field relative coordinate system, false if using a robot relive coordinate system.     */
-    public void setDesiredState(ChassisSpeeds speeds, boolean powerDriveMode, boolean fieldRelative) {
-
-        if (fieldRelative) {
-            speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getHeading());
-        }
-
-        setDesiredState(speeds, powerDriveMode);
-    }
-    
-    /**
-     * Get all the swerve module positions
-     * 
-     * @return a {@link SwerveDriveWheelPositions} object which contains an array of all swerve module positions
-     */
-    public SwerveDriveWheelPositions getWheelPositions() {
-        return new SwerveDriveWheelPositions(
-            modulesMap(SwerveModule::getPosition, SwerveModulePosition[]::new)
-        );
-    }
-
-
-    // --- Gyro methods ---
-
-
-    /**
-     * <p>Return the heading of the robot as a rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).</p>
-     * 
-     * <p>The angle is continuous, that is it will continue from 360 to 361 degrees. This allows algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from 360 to 0 on the second time around.</p>
-     * 
-     * <p>The angle is expected to increase as the gyro turns counterclockwise when looked at from the top.</p>
-     * It needs to follow the NWU axis convention.
-     * 
-     * @return the current heading of the robot as a {@link Rotation2d}.
-     * 
-     * @see https://ibb.co/dJrL259 NWU Axis Convention 
-     */
-    public Rotation2d getHeading() {
-        return gyro.getRotation2d();
-    }
-
-    /**
-     * <p>Return the heading of the robot in as a rotation in a 3D coordinate frame represented by a quaternion.</p>
-     * 
-     * @return the current heading of the robot as a {@link Rotation3d}.
-     */
-    public Rotation3d getHeading3d() {
-        return gyro.getRotation3d();
-    }
-
-    /** Reset the gyro. */
-    public void resetGyro() {
-        gyro.reset();
-    }
-
-    /**
-     * Zero the yaw of the gyro. Can only change zero yaw when sensor is done calibrating
-     * 
-     * @return successful?
-     */
-    public boolean zeroGyroYaw() {
-        if (gyro.isCalibrating()) return false;
-
-        gyro.zeroYaw();
-
-        return true;
-    }
-
-    // --- Util ---
-
-    /**
-     * Utility method. Function to easily run a function on each swerve module
-     * 
-     * @param func function to run on each swerve module, takes one argument and returns nothing, operates via side effects.
-     */
-    private void modulesMap(Consumer<SwerveModule> func) {
-        Arrays.stream(modules).forEach(func);
-    }
-
-    /**
-     * Utility method. Function to easily run a function on each swerve module and collect results to array.
-     * Insures that we don't mix up order of swerve modules, as this could lead to hard to spot bugs.
-     * 
-     * @param <T> type that is returned by function and should be collected
-     * @param func function that gets some data off each swerve module
-     * @param arrayInitializer constructor function for array to collect results in, use T[]::new
-     * @return array of results from func.
-     */
-    private <T> T[] modulesMap(Function<? super SwerveModule, ? extends T> func, IntFunction<T[]> arrayInitializer) {
-        // Private method with template argument T. Returns array of type T.
-        // Takes in function that accepts a SwerveModule or something higher on the inheritance chain (For example: Object, SubsystemBase)
-        // and returns something of type T or something lower on the inheritance chain. (For example if T is Object: Integer)
-        // Also takes a T array initializer
-        return Arrays.stream(modules).map(func).toArray(arrayInitializer);
-    }   
+	/**
+	 * The SwerveDriveKinematics class is a useful tool that converts between a
+	 * ChassisSpeeds object
+	 * and several SwerveModuleState objects, which contains velocities and angles
+	 * for each swerve module of a swerve drive robot.
+	 * 
+	 * @see https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html
+	 */
+	private final SwerveDriveKinematics kinematics;
+
+	/**
+	 * The SwerveDriveOdometry class can be used to track the position of a swerve
+	 * drive robot on the field *
+	 * 
+	 * @see https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-odometry.html
+	 */
+	private final SwerveDriveOdometry odometry;
+
+	// Our 4 swerve Modules
+	private final SwerveModule moduleFL, moduleFR, moduleBL, moduleBR;
+	private final SwerveModule[] modules;
+
+	/**
+	 * The Gyroscope on the robot. It gives data on Pitch, Yaw, and Roll of robot,
+	 * as well as many other things
+	 * 
+	 * @see https://www.kauailabs.com/public_files/navx-mxp/apidocs/java/com/kauailabs/navx/frc/AHRS.html
+	 * @see https://ibb.co/dJrL259
+	 */
+	private final AHRS gyro;
+
+	/**
+	 * Pose of robot. The pose is the current the X, Y and Rotation position of the
+	 * robot, relative to the last reset.
+	 * 
+	 * @see https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/pose.html#pose
+	 */
+	private Pose2d pose;
+
+	/**
+	 * Constructor the drivetrain subsystem.
+	 * 
+	 * @param gyro           Gyroscope on robot, should be physically located near
+	 *                       center of robot
+	 * @param swerveModuleFL Front left swerve module
+	 * @param swerveModuleFR Front right swerve module
+	 * @param swerveModuleBL Back left swerve module
+	 * @param swerveModuleBR Back right swerve module
+	 */
+	public SwerveDrivetrain(AHRS gyro, SwerveModule swerveModuleFL, SwerveModule swerveModuleFR,
+			SwerveModule swerveModuleBL, SwerveModule swerveModuleBR) {
+
+		// save parameters
+		this.gyro = gyro;
+
+		moduleFL = swerveModuleFL;
+		moduleFR = swerveModuleFR;
+		moduleBL = swerveModuleBL;
+		moduleBR = swerveModuleBR;
+
+		modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR };
+
+		// create kinematics object using swerve module distance from center
+		kinematics = new SwerveDriveKinematics(
+				modulesMap(SwerveModule::getDistanceFromCenter, Translation2d[]::new));
+
+		// create starting state for odometry
+		odometry = new SwerveDriveOdometry(
+				kinematics,
+				getHeading(),
+				modulesMap(SwerveModule::getPosition, SwerveModulePosition[]::new));
+
+		for (SwerveModule module : modules) {
+			addChild(module.getName(), module);
+		}
+
+		pose = new Pose2d();
+	}
+
+	// --- Pose Related Methods ---
+
+	/**
+	 * This is the periodic function of the swerve drivetrain.
+	 * This method is called periodically by the CommandScheduler, about every 20ms.
+	 */
+	@Override
+	public void periodic() {
+		// use odometry to update the estimated pose
+		pose = odometry.update(getHeading(), getWheelPositions());
+	}
+
+	/**
+	 * Get the pose of robot, as calculated by
+	 * 
+	 * @return The current positions of the robot, contains translational and
+	 *         rotational elements.
+	 */
+	public Pose2d getPosition() {
+		return pose;
+	}
+
+	/**
+	 * Resets the robot's pose on the field in software.
+	 * Basically zeros out position.
+	 */
+	public void resetPosition() {
+		odometry.resetPosition(
+				gyro.getRotation2d(),
+				getWheelPositions(),
+				pose);
+	}
+
+	// --- Action Methods ---
+
+	/** Stop all swerve modules */
+	public void stop() {
+		modulesMap(SwerveModule::stop);
+	}
+
+	/** Put all swerve modules to default state, facing forward and staying still */
+	public void toDefaultStates() {
+		modulesMap(SwerveModule::toDefaultState);
+	}
+
+	// --- Chassis Speeds to Swerve Module State Methods ---
+
+	/**
+	 * Get speeds of robot.
+	 * <p>
+	 * vx: The velocity of the robot in the x (forward) direction in meter per
+	 * second.
+	 * </p>
+	 * <p>
+	 * vy: The velocity of the robot in the y (sideways) direction in meter per
+	 * second. (Positive values mean the robot is moving to the left).
+	 * </p>
+	 * <p>
+	 * omega: The angular velocity of the robot in radians per second.
+	 * </p>
+	 * 
+	 * @return Speeds of drivetrain (from swerve modules)
+	 */
+	public ChassisSpeeds getState() {
+		// get all module states and convert them into chassis speeds
+		ChassisSpeeds speeds = kinematics.toChassisSpeeds(
+				modulesMap(SwerveModule::getState, SwerveModuleState[]::new));
+
+		return speeds;
+	}
+
+	/**
+	 * Set robot relative speeds of robot in meters per second mode.
+	 * <li>vx: The velocity of the robot in the x (forward) direction in meter per
+	 * second.</li>
+	 * <li>vy: The velocity of the robot in the y (sideways) direction in meter per
+	 * second. (Positive values mean the robot is moving to the left).</li>
+	 * <li>omega: The angular velocity of the robot in radians per second.</li>
+	 * 
+	 * @param speeds Desired speeds of drivetrain (using swerve modules)
+	 */
+	public void setDesiredState(ChassisSpeeds speeds) {
+		setDesiredState(speeds, false);
+	}
+
+	/**
+	 * Set robot relative speeds of robot.
+	 * <li>vx: The velocity of the robot in the x (forward) direction</li>
+	 * <li>vy: The velocity of the robot in the y (sideways) direction. (Positive
+	 * values mean the robot is moving to the left).</li>
+	 * <li>omega: The angular velocity of the robot.</li>
+	 * 
+	 * <p>
+	 * If power drive mode then speeds X, Y, and Omega are in motor powers from -1
+	 * to 1.
+	 * If normal drive mode then X and Y are in meters per second and Omega is in
+	 * radians per second
+	 * </p>
+	 * 
+	 * @param speeds         desired speeds of drivetrain (using swerve modules)
+	 * @param powerDriveMode if {@code true} speeds are in motor power (-1 to 1), if
+	 *                       not then they are in default unit
+	 */
+	public void setDesiredState(ChassisSpeeds speeds, boolean powerDriveMode) {
+		SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);
+		for (int i = 0; i < modules.length; i++) {
+			modules[i].setDesiredState(states[i], powerDriveMode);
+		}
+	}
+
+	/**
+	 * Set speeds of robot.
+	 * 
+	 * <p>
+	 * vx: The velocity of the robot in the x (forward) direction in meter per
+	 * second.
+	 * vy: The velocity of the robot in the y (sideways) direction in meter per
+	 * second. (Positive values mean the robot is moving to the left).
+	 * omega: The angular velocity of the robot in radians per second.
+	 * </p>
+	 * 
+	 * <p>
+	 * If power drive mode then speeds X, Y, and Omega are in motor powers from -1
+	 * to 1.
+	 * If normal drive mode then X and Y are in meters per second and Omega is in
+	 * radians per second
+	 * </p>
+	 * 
+	 * <p>
+	 * If field relative, forward will be directly away from driver, no matter the
+	 * rotation of the robot.
+	 * If robot relative, forward will be whatever direction the robot is facing in.
+	 * </p>
+	 * 
+	 * @see https://ibb.co/dJrL259
+	 * 
+	 * @param speeds         Desired speeds of drivetrain (using swerve modules)
+	 * @param powerDriveMode True if in power drive mode with motor powers, false if
+	 *                       in normal drive mode with default units
+	 * @param fieldRelative  True if the robot is using a field relative coordinate
+	 *                       system, false if using a robot relive coordinate
+	 *                       system.
+	 */
+	public void setDesiredState(ChassisSpeeds speeds, boolean powerDriveMode, boolean fieldRelative) {
+
+		if (fieldRelative) {
+			speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getHeading());
+		}
+
+		setDesiredState(speeds, powerDriveMode);
+	}
+
+	/**
+	 * Get all the swerve module positions
+	 * 
+	 * @return a {@link SwerveDriveWheelPositions} object which contains an array of
+	 *         all swerve module positions
+	 */
+	public SwerveDriveWheelPositions getWheelPositions() {
+		return new SwerveDriveWheelPositions(
+				modulesMap(SwerveModule::getPosition, SwerveModulePosition[]::new));
+	}
+
+	// --- Gyro methods ---
+
+	/**
+	 * <p>
+	 * Return the heading of the robot as a rotation in a 2D coordinate frame
+	 * represented by a point on the unit circle (cosine and sine).
+	 * </p>
+	 * 
+	 * <p>
+	 * The angle is continuous, that is it will continue from 360 to 361 degrees.
+	 * This allows algorithms that wouldn't want to see a discontinuity in the gyro
+	 * output as it sweeps past from 360 to 0 on the second time around.
+	 * </p>
+	 * 
+	 * <p>
+	 * The angle is expected to increase as the gyro turns counterclockwise when
+	 * looked at from the top.
+	 * </p>
+	 * It needs to follow the NWU axis convention.
+	 * 
+	 * @return the current heading of the robot as a {@link Rotation2d}.
+	 * 
+	 * @see https://ibb.co/dJrL259 NWU Axis Convention
+	 */
+	public Rotation2d getHeading() {
+		return gyro.getRotation2d();
+	}
+
+	/**
+	 * <p>
+	 * Return the heading of the robot in as a rotation in a 3D coordinate frame
+	 * represented by a quaternion.
+	 * </p>
+	 * 
+	 * @return the current heading of the robot as a {@link Rotation3d}.
+	 */
+	public Rotation3d getHeading3d() {
+		return gyro.getRotation3d();
+	}
+
+	/** Reset the gyro. */
+	public void resetGyro() {
+		gyro.reset();
+	}
+
+	/**
+	 * Zero the yaw of the gyro. Can only change zero yaw when sensor is done
+	 * calibrating
+	 * 
+	 * @return successful?
+	 */
+	public boolean zeroGyroYaw() {
+		if (gyro.isCalibrating())
+			return false;
+
+		gyro.zeroYaw();
+
+		return true;
+	}
+
+	// --- Util ---
+
+	/**
+	 * Utility method. Function to easily run a function on each swerve module
+	 * 
+	 * @param func function to run on each swerve module, takes one argument and
+	 *             returns nothing, operates via side effects.
+	 */
+	private void modulesMap(Consumer<SwerveModule> func) {
+		Arrays.stream(modules).forEach(func);
+	}
+
+	/**
+	 * Utility method. Function to easily run a function on each swerve module and
+	 * collect results to array.
+	 * Insures that we don't mix up order of swerve modules, as this could lead to
+	 * hard to spot bugs.
+	 * 
+	 * @param <T>              type that is returned by function and should be
+	 *                         collected
+	 * @param func             function that gets some data off each swerve module
+	 * @param arrayInitializer constructor function for array to collect results in,
+	 *                         use T[]::new
+	 * @return array of results from func.
+	 */
+	private <T> T[] modulesMap(Function<? super SwerveModule, ? extends T> func, IntFunction<T[]> arrayInitializer) {
+		// Private method with template argument T. Returns array of type T.
+		// Takes in function that accepts a SwerveModule or something higher on the
+		// inheritance chain (For example: Object, SubsystemBase)
+		// and returns something of type T or something lower on the inheritance chain.
+		// (For example if T is Object: Integer)
+		// Also takes a T array initializer
+		return Arrays.stream(modules).map(func).toArray(arrayInitializer);
+	}
 }
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 8337b04..2bced81 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -20,341 +20,375 @@
 import frc.robot.Constants.SwerveModuleConstants;
 
 /**
- * Subsystem for individual swerve module on robot. Each swerve module has one drive motor and one steering motor.
+ * Subsystem for individual swerve module on robot. Each swerve module has one
+ * drive motor and one steering motor.
  * 
- * @see <a href="https://www.swervedrivespecialties.com/products/mk4-swerve-module">Swerve Module Kit</a>
+ * @see <a href=
+ *      "https://www.swervedrivespecialties.com/products/mk4-swerve-module">Swerve
+ *      Module Kit</a>
  */
 public class SwerveModule extends SubsystemBase {
 
-    // the drive motor is the motor that spins the wheel making the robot move across the ground (aka wheel or velocity motor)
-    private final CANSparkMax driveMotor;
-    private final RelativeEncoder driveEncoder;
-    private final SparkPIDController drivePIDController;
-   
-    // the steering motor is the motor that changes the rotation of the wheel allowing the robot to drive in any direction (aka spin or angular motor)
-    // Also allows for spinning in place
-    private final CANSparkMax steeringMotor;
-    private final CANcoder steeringEncoder;
-    private final PIDController steeringPIDController;
-
-    /** Status Signal that gives steering encoders current position in rotations */
-    private final StatusSignal<Double> steeringPosition;
-
-    /** Default state, forward and still */
-    private final static SwerveModuleState defaultState = new SwerveModuleState();
-
-    /** Locations of the wheel relative to the physical center of the robot. */
-    private final Translation2d distanceFromCenter;
-
-    /**
-     * Constructor for an individual Swerve Module.
-     * Sets up both drive and angular motor for swerve module as well as systems to monitor and control them
-     * 
-     * @param velocityMotorDeviceID  device ID for drive motor
-     * @param steeringMotorDeviceId  device ID for steering motor
-     * @param angularEncoderDeviceID device ID for the angular motor's absolute encoder
-     * @param distanceFromCenter     distance from center of robot to center of swerve module
-     * @param steeringEncoderZero    the zero (forward) position for the angular motor's absolute encoder, in rotations
-     */
-    public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steeringAbsoluteEncoderId, double steeringEncoderZero, Translation2d distanceFromCenter) {
-        // --- Drive Motor ---
-        driveMotor = new CANSparkMax(driveMotorDeviceId, MotorType.kBrushless);
-
-        // You most restore factory defaults if you want to use velocity encoder.
-        // If you do not do this, everything will break and shake itself to death
-        driveMotor.restoreFactoryDefaults();
-
-        driveMotor.setIdleMode(IdleMode.kBrake);
-
-        // --- Drive Encoder ---
-        driveEncoder = driveMotor.getEncoder();
-
-        driveEncoder.setVelocityConversionFactor(SwerveModuleConstants.DRIVE_MOTOR_GEAR_RATIO);
-        driveEncoder.setPositionConversionFactor(SwerveModuleConstants.DRIVE_MOTOR_GEAR_RATIO);
-
-        // --- Drive PID ---
-        drivePIDController = driveMotor.getPIDController();
-        drivePIDController.setP(SwerveModuleConstants.DRIVE_PID_P);
-        drivePIDController.setI(SwerveModuleConstants.DRIVE_PID_I);
-        drivePIDController.setD(SwerveModuleConstants.DRIVE_PID_D);
-        drivePIDController.setFF(SwerveModuleConstants.DRIVE_PID_FF);
-        drivePIDController.setIAccum(SwerveModuleConstants.DRIVE_PID_MAX_I);
-        drivePIDController.setOutputRange(-1, 1);
-
-        // --- Steering Motor ---
-        steeringMotor = new CANSparkMax(steeringMotorDeviceId, MotorType.kBrushless);
-
-        steeringMotor.setIdleMode(IdleMode.kBrake);
-
-        // --- Steering Encoder ---
-        steeringEncoder = new CANcoder(steeringAbsoluteEncoderId);
-
-        // Use 0-1 for rotational
-        steeringEncoder.getConfigurator().apply(
-            new MagnetSensorConfigs()
-                .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
-                .withMagnetOffset(steeringEncoderZero)
-        );
-        
-        // --- Steering PID ---
-        steeringPIDController = new PIDController(
-            SwerveModuleConstants.STEERING_PID_P,
-            SwerveModuleConstants.STEERING_PID_I,
-            SwerveModuleConstants.STEERING_PID_D
-        );
-        steeringPIDController.enableContinuousInput(0, 1);
-        
-        // --- Save other values ---
-        this.distanceFromCenter = distanceFromCenter;
-
-        steeringPosition = steeringEncoder.getAbsolutePosition();
-
-        setName(toString());
-    }
-
-    /**
-     * This is the periodic function of the swerve module.
-     * This method is called periodically by the CommandScheduler, about every 20ms.
-     * 
-     * We use it to constantly keep the 
-     */
-    @Override
-    public void periodic() {
-        // Calculate how fast to spin the motor to get to the desired angle using our PID controller, then set the motor to spin at that speed
-        steeringMotor.set(steeringPIDController.calculate(getSteeringAngleRotations()));
-    }
-
-    // --- Direct control methods ---
-
-    /** Stop drive and steering motor of swerve module and set desired state to nothing */
-    public void stop() {
-        // Manually stop both motors in swerve module
-        driveMotor.stopMotor();
-        steeringMotor.stopMotor();
-    }
-
-    /** 
-     * Set the desired state of the Swerve Module to the default/starting state.
-     * This should have the module facing forward and not spinning.
-     */
-    public void toDefaultState() {
-        setDesiredState(defaultState, false);
-    }
-
-    // --- Getters and setters for modules desired SwerveModuleState ---
-
-    /**
-     * Get the state of the swerve module.
-     * The state is the speed of our drive motor and angle of our steering motor.
-     * 
-     * @return Current state of swerve module, contains speed (in m/s) and angle as {@link Rotation2d}
-     */
-    public SwerveModuleState getState() {
-        return new SwerveModuleState(
-                getDriveSpeedMetersPerSecond(),
-                Rotation2d.fromRotations(getSteeringAngleRotations()));
-    }
-
-    /**
-     * Set the state of the swerve module. The state is the speed and angle of the swerve module.
-     * You can use {@code Rotation2d.fromDegrees()} to create angle.
-     * 
-     * @param state New state of swerve module, contains speed in meters per second and angle as {@link Rotation2d}
-     * @param powerDriveMode whether the SwerveModuleState is in meters per second (false) or motor power (true)
-     * @param shouldOptimize Whether to optimize the way the swerve module gets to the desired state
-     */
-    public void setDesiredState(SwerveModuleState state, boolean powerDriveMode, boolean shouldOptimize) {
-        if (shouldOptimize && state != null) {
-            // Optimize the reference state to avoid spinning further than 90 degrees
-            state = optimize(state,  getState().angle);
-        }
-
-        // --- Set steering motor ---
-        steeringPIDController.setSetpoint(state.angle.getRotations());
-        
-        // --- Set drive motor ---
-
-        if (state.speedMetersPerSecond == 0) {
-            // If our desired speed is 0, just use the builtin motor stop, no matter the mode.
-            
-            // Stops motor movement. Motor can be moved again by calling set without having to re-enable the motor.
-            driveMotor.stopMotor();
-        }
-        else if (powerDriveMode) {
-            // If we are in power drive mode just directly set power to our desired speed.
-
-            // This is a bit of an abuse of the SwerveModuleState object as we treat speeds as power values from -1 to 1.
-            // We do this because we don't want to have to deal with a PID controller when we are just driving, as a human driver does not care about they exact speed mapping.
-            driveMotor.set(state.speedMetersPerSecond);
-        }
-        else {
-            // If we are in normal drive mode use the motor controller to set our target velocity
-
-            // The CANSparkMaxes have a builtin PID controller on them we can use to set a target velocity.
-            // We first convert our speed from meters per second to rotations per minute, as that is the native unit of our devices
-            final double desiredDriveRotationsPerMinute = (state.speedMetersPerSecond * 60) / SwerveModuleConstants.WHEEL_CIRCUMFERENCE;
-            drivePIDController.setReference(desiredDriveRotationsPerMinute, ControlType.kVelocity);
-        }
-    }
-
-    /**
-     * Set the state of the swerve module. Will automatically optimize.
-     * The state is the speed and angle of the swerve module.
-     * You can use {@code Rotation2d.fromDegrees()} to create angle.
-     * 
-     * @param state New state of swerve module, contains speed in meters per second and angle.
-     */
-    public void setDesiredState(SwerveModuleState state, boolean powerDriveMode) {
-        setDesiredState(state, powerDriveMode, true);
-    }
-
-    // --- Public info getters ---
-
-    /**
-     * Get the position of the swerve module. The position is the distance traveled by the drive motor and angle of the drive motor.
-     * 
-     * @return Current position of swerve module, contains distance traveled by motor in meters and angle as {@link Rotation2d}
-     */
-    public SwerveModulePosition getPosition() {
-        return new SwerveModulePosition(
-                getDriveDistanceMeters(),
-                Rotation2d.fromRotations(getSteeringAngleRotations()));
-    }
-
-    /**
-     * Get locations of the wheel relative to the physical center of the robot.
-     * Useful for kinematics.
-     * 
-     * @return Translation2d representing distance from center of bot
-     */
-    public Translation2d getDistanceFromCenter() {
-        return distanceFromCenter;
-    }
-
-    // --- Private getters ---
-
-    /**
-     * Get the velocity of the drive motor.
-     * 
-     * @return Rotations per minute of the drive motor
-     */
-    private double getDriveSpeedRotationsPerMinute() {
-        return driveEncoder.getVelocity();
-    }
-
-    /**
-     * Get the velocity of the drive motor.
-     * 
-     * @return Meters per second of the drive wheel
-     */
-    private double getDriveSpeedMetersPerSecond() {
-        return (SwerveModuleConstants.WHEEL_CIRCUMFERENCE * getDriveSpeedRotationsPerMinute()) / 60;
-    }
-
-    /**
-     * Get the position of the drive motor.
-     * 
-     * @return Meters traveled by the drive wheel
-     */
-    private double getDriveDistanceMeters() {
-        return SwerveModuleConstants.WHEEL_CIRCUMFERENCE * getDriveDistanceRotations();
-    }
-
-    /**
-     * Get the position of the drive motor.
-     * 
-     * @return Total number of rotations of the drive motor
-     */
-    private double getDriveDistanceRotations() {
-        return driveEncoder.getPosition();
-    }
-
-    /**
-     * Get the angel of the steering motor.
-     * 
-     * @return Current position in rotations of the steering motor, accounting for offset
-     */
-    private double getSteeringAngleRotations() {
-        return steeringPosition.refresh().getValueAsDouble();
-    }
-
-    // --- Util ---
-
-    /**
-     * Optimize a swerve module state so that instead of suddenly rotating the wheel (with steering motor)
-     * to go a certain direction we can instead just turn a half as much and switch
-     * the speed of wheel to go in reverse.
-     * 
-     * @param desiredState The state you want the swerve module to be in
-     * @param currentAngle The current angle of the swerve module in degrees
-     * @return             An optimized version of desiredState
-     */
-    private static SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle) {
-        // find the target angle in the same 0-360 scope as the desired state
-        double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees());
-
-        // keep the same target speed
-        double targetSpeed = desiredState.speedMetersPerSecond;
-
-        // found how much we have to move to get to target angle
-        double delta = targetAngle - currentAngle.getDegrees();
-
-        // If we have to flip around more than 90 degrees than instead just reverse our direction
-        // and only turn enough so that we have the motor facing in the same direction, just the other way 
-        if (Math.abs(delta) > 90) {
-            targetSpeed = -targetSpeed;
-            targetAngle += delta > 0 ? -180 : 180;
-        }
-
-        return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle));
-    }
-
-    /**
-     * Utility method. Move an angle into the range of the reference. Finds the relative 0 and 360
-     * position for a scope reference and moves the new angle into that.
-     * Example: {@code placeInAppropriate0To360Scope(90, 370) = 10.0}
-     * {@code placeInAppropriate0To360Scope(720, 10) = 730.0}
-     * 
-     * @param scopeReference The reference to find which 0-360 scope we are in.
-     *                       For example {@code 10} is in {@code 0-360} scope while {@code 370} is in {@code 360-720} scope.
-     * @param newAngle       The angle we want to move into found scope.
-     *                       For example if the scope was {@code 0-360} and our angle was {@code 370} it would become {@code 10}
-     * @return               {@code newAngle} in the same scope as {@code scopeReference}
-     */
-    private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) {
-        double lowerBound;
-        double upperBound;
-        double lowerOffset = scopeReference % 360;
-        if (lowerOffset >= 0) {
-            lowerBound = scopeReference - lowerOffset;
-            upperBound = scopeReference + (360 - lowerOffset);
-        } else {
-            upperBound = scopeReference - lowerOffset;
-            lowerBound = scopeReference - (360 + lowerOffset);
-        }
-        while (newAngle < lowerBound) {
-            newAngle += 360;
-        }
-        while (newAngle > upperBound) {
-            newAngle -= 360;
-        }
-        if (newAngle - scopeReference > 180) {
-            newAngle -= 360;
-        } else if (newAngle - scopeReference < -180) {
-            newAngle += 360;
-        }
-        return newAngle;
-    }
-
-    @Override
-    public String toString() {
-        
-        final String[] yPositions = {"Back", "Middle", "Front"}; 
-        final String[] xPositions = {"Right", "Middle", "Left"};
-
-        final int ySignum = (int) Math.signum(distanceFromCenter.getY());
-        final int xSignum = (int) Math.signum(distanceFromCenter.getX());
-        
-        return xPositions[xSignum + 1] + yPositions[ySignum + 1] + "SwerveModule";
-    }
+	// the drive motor is the motor that spins the wheel making the robot move
+	// across the ground (aka wheel or velocity motor)
+	private final CANSparkMax driveMotor;
+	private final RelativeEncoder driveEncoder;
+	private final SparkPIDController drivePIDController;
+
+	// the steering motor is the motor that changes the rotation of the wheel
+	// allowing the robot to drive in any direction (aka spin or angular motor)
+	// Also allows for spinning in place
+	private final CANSparkMax steeringMotor;
+	private final CANcoder steeringEncoder;
+	private final PIDController steeringPIDController;
+
+	/** Status Signal that gives steering encoders current position in rotations */
+	private final StatusSignal<Double> steeringPosition;
+
+	/** Default state, forward and still */
+	private final static SwerveModuleState defaultState = new SwerveModuleState();
+
+	/** Locations of the wheel relative to the physical center of the robot. */
+	private final Translation2d distanceFromCenter;
+
+	/**
+	 * Constructor for an individual Swerve Module.
+	 * Sets up both drive and angular motor for swerve module as well as systems to
+	 * monitor and control them
+	 * 
+	 * @param velocityMotorDeviceID  device ID for drive motor
+	 * @param steeringMotorDeviceId  device ID for steering motor
+	 * @param angularEncoderDeviceID device ID for the angular motor's absolute
+	 *                               encoder
+	 * @param distanceFromCenter     distance from center of robot to center of
+	 *                               swerve module
+	 * @param steeringEncoderZero    the zero (forward) position for the angular
+	 *                               motor's absolute encoder, in rotations
+	 */
+	public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steeringAbsoluteEncoderId,
+			double steeringEncoderZero, Translation2d distanceFromCenter) {
+		// --- Drive Motor ---
+		driveMotor = new CANSparkMax(driveMotorDeviceId, MotorType.kBrushless);
+
+		// You most restore factory defaults if you want to use velocity encoder.
+		// If you do not do this, everything will break and shake itself to death
+		driveMotor.restoreFactoryDefaults();
+
+		driveMotor.setIdleMode(IdleMode.kBrake);
+
+		// --- Drive Encoder ---
+		driveEncoder = driveMotor.getEncoder();
+
+		driveEncoder.setVelocityConversionFactor(SwerveModuleConstants.DRIVE_MOTOR_GEAR_RATIO);
+		driveEncoder.setPositionConversionFactor(SwerveModuleConstants.DRIVE_MOTOR_GEAR_RATIO);
+
+		// --- Drive PID ---
+		drivePIDController = driveMotor.getPIDController();
+		drivePIDController.setP(SwerveModuleConstants.DRIVE_PID_P);
+		drivePIDController.setI(SwerveModuleConstants.DRIVE_PID_I);
+		drivePIDController.setD(SwerveModuleConstants.DRIVE_PID_D);
+		drivePIDController.setFF(SwerveModuleConstants.DRIVE_PID_FF);
+		drivePIDController.setIAccum(SwerveModuleConstants.DRIVE_PID_MAX_I);
+		drivePIDController.setOutputRange(-1, 1);
+
+		// --- Steering Motor ---
+		steeringMotor = new CANSparkMax(steeringMotorDeviceId, MotorType.kBrushless);
+
+		steeringMotor.setIdleMode(IdleMode.kBrake);
+
+		// --- Steering Encoder ---
+		steeringEncoder = new CANcoder(steeringAbsoluteEncoderId);
+
+		// Use 0-1 for rotational
+		steeringEncoder.getConfigurator().apply(
+				new MagnetSensorConfigs()
+						.withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
+						.withMagnetOffset(steeringEncoderZero));
+
+		// --- Steering PID ---
+		steeringPIDController = new PIDController(
+				SwerveModuleConstants.STEERING_PID_P,
+				SwerveModuleConstants.STEERING_PID_I,
+				SwerveModuleConstants.STEERING_PID_D);
+		steeringPIDController.enableContinuousInput(0, 1);
+
+		// --- Save other values ---
+		this.distanceFromCenter = distanceFromCenter;
+
+		steeringPosition = steeringEncoder.getAbsolutePosition();
+
+		setName(toString());
+	}
+
+	/**
+	 * This is the periodic function of the swerve module.
+	 * This method is called periodically by the CommandScheduler, about every 20ms.
+	 * 
+	 * We use it to constantly keep the
+	 */
+	@Override
+	public void periodic() {
+		// Calculate how fast to spin the motor to get to the desired angle using our
+		// PID controller, then set the motor to spin at that speed
+		steeringMotor.set(steeringPIDController.calculate(getSteeringAngleRotations()));
+	}
+
+	// --- Direct control methods ---
+
+	/**
+	 * Stop drive and steering motor of swerve module and set desired state to
+	 * nothing
+	 */
+	public void stop() {
+		// Manually stop both motors in swerve module
+		driveMotor.stopMotor();
+		steeringMotor.stopMotor();
+	}
+
+	/**
+	 * Set the desired state of the Swerve Module to the default/starting state.
+	 * This should have the module facing forward and not spinning.
+	 */
+	public void toDefaultState() {
+		setDesiredState(defaultState, false);
+	}
+
+	// --- Getters and setters for modules desired SwerveModuleState ---
+
+	/**
+	 * Get the state of the swerve module.
+	 * The state is the speed of our drive motor and angle of our steering motor.
+	 * 
+	 * @return Current state of swerve module, contains speed (in m/s) and angle as
+	 *         {@link Rotation2d}
+	 */
+	public SwerveModuleState getState() {
+		return new SwerveModuleState(
+				getDriveSpeedMetersPerSecond(),
+				Rotation2d.fromRotations(getSteeringAngleRotations()));
+	}
+
+	/**
+	 * Set the state of the swerve module. The state is the speed and angle of the
+	 * swerve module.
+	 * You can use {@code Rotation2d.fromDegrees()} to create angle.
+	 * 
+	 * @param state          New state of swerve module, contains speed in meters
+	 *                       per second and angle as {@link Rotation2d}
+	 * @param powerDriveMode whether the SwerveModuleState is in meters per second
+	 *                       (false) or motor power (true)
+	 * @param shouldOptimize Whether to optimize the way the swerve module gets to
+	 *                       the desired state
+	 */
+	public void setDesiredState(SwerveModuleState state, boolean powerDriveMode, boolean shouldOptimize) {
+		if (shouldOptimize && state != null) {
+			// Optimize the reference state to avoid spinning further than 90 degrees
+			state = optimize(state, getState().angle);
+		}
+
+		// --- Set steering motor ---
+		steeringPIDController.setSetpoint(state.angle.getRotations());
+
+		// --- Set drive motor ---
+
+		if (state.speedMetersPerSecond == 0) {
+			// If our desired speed is 0, just use the builtin motor stop, no matter the
+			// mode.
+
+			// Stops motor movement. Motor can be moved again by calling set without having
+			// to re-enable the motor.
+			driveMotor.stopMotor();
+		} else if (powerDriveMode) {
+			// If we are in power drive mode just directly set power to our desired speed.
+
+			// This is a bit of an abuse of the SwerveModuleState object as we treat speeds
+			// as power values from -1 to 1.
+			// We do this because we don't want to have to deal with a PID controller when
+			// we are just driving, as a human driver does not care about they exact speed
+			// mapping.
+			driveMotor.set(state.speedMetersPerSecond);
+		} else {
+			// If we are in normal drive mode use the motor controller to set our target
+			// velocity
+
+			// The CANSparkMaxes have a builtin PID controller on them we can use to set a
+			// target velocity.
+			// We first convert our speed from meters per second to rotations per minute, as
+			// that is the native unit of our devices
+			final double desiredDriveRotationsPerMinute = (state.speedMetersPerSecond * 60)
+					/ SwerveModuleConstants.WHEEL_CIRCUMFERENCE;
+			drivePIDController.setReference(desiredDriveRotationsPerMinute, ControlType.kVelocity);
+		}
+	}
+
+	/**
+	 * Set the state of the swerve module. Will automatically optimize.
+	 * The state is the speed and angle of the swerve module.
+	 * You can use {@code Rotation2d.fromDegrees()} to create angle.
+	 * 
+	 * @param state New state of swerve module, contains speed in meters per second
+	 *              and angle.
+	 */
+	public void setDesiredState(SwerveModuleState state, boolean powerDriveMode) {
+		setDesiredState(state, powerDriveMode, true);
+	}
+
+	// --- Public info getters ---
+
+	/**
+	 * Get the position of the swerve module. The position is the distance traveled
+	 * by the drive motor and angle of the drive motor.
+	 * 
+	 * @return Current position of swerve module, contains distance traveled by
+	 *         motor in meters and angle as {@link Rotation2d}
+	 */
+	public SwerveModulePosition getPosition() {
+		return new SwerveModulePosition(
+				getDriveDistanceMeters(),
+				Rotation2d.fromRotations(getSteeringAngleRotations()));
+	}
+
+	/**
+	 * Get locations of the wheel relative to the physical center of the robot.
+	 * Useful for kinematics.
+	 * 
+	 * @return Translation2d representing distance from center of bot
+	 */
+	public Translation2d getDistanceFromCenter() {
+		return distanceFromCenter;
+	}
+
+	// --- Private getters ---
+
+	/**
+	 * Get the velocity of the drive motor.
+	 * 
+	 * @return Rotations per minute of the drive motor
+	 */
+	private double getDriveSpeedRotationsPerMinute() {
+		return driveEncoder.getVelocity();
+	}
+
+	/**
+	 * Get the velocity of the drive motor.
+	 * 
+	 * @return Meters per second of the drive wheel
+	 */
+	private double getDriveSpeedMetersPerSecond() {
+		return (SwerveModuleConstants.WHEEL_CIRCUMFERENCE * getDriveSpeedRotationsPerMinute()) / 60;
+	}
+
+	/**
+	 * Get the position of the drive motor.
+	 * 
+	 * @return Meters traveled by the drive wheel
+	 */
+	private double getDriveDistanceMeters() {
+		return SwerveModuleConstants.WHEEL_CIRCUMFERENCE * getDriveDistanceRotations();
+	}
+
+	/**
+	 * Get the position of the drive motor.
+	 * 
+	 * @return Total number of rotations of the drive motor
+	 */
+	private double getDriveDistanceRotations() {
+		return driveEncoder.getPosition();
+	}
+
+	/**
+	 * Get the angel of the steering motor.
+	 * 
+	 * @return Current position in rotations of the steering motor, accounting for
+	 *         offset
+	 */
+	private double getSteeringAngleRotations() {
+		return steeringPosition.refresh().getValueAsDouble();
+	}
+
+	// --- Util ---
+
+	/**
+	 * Optimize a swerve module state so that instead of suddenly rotating the wheel
+	 * (with steering motor)
+	 * to go a certain direction we can instead just turn a half as much and switch
+	 * the speed of wheel to go in reverse.
+	 * 
+	 * @param desiredState The state you want the swerve module to be in
+	 * @param currentAngle The current angle of the swerve module in degrees
+	 * @return An optimized version of desiredState
+	 */
+	private static SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle) {
+		// find the target angle in the same 0-360 scope as the desired state
+		double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees());
+
+		// keep the same target speed
+		double targetSpeed = desiredState.speedMetersPerSecond;
+
+		// found how much we have to move to get to target angle
+		double delta = targetAngle - currentAngle.getDegrees();
+
+		// If we have to flip around more than 90 degrees than instead just reverse our
+		// direction
+		// and only turn enough so that we have the motor facing in the same direction,
+		// just the other way
+		if (Math.abs(delta) > 90) {
+			targetSpeed = -targetSpeed;
+			targetAngle += delta > 0 ? -180 : 180;
+		}
+
+		return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle));
+	}
+
+	/**
+	 * Utility method. Move an angle into the range of the reference. Finds the
+	 * relative 0 and 360
+	 * position for a scope reference and moves the new angle into that.
+	 * Example: {@code placeInAppropriate0To360Scope(90, 370) = 10.0}
+	 * {@code placeInAppropriate0To360Scope(720, 10) = 730.0}
+	 * 
+	 * @param scopeReference The reference to find which 0-360 scope we are in.
+	 *                       For example {@code 10} is in {@code 0-360} scope while
+	 *                       {@code 370} is in {@code 360-720} scope.
+	 * @param newAngle       The angle we want to move into found scope.
+	 *                       For example if the scope was {@code 0-360} and our
+	 *                       angle was {@code 370} it would become {@code 10}
+	 * @return {@code newAngle} in the same scope as {@code scopeReference}
+	 */
+	private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) {
+		double lowerBound;
+		double upperBound;
+		double lowerOffset = scopeReference % 360;
+		if (lowerOffset >= 0) {
+			lowerBound = scopeReference - lowerOffset;
+			upperBound = scopeReference + (360 - lowerOffset);
+		} else {
+			upperBound = scopeReference - lowerOffset;
+			lowerBound = scopeReference - (360 + lowerOffset);
+		}
+		while (newAngle < lowerBound) {
+			newAngle += 360;
+		}
+		while (newAngle > upperBound) {
+			newAngle -= 360;
+		}
+		if (newAngle - scopeReference > 180) {
+			newAngle -= 360;
+		} else if (newAngle - scopeReference < -180) {
+			newAngle += 360;
+		}
+		return newAngle;
+	}
+
+	@Override
+	public String toString() {
+
+		final String[] yPositions = { "Back", "Middle", "Front" };
+		final String[] xPositions = { "Right", "Middle", "Left" };
+
+		final int ySignum = (int) Math.signum(distanceFromCenter.getY());
+		final int xSignum = (int) Math.signum(distanceFromCenter.getX());
+
+		return xPositions[xSignum + 1] + yPositions[ySignum + 1] + "SwerveModule";
+	}
 }
diff --git a/src/main/java/frc/robot/utils/OptionButton.java b/src/main/java/frc/robot/utils/OptionButton.java
index 90d9ae1..19e614d 100644
--- a/src/main/java/frc/robot/utils/OptionButton.java
+++ b/src/main/java/frc/robot/utils/OptionButton.java
@@ -11,96 +11,114 @@
  */
 public class OptionButton {
 
-    private final Supplier<Trigger> button;
-
-    private final ActivationMode mode;
-
-    private boolean isToggled;
-
-    /**
-     * Different modes which a OptionButton can be in
-     */
-    public static enum ActivationMode {
-        /** Like toggle, but stays on until manually toggled off with .toggleOff(). Probably just use .onTrue() instead */
-        TAP,
-
-        /** standard button behavior, on when pressed, off when released */
-        HOLD,
-
-        /** makes button into on/off toggle */
-        TOGGLE
-    }
-    /**
-     * Create Option button.
-     * 
-     * @param commandDevice device that button is on
-     * @param button button number
-     * @param mode whether or not we want button to act as toggle or hold button
-     */
-    public OptionButton(CommandGenericHID controller, int button, ActivationMode mode) {
-        this(() -> controller.button(button), mode);
-    }
-    
-    /**
-     * Create Option button.
-     * 
-     * @param commandDevice device that button is on
-     * @param button button number
-     * @param mode whether or not we want button to act as toggle or hold button
-     */
-    public OptionButton(Supplier<Trigger> button, ActivationMode mode) {
-        this.button = button;
-        this.mode = mode;
-
-        if (mode == ActivationMode.TOGGLE) {
-            button.get().onTrue(new InstantCommand(this::toggle));
-        }
-        else if (mode == ActivationMode.TAP) {
-            button.get().onTrue(new InstantCommand(this::toggleOn));
-        }
-    }
-
-    /** Manually toggle state, has no effect if it is hold button */
-    public void toggle() {
-        isToggled = !isToggled;
-    }
-
-    /** Manually toggle state on, has no effect if it is hold button */
-    public void toggleOn() {
-        isToggled = true;
-    }
-
-    /** Manually toggle state off, has no effect if it is hold button */
-    public void toggleOff() {
-        isToggled = false;
-    }
-
-    /**
-     * <p>Get the state of the button</p>
-     * 
-     * <p>for TAP buttons this returns true if the button has been tapped</p>
-     * 
-     * <p>for HOLD buttons this returns true if the button is currently being held down</p>
-     * 
-     * <p>for TOGGLE buttons this returns true if the button has been tapped an odd number of times</p>
-     * 
-     * @return boolean with true being the button is active
-     */
-    public boolean getState() {
-        switch (mode) {
-            case HOLD: return button.get().getAsBoolean();       
-            case TAP: return isToggled;
-            case TOGGLE: return isToggled;
-            default: return false; 
-        }
-    }
-
-    /**
-     * Get state, but return it as a int instead of a boolean
-     * 
-     * @return 1 if true, 0 is false
-     */
-    public int getStateAsInt() {
-        return getState() ? 1 : 0;
-    }
+	private final Supplier<Trigger> button;
+
+	private final ActivationMode mode;
+
+	private boolean isToggled;
+
+	/**
+	 * Different modes which a OptionButton can be in
+	 */
+	public static enum ActivationMode {
+		/**
+		 * Like toggle, but stays on until manually toggled off with .toggleOff().
+		 * Probably just use .onTrue() instead
+		 */
+		TAP,
+
+		/** standard button behavior, on when pressed, off when released */
+		HOLD,
+
+		/** makes button into on/off toggle */
+		TOGGLE
+	}
+
+	/**
+	 * Create Option button.
+	 * 
+	 * @param commandDevice device that button is on
+	 * @param button        button number
+	 * @param mode          whether or not we want button to act as toggle or hold
+	 *                      button
+	 */
+	public OptionButton(CommandGenericHID controller, int button, ActivationMode mode) {
+		this(() -> controller.button(button), mode);
+	}
+
+	/**
+	 * Create Option button.
+	 * 
+	 * @param commandDevice device that button is on
+	 * @param button        button number
+	 * @param mode          whether or not we want button to act as toggle or hold
+	 *                      button
+	 */
+	public OptionButton(Supplier<Trigger> button, ActivationMode mode) {
+		this.button = button;
+		this.mode = mode;
+
+		if (mode == ActivationMode.TOGGLE) {
+			button.get().onTrue(new InstantCommand(this::toggle));
+		} else if (mode == ActivationMode.TAP) {
+			button.get().onTrue(new InstantCommand(this::toggleOn));
+		}
+	}
+
+	/** Manually toggle state, has no effect if it is hold button */
+	public void toggle() {
+		isToggled = !isToggled;
+	}
+
+	/** Manually toggle state on, has no effect if it is hold button */
+	public void toggleOn() {
+		isToggled = true;
+	}
+
+	/** Manually toggle state off, has no effect if it is hold button */
+	public void toggleOff() {
+		isToggled = false;
+	}
+
+	/**
+	 * <p>
+	 * Get the state of the button
+	 * </p>
+	 * 
+	 * <p>
+	 * for TAP buttons this returns true if the button has been tapped
+	 * </p>
+	 * 
+	 * <p>
+	 * for HOLD buttons this returns true if the button is currently being held down
+	 * </p>
+	 * 
+	 * <p>
+	 * for TOGGLE buttons this returns true if the button has been tapped an odd
+	 * number of times
+	 * </p>
+	 * 
+	 * @return boolean with true being the button is active
+	 */
+	public boolean getState() {
+		switch (mode) {
+			case HOLD:
+				return button.get().getAsBoolean();
+			case TAP:
+				return isToggled;
+			case TOGGLE:
+				return isToggled;
+			default:
+				return false;
+		}
+	}
+
+	/**
+	 * Get state, but return it as a int instead of a boolean
+	 * 
+	 * @return 1 if true, 0 is false
+	 */
+	public int getStateAsInt() {
+		return getState() ? 1 : 0;
+	}
 }

From ba6cff6847f1d04b39a403d281edbede98047880 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Mon, 5 Feb 2024 16:58:35 -0800
Subject: [PATCH 27/99] Rewrite some new DriveToPose and create

---
 .../java/frc/robot/commands/DriveToPose.java  |  94 ++-------------
 .../frc/robot/commands/DriveToPoseBase.java   | 107 ++++++++++++++++++
 .../frc/robot/commands/DriveTransform.java    |  21 +---
 .../java/frc/robot/commands/FollowTag.java    |  80 ++++++++-----
 4 files changed, 171 insertions(+), 131 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/DriveToPoseBase.java

diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java
index 944d6c8..e69574d 100644
--- a/src/main/java/frc/robot/commands/DriveToPose.java
+++ b/src/main/java/frc/robot/commands/DriveToPose.java
@@ -1,103 +1,25 @@
 package frc.robot.commands;
 
-import frc.robot.subsystems.SwerveDrivetrain;
-import edu.wpi.first.wpilibj2.command.Command;
-
-import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.Constants.RobotMovementConstants;
+import frc.robot.subsystems.SwerveDrivetrain;
 
 /** Command to automatically drive to a certain Pose on the field */
-public class DriveToPose extends Command {
-    private final SwerveDrivetrain drivetrain;
-    private final PIDController xController, yController, rotationController;
+public class DriveToPose extends DriveToPoseBase {
 
     /**
-     * Create a new DriveToPose command. Tries to drive to a certain Pose based on odometry 
+     * Create a new DriveToPose command. Tries to drive to a set Pose based on odometry 
      * 
      * <p>This drives relative to the robot starting position, so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
      * where forward is whatever direction the robot started in</p>
      * 
      * <p>The last place the drivetrain position was reset counts as the starting position</p>
      * 
-     * @param drivetrain The drivetrain of the robot
-     * @param targetPose Target pose
+     * @param drivetrain the drivetrain of the robot
+     * @param targetPosition the pose the robot is trying to achieve
      */
-    public DriveToPose(SwerveDrivetrain drivetrain, Pose2d targetPose) {
-
-        // Save drivetrain
-        this.drivetrain = drivetrain;
-
-        // Setup all PID controllers
-        xController = new PIDController(
-            RobotMovementConstants.TRANSLATION_PID_P,
-            RobotMovementConstants.TRANSLATION_PID_I,
-            RobotMovementConstants.TRANSLATION_PID_D
-        );
-        xController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
-
-        yController = new PIDController(
-            RobotMovementConstants.TRANSLATION_PID_P,
-            RobotMovementConstants.TRANSLATION_PID_I,
-            RobotMovementConstants.TRANSLATION_PID_D
-        );
-        yController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
-
-        rotationController = new PIDController(
-            RobotMovementConstants.ROTATION_PID_P,
-            RobotMovementConstants.ROTATION_PID_I,
-            RobotMovementConstants.ROTATION_PID_D
-        );
-        rotationController.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
-
-        // Set setpoints 
-        xController.setSetpoint(targetPose.getX());
-        yController.setSetpoint(targetPose.getY());
-        rotationController.setSetpoint(targetPose.getRotation().getRadians());
-
-        // Add drivetrain as a requirement so no other commands try to use it
-        addRequirements(this.drivetrain);
-    }
-
-    @Override
-    public void initialize() {
-        // Put all swerve modules in default position
-        drivetrain.toDefaultStates();
-    }
-
-    @Override
-    public void execute() {
-
-        // Get our current pose
-        final Pose2d measuredPosition = drivetrain.getPosition();
-
-        // Put current pose position on SmartDashboard
-        SmartDashboard.putNumber("PoseY", measuredPosition.getY());
-        SmartDashboard.putNumber("PoseX", measuredPosition.getX());
-		SmartDashboard.putNumber("PoseDegrees", measuredPosition.getRotation().getDegrees()); 
-
-        // Calculate our robot speeds with the PID controllers
-        final ChassisSpeeds speeds = new ChassisSpeeds(
-            xController.calculate(measuredPosition.getX()),
-            yController.calculate(measuredPosition.getY()),
-            rotationController.calculate(measuredPosition.getRotation().getRadians())
-        );
-
-        // Set those speeds
-        drivetrain.setDesiredState(speeds);
-    }
-
-    @Override
-    public boolean isFinished() {
-        // Finish once all controllers are within tolerance
-        return xController.atSetpoint() && yController.atSetpoint() && rotationController.atSetpoint();
-    }
+    public DriveToPose(SwerveDrivetrain drivetrain, Pose2d targetPosition) {
+        super(drivetrain);
 
-    @Override
-    public void end(boolean interrupted) {
-        // Stop all swerve modules at end
-        drivetrain.stop();
+        setDesiredPosition(targetPosition);
     }
 }
diff --git a/src/main/java/frc/robot/commands/DriveToPoseBase.java b/src/main/java/frc/robot/commands/DriveToPoseBase.java
new file mode 100644
index 0000000..1726f65
--- /dev/null
+++ b/src/main/java/frc/robot/commands/DriveToPoseBase.java
@@ -0,0 +1,107 @@
+package frc.robot.commands;
+
+import frc.robot.subsystems.SwerveDrivetrain;
+import edu.wpi.first.wpilibj2.command.Command;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Constants.RobotMovementConstants;
+
+/** Base Command for automatically driving to a certain Pose on the field */
+public class DriveToPoseBase extends Command {
+    private final SwerveDrivetrain drivetrain;
+    private final PIDController xController, yController, rotationController;
+
+    /**
+     * Create a new DriveToPose command. Tries to drive to a set Pose based on odometry 
+     * 
+     * <p>This drives relative to the robot starting position, so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
+     * where forward is whatever direction the robot started in</p>
+     * 
+     * <p>The last place the drivetrain position was reset counts as the starting position</p>
+     * 
+     * @param drivetrain the drivetrain of the robot
+     */
+    public DriveToPoseBase(SwerveDrivetrain drivetrain) {
+
+        // Save drivetrain
+        this.drivetrain = drivetrain;
+
+        // Setup all PID controllers
+        xController = new PIDController(
+            RobotMovementConstants.TRANSLATION_PID_P,
+            RobotMovementConstants.TRANSLATION_PID_I,
+            RobotMovementConstants.TRANSLATION_PID_D
+        );
+        xController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
+
+        yController = new PIDController(
+            RobotMovementConstants.TRANSLATION_PID_P,
+            RobotMovementConstants.TRANSLATION_PID_I,
+            RobotMovementConstants.TRANSLATION_PID_D
+        );
+        yController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
+
+        rotationController = new PIDController(
+            RobotMovementConstants.ROTATION_PID_P,
+            RobotMovementConstants.ROTATION_PID_I,
+            RobotMovementConstants.ROTATION_PID_D
+        );
+        rotationController.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
+
+        // Add drivetrain as a requirement so no other commands try to use it
+        addRequirements(this.drivetrain);
+    }
+
+    public void setDesiredPosition(Pose2d targetPose) {
+        xController.setSetpoint(targetPose.getX());
+        yController.setSetpoint(targetPose.getY());
+        rotationController.setSetpoint(targetPose.getRotation().getRadians());
+    }
+
+    public Pose2d getPosition() {
+        return drivetrain.getPosition();
+    }
+
+    @Override
+    public void initialize() {
+        // Put all swerve modules in default position
+        drivetrain.toDefaultStates();
+    }
+
+    @Override
+    public void execute() {
+
+        // Get our current pose
+        final Pose2d measuredPosition = getPosition();
+
+        // Put current pose position on SmartDashboard
+        SmartDashboard.putNumber("PoseY", measuredPosition.getY());
+        SmartDashboard.putNumber("PoseX", measuredPosition.getX());
+		SmartDashboard.putNumber("PoseDegrees", measuredPosition.getRotation().getDegrees()); 
+
+        // Calculate our robot speeds with the PID controllers
+        final ChassisSpeeds speeds = new ChassisSpeeds(
+            xController.calculate(measuredPosition.getX()),
+            yController.calculate(measuredPosition.getY()),
+            rotationController.calculate(measuredPosition.getRotation().getRadians())
+        );
+
+        // Set those speeds
+        drivetrain.setDesiredState(speeds);
+    }
+
+    @Override
+    public boolean isFinished() {
+        // Finish once all controllers are within tolerance
+        return xController.atSetpoint() && yController.atSetpoint() && rotationController.atSetpoint();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        // Stop all swerve modules at end
+        drivetrain.stop();
+    }
+}
diff --git a/src/main/java/frc/robot/commands/DriveTransform.java b/src/main/java/frc/robot/commands/DriveTransform.java
index 050024c..c6d2913 100644
--- a/src/main/java/frc/robot/commands/DriveTransform.java
+++ b/src/main/java/frc/robot/commands/DriveTransform.java
@@ -4,13 +4,10 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.subsystems.SwerveDrivetrain;
 
 /** Command to automatically drive a certain transform */
-public class DriveTransform extends Command {
-    private final SwerveDrivetrain drivetrain;
+public class DriveTransform extends DriveToPoseBase {
     private final Transform2d transform;
 
     /**
@@ -19,11 +16,11 @@ public class DriveTransform extends Command {
      * <p>This drives relative to the robot, so a transform of +2x and +1y will drive 2 meters forward and 1 meter left,
      * where forward is whatever direction the robot is currently facing</p>
      * 
-     * @param drivetrain The drivetrain of the robot
-     * @param transform Target transform to drive to, will be added to current position to get target pose
+     * @param drivetrain the drivetrain of the robot
+     * @param transform target transform to drive to, will be added to current position to get target pose
      */
     public DriveTransform(SwerveDrivetrain drivetrain, Transform2d transform) {
-        this.drivetrain = drivetrain;
+        super(drivetrain);
         this.transform = transform;
     }
     
@@ -43,14 +40,6 @@ public DriveTransform(SwerveDrivetrain drivetrain, Translation2d translation, Ro
 
     @Override
     public void initialize() {
-        CommandScheduler.getInstance().schedule(
-            new DriveToPose(drivetrain, drivetrain.getPosition().plus(transform))
-        );
-    }
-
-    @Override
-    public boolean isFinished() {
-        // Instantly finish since the entire command just calls another command 
-        return true;
+        setDesiredPosition(getPosition().plus(transform));
     }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java
index 96d4485..67c6ac7 100644
--- a/src/main/java/frc/robot/commands/FollowTag.java
+++ b/src/main/java/frc/robot/commands/FollowTag.java
@@ -1,47 +1,69 @@
 package frc.robot.commands;
 
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants.RobotMovementConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
-import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
-import frc.robot.Constants.RobotMovementConstants;
 
-public class FollowTag extends Command {
-    private final SwerveDrivetrain drivetrain;
+/** Command to automatically drive a follow a tag a certain translation away */
+public class FollowTag extends DriveToPoseBase {
     private final int tagID;
-    private final Translation2d targetDistance;
-    private final PIDController xController, yController, rotationController;
+    private final Transform2d targetDistance;
+
+    private final double loseTagAfterSeconds;
+    private double secondsSinceTagLastSeen;
+
+    /**
+     * Create a new FollowTag command. Tries to follow a tag while staying a certain distance away.
+     * 
+     * @param drivetrain the drivetrain of the robot
+     * @param tagID the numerical ID of the the tag to follow
+     * @param targetDistanceToTag the target distance away from the tag to be
+     * @param loseTagAfterSeconds how long to wait before giving up on rediscover tag, set to -1 to never finish
+     */
+    public FollowTag(SwerveDrivetrain drivetrain, int tagID, Transform2d targetDistanceToTag, int loseTagAfterSeconds) {
+        super(drivetrain);
 
-    public FollowTag(SwerveDrivetrain drivetrain, int tagID, Translation2d targetDistanceToTag) {
-        this.drivetrain = drivetrain;
         this.tagID = tagID;
         this.targetDistance = targetDistanceToTag;
+        this.loseTagAfterSeconds = loseTagAfterSeconds;
+    }
+    
+    /**
+     * Create a new FollowTag command. Tries to follow a tag while staying a certain distance away.
+     * 
+     * @param drivetrain the drivetrain of the robot
+     * @param tagID the numerical ID of the the tag to follow
+     * @param targetDistanceToTag the target distance away from the tag to be
+     * @param loseTagAfterSeconds how long to wait before giving up on rediscover tag, set to -1 to never finish
+     */
+    public FollowTag(SwerveDrivetrain drivetrain, int tagID, Translation2d targetDistanceToTag, int loseTagAfterSeconds) {
+        this(drivetrain, tagID, new Transform2d(targetDistanceToTag, new Rotation2d()), loseTagAfterSeconds);
+    }
 
-        xController = new PIDController(
-            RobotMovementConstants.TRANSLATION_PID_P,
-            RobotMovementConstants.TRANSLATION_PID_I,
-            RobotMovementConstants.TRANSLATION_PID_D
-        );
-        xController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
+    @Override
+    public void execute() {
 
-        yController = new PIDController(
-            RobotMovementConstants.TRANSLATION_PID_P,
-            RobotMovementConstants.TRANSLATION_PID_I,
-            RobotMovementConstants.TRANSLATION_PID_D
-        );
-        yController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
+        // Sudo code, assume distance from front center of robot
+        final Transform3d tagPosition3d = new Transform3d(tagID * 5, 0, 0, new Rotation3d());
 
-        rotationController = new PIDController(
-            RobotMovementConstants.ROTATION_PID_P,
-            RobotMovementConstants.ROTATION_PID_I,
-            RobotMovementConstants.ROTATION_PID_D
+        // https://docs.photonvision.org/en/latest/docs/apriltag-pipelines/coordinate-systems.html
+        // https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/geometry/Rotation3d.html#getZ()
+        final Transform2d tagPosition = new Transform2d(
+            tagPosition3d.getZ(),
+            tagPosition3d.getX(),
+            Rotation2d.fromRadians(tagPosition3d.getRotation().getZ())
         );
-        rotationController.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
+
+        final Transform2d driveTransform = tagPosition.plus(targetDistance.inverse());
+
+        setDesiredPosition(getPosition().plus(driveTransform));
     }
 
     @Override
-    public void execute() {
-        
+    public boolean isFinished() {
+        return (loseTagAfterSeconds != -1) && (secondsSinceTagLastSeen < loseTagAfterSeconds);
     }
 }

From b0794172f5c3e37f9d87a5c334f183ba23d6d162 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Mon, 5 Feb 2024 17:27:21 -0800
Subject: [PATCH 28/99] Revert "rename teleop control commands"

This reverts commit 6f212f5634a990fd3108187612ddae90ce5f879d.
---
 src/main/java/frc/robot/RobotContainer.java          | 12 ++++++------
 ...{BaseControl.java => SwerveDriveBaseControl.java} |  4 ++--
 ...kControl.java => SwerveDriveJoystickControl.java} |  6 +++---
 .../{PS4Control.java => SwerveDrivePS4Control.java}  |  4 ++--
 ...{XboxControl.java => SwerveDriveXboxControl.java} |  4 ++--
 5 files changed, 15 insertions(+), 15 deletions(-)
 rename src/main/java/frc/robot/commands/SwerveRemoteOperation/{BaseControl.java => SwerveDriveBaseControl.java} (95%)
 rename src/main/java/frc/robot/commands/SwerveRemoteOperation/{JoystickControl.java => SwerveDriveJoystickControl.java} (93%)
 rename src/main/java/frc/robot/commands/SwerveRemoteOperation/{PS4Control.java => SwerveDrivePS4Control.java} (92%)
 rename src/main/java/frc/robot/commands/SwerveRemoteOperation/{XboxControl.java => SwerveDriveXboxControl.java} (94%)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index b9b2d40..ab16728 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,9 +4,9 @@
 import frc.robot.Constants.SwerveDrivetrainConstants;
 import frc.robot.Constants.SwerveModuleConstants;
 import frc.robot.commands.Autos;
-import frc.robot.commands.SwerveRemoteOperation.BaseControl;
-import frc.robot.commands.SwerveRemoteOperation.JoystickControl;
-import frc.robot.commands.SwerveRemoteOperation.XboxControl;
+import frc.robot.commands.SwerveRemoteOperation.SwerveDriveBaseControl;
+import frc.robot.commands.SwerveRemoteOperation.SwerveDriveJoystickControl;
+import frc.robot.commands.SwerveRemoteOperation.SwerveDriveXboxControl;
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.SwerveModule;
 
@@ -81,12 +81,12 @@ public void setUpDriveController() {
 
         drivetrain.removeDefaultCommand();
 
-        BaseControl control;
+        SwerveDriveBaseControl control;
 
         if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
-            control = new JoystickControl(drivetrain, new CommandJoystick(genericHID.getPort()));
+            control = new SwerveDriveJoystickControl(drivetrain, new CommandJoystick(genericHID.getPort()));
         } else {
-            control = new XboxControl(drivetrain, new CommandXboxController(genericHID.getPort()));
+            control = new SwerveDriveXboxControl(drivetrain, new CommandXboxController(genericHID.getPort()));
         }
 
         drivetrain.setDefaultCommand(control);
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/BaseControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java
similarity index 95%
rename from src/main/java/frc/robot/commands/SwerveRemoteOperation/BaseControl.java
rename to src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java
index 9beb53e..74dd343 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/BaseControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java
@@ -10,7 +10,7 @@
 /**
  * This can be the default command for the drivetrain, allowing for remote operation with a controller
  */
-public abstract class BaseControl extends Command {
+public abstract class SwerveDriveBaseControl extends Command {
    protected final SwerveDrivetrain drivetrain;
    protected final CommandGenericHID controller;
 
@@ -20,7 +20,7 @@ public abstract class BaseControl extends Command {
     * @param drivetrain       The drivetrain of the robot
     * @param driverController The device used to control drivetrain
     */
-   public BaseControl(SwerveDrivetrain drivetrain, CommandGenericHID driverController) {
+   public SwerveDriveBaseControl(SwerveDrivetrain drivetrain, CommandGenericHID driverController) {
 
       // save parameters
       this.controller = driverController;
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
similarity index 93%
rename from src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java
rename to src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
index b617a95..bd74cbb 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
@@ -12,7 +12,7 @@
 /**
  * This is the default command for the drivetrain, allowing for remote operation with joystick
  */
-public class JoystickControl extends BaseControl {
+public class SwerveDriveJoystickControl extends SwerveDriveBaseControl {
     private final OptionButton preciseModeButton;
     private final OptionButton boostModeButton;
     private final OptionButton fieldRelativeButton;
@@ -23,7 +23,7 @@ public class JoystickControl extends BaseControl {
 	 * @param drivetrain The drivetrain of the robot
 	 * @param driverJoystick The joystick used to control drivetrain
 	 */
-    public JoystickControl(SwerveDrivetrain drivetrain, CommandJoystick driverJoystick) {
+    public SwerveDriveJoystickControl(SwerveDrivetrain drivetrain, CommandJoystick driverJoystick) {
         super(drivetrain, driverJoystick);
 
         // Create and configure buttons
@@ -31,7 +31,7 @@ public JoystickControl(SwerveDrivetrain drivetrain, CommandJoystick driverJoysti
         boostModeButton = new OptionButton(driverJoystick, 1, ActivationMode.HOLD);
         fieldRelativeButton = new OptionButton(driverJoystick, 3, ActivationMode.TOGGLE);
 
-        driverJoystick.button(5).onTrue(new InstantCommand(drivetrain::resetPosition));
+        driverJoystick.button(5).onTrue(new InstantCommand(drivetrain::zeroGyroYaw));
     }
 
     @Override
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java
similarity index 92%
rename from src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java
rename to src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java
index 040f355..4b9d769 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java
@@ -11,7 +11,7 @@
 /**
  * This is the default command for the drivetrain, allowing for remote operation with PS4 controller
  */
-public class PS4Control extends BaseControl {
+public class SwerveDrivePS4Control extends SwerveDriveBaseControl {
     private final OptionButton preciseModeButton;
     private final OptionButton boostModeButton;
 
@@ -21,7 +21,7 @@ public class PS4Control extends BaseControl {
      * @param drivetrain The drive train of the robot
      * @param driverPS4Controller The PS4 controller used to control the drive train
      */
-    public PS4Control(SwerveDrivetrain drivetrain, CommandPS4Controller driverPS4Controller){
+    public SwerveDrivePS4Control(SwerveDrivetrain drivetrain, CommandPS4Controller driverPS4Controller){
         super(drivetrain, driverPS4Controller);
 
         // Create and configure buttons
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/XboxControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
similarity index 94%
rename from src/main/java/frc/robot/commands/SwerveRemoteOperation/XboxControl.java
rename to src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
index e5c139a..54901f4 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/XboxControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
@@ -14,7 +14,7 @@
 /**
  * This is the default command for the drivetrain, allowing for remote operation with xbox controller
  */
-public class XboxControl extends BaseControl {
+public class SwerveDriveXboxControl extends SwerveDriveBaseControl {
     private final OptionButton preciseModeButton;
     private final OptionButton boostModeButton;
     private final OptionButton fieldRelativeButton;
@@ -27,7 +27,7 @@ public class XboxControl extends BaseControl {
      * @param drivetrain           The drivetrain of the robot
      * @param driverXboxController The xbox controller used to control drivetrain
      */
-    public XboxControl(SwerveDrivetrain drivetrain, CommandXboxController driverXboxController) {
+    public SwerveDriveXboxControl(SwerveDrivetrain drivetrain, CommandXboxController driverXboxController) {
         super(drivetrain, driverXboxController);
         
         // Create and configure buttons

From 317a6dda5e6b507729ec964ef72e96fb9522c9bd Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Mon, 5 Feb 2024 17:31:09 -0800
Subject: [PATCH 29/99] Rename some files

---
 src/main/java/frc/robot/RobotContainer.java          | 12 ++++++------
 ...{SwerveDriveBaseControl.java => BaseControl.java} |  4 ++--
 ...veDriveXboxControl.java => DriveXboxControl.java} |  4 ++--
 ...riveJoystickControl.java => JoystickControl.java} |  7 ++-----
 .../{SwerveDrivePS4Control.java => PS4Control.java}  |  4 ++--
 5 files changed, 14 insertions(+), 17 deletions(-)
 rename src/main/java/frc/robot/commands/SwerveRemoteOperation/{SwerveDriveBaseControl.java => BaseControl.java} (95%)
 rename src/main/java/frc/robot/commands/SwerveRemoteOperation/{SwerveDriveXboxControl.java => DriveXboxControl.java} (94%)
 rename src/main/java/frc/robot/commands/SwerveRemoteOperation/{SwerveDriveJoystickControl.java => JoystickControl.java} (89%)
 rename src/main/java/frc/robot/commands/SwerveRemoteOperation/{SwerveDrivePS4Control.java => PS4Control.java} (92%)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index a42c099..28b63a5 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,9 +4,9 @@
 import frc.robot.Constants.SwerveDrivetrainConstants;
 import frc.robot.Constants.SwerveModuleConstants;
 import frc.robot.commands.Autos;
-import frc.robot.commands.SwerveRemoteOperation.SwerveDriveBaseControl;
-import frc.robot.commands.SwerveRemoteOperation.SwerveDriveJoystickControl;
-import frc.robot.commands.SwerveRemoteOperation.SwerveDriveXboxControl;
+import frc.robot.commands.SwerveRemoteOperation.BaseControl;
+import frc.robot.commands.SwerveRemoteOperation.JoystickControl;
+import frc.robot.commands.SwerveRemoteOperation.DriveXboxControl;
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.SwerveModule;
 
@@ -91,12 +91,12 @@ public void setUpDriveController() {
 
 		drivetrain.removeDefaultCommand();
 
-		SwerveDriveBaseControl control;
+		BaseControl control;
 
 		if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
-			control = new SwerveDriveJoystickControl(drivetrain, new CommandJoystick(genericHID.getPort()));
+			control = new JoystickControl(drivetrain, new CommandJoystick(genericHID.getPort()));
 		} else {
-			control = new SwerveDriveXboxControl(drivetrain, new CommandXboxController(genericHID.getPort()));
+			control = new DriveXboxControl(drivetrain, new CommandXboxController(genericHID.getPort()));
 		}
 
 		drivetrain.setDefaultCommand(control);
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/BaseControl.java
similarity index 95%
rename from src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java
rename to src/main/java/frc/robot/commands/SwerveRemoteOperation/BaseControl.java
index 7911871..1a80d29 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveBaseControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/BaseControl.java
@@ -11,7 +11,7 @@
  * This can be the default command for the drivetrain, allowing for remote
  * operation with a controller
  */
-public abstract class SwerveDriveBaseControl extends Command {
+public abstract class BaseControl extends Command {
 	protected final SwerveDrivetrain drivetrain;
 	protected final CommandGenericHID controller;
 
@@ -21,7 +21,7 @@ public abstract class SwerveDriveBaseControl extends Command {
 	 * @param drivetrain       The drivetrain of the robot
 	 * @param driverController The device used to control drivetrain
 	 */
-	public SwerveDriveBaseControl(SwerveDrivetrain drivetrain, CommandGenericHID driverController) {
+	public BaseControl(SwerveDrivetrain drivetrain, CommandGenericHID driverController) {
 
 		// save parameters
 		this.controller = driverController;
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/DriveXboxControl.java
similarity index 94%
rename from src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
rename to src/main/java/frc/robot/commands/SwerveRemoteOperation/DriveXboxControl.java
index 54901f4..5326fde 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveXboxControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/DriveXboxControl.java
@@ -14,7 +14,7 @@
 /**
  * This is the default command for the drivetrain, allowing for remote operation with xbox controller
  */
-public class SwerveDriveXboxControl extends SwerveDriveBaseControl {
+public class DriveXboxControl extends BaseControl {
     private final OptionButton preciseModeButton;
     private final OptionButton boostModeButton;
     private final OptionButton fieldRelativeButton;
@@ -27,7 +27,7 @@ public class SwerveDriveXboxControl extends SwerveDriveBaseControl {
      * @param drivetrain           The drivetrain of the robot
      * @param driverXboxController The xbox controller used to control drivetrain
      */
-    public SwerveDriveXboxControl(SwerveDrivetrain drivetrain, CommandXboxController driverXboxController) {
+    public DriveXboxControl(SwerveDrivetrain drivetrain, CommandXboxController driverXboxController) {
         super(drivetrain, driverXboxController);
         
         // Create and configure buttons
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java
similarity index 89%
rename from src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
rename to src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java
index bd74cbb..f702abe 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java
@@ -2,7 +2,6 @@
 
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
 import frc.robot.Constants.DriverConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
@@ -12,7 +11,7 @@
 /**
  * This is the default command for the drivetrain, allowing for remote operation with joystick
  */
-public class SwerveDriveJoystickControl extends SwerveDriveBaseControl {
+public class JoystickControl extends BaseControl {
     private final OptionButton preciseModeButton;
     private final OptionButton boostModeButton;
     private final OptionButton fieldRelativeButton;
@@ -23,15 +22,13 @@ public class SwerveDriveJoystickControl extends SwerveDriveBaseControl {
 	 * @param drivetrain The drivetrain of the robot
 	 * @param driverJoystick The joystick used to control drivetrain
 	 */
-    public SwerveDriveJoystickControl(SwerveDrivetrain drivetrain, CommandJoystick driverJoystick) {
+    public JoystickControl(SwerveDrivetrain drivetrain, CommandJoystick driverJoystick) {
         super(drivetrain, driverJoystick);
 
         // Create and configure buttons
         preciseModeButton = new OptionButton(driverJoystick, 2, ActivationMode.TOGGLE);
         boostModeButton = new OptionButton(driverJoystick, 1, ActivationMode.HOLD);
         fieldRelativeButton = new OptionButton(driverJoystick, 3, ActivationMode.TOGGLE);
-
-        driverJoystick.button(5).onTrue(new InstantCommand(drivetrain::zeroGyroYaw));
     }
 
     @Override
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java
similarity index 92%
rename from src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java
rename to src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java
index 4b9d769..040f355 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDrivePS4Control.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java
@@ -11,7 +11,7 @@
 /**
  * This is the default command for the drivetrain, allowing for remote operation with PS4 controller
  */
-public class SwerveDrivePS4Control extends SwerveDriveBaseControl {
+public class PS4Control extends BaseControl {
     private final OptionButton preciseModeButton;
     private final OptionButton boostModeButton;
 
@@ -21,7 +21,7 @@ public class SwerveDrivePS4Control extends SwerveDriveBaseControl {
      * @param drivetrain The drive train of the robot
      * @param driverPS4Controller The PS4 controller used to control the drive train
      */
-    public SwerveDrivePS4Control(SwerveDrivetrain drivetrain, CommandPS4Controller driverPS4Controller){
+    public PS4Control(SwerveDrivetrain drivetrain, CommandPS4Controller driverPS4Controller){
         super(drivetrain, driverPS4Controller);
 
         // Create and configure buttons

From 90d7d3e4c24032aa71fc36cfa2742b23dfecd8d4 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Mon, 5 Feb 2024 17:33:36 -0800
Subject: [PATCH 30/99] Auto format changed files

Co-Authored-By: Aceius E. <144858100+AceiusRedshift@users.noreply.github.com>
---
 .../frc/robot/commands/DriveToPoseBase.java   | 190 ++---
 .../frc/robot/commands/DriveTransform.java    |  74 +-
 .../java/frc/robot/commands/FollowTag.java    | 100 +--
 .../DriveXboxControl.java                     | 111 +--
 .../JoystickControl.java                      |  74 +-
 .../SwerveRemoteOperation/PS4Control.java     |  90 +--
 .../robot/subsystems/SwerveDrivetrain.java    | 579 ++++++++-------
 .../frc/robot/subsystems/SwerveModule.java    | 662 +++++++++---------
 8 files changed, 984 insertions(+), 896 deletions(-)

diff --git a/src/main/java/frc/robot/commands/DriveToPoseBase.java b/src/main/java/frc/robot/commands/DriveToPoseBase.java
index 1726f65..c2b3314 100644
--- a/src/main/java/frc/robot/commands/DriveToPoseBase.java
+++ b/src/main/java/frc/robot/commands/DriveToPoseBase.java
@@ -11,97 +11,101 @@
 
 /** Base Command for automatically driving to a certain Pose on the field */
 public class DriveToPoseBase extends Command {
-    private final SwerveDrivetrain drivetrain;
-    private final PIDController xController, yController, rotationController;
-
-    /**
-     * Create a new DriveToPose command. Tries to drive to a set Pose based on odometry 
-     * 
-     * <p>This drives relative to the robot starting position, so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
-     * where forward is whatever direction the robot started in</p>
-     * 
-     * <p>The last place the drivetrain position was reset counts as the starting position</p>
-     * 
-     * @param drivetrain the drivetrain of the robot
-     */
-    public DriveToPoseBase(SwerveDrivetrain drivetrain) {
-
-        // Save drivetrain
-        this.drivetrain = drivetrain;
-
-        // Setup all PID controllers
-        xController = new PIDController(
-            RobotMovementConstants.TRANSLATION_PID_P,
-            RobotMovementConstants.TRANSLATION_PID_I,
-            RobotMovementConstants.TRANSLATION_PID_D
-        );
-        xController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
-
-        yController = new PIDController(
-            RobotMovementConstants.TRANSLATION_PID_P,
-            RobotMovementConstants.TRANSLATION_PID_I,
-            RobotMovementConstants.TRANSLATION_PID_D
-        );
-        yController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
-
-        rotationController = new PIDController(
-            RobotMovementConstants.ROTATION_PID_P,
-            RobotMovementConstants.ROTATION_PID_I,
-            RobotMovementConstants.ROTATION_PID_D
-        );
-        rotationController.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
-
-        // Add drivetrain as a requirement so no other commands try to use it
-        addRequirements(this.drivetrain);
-    }
-
-    public void setDesiredPosition(Pose2d targetPose) {
-        xController.setSetpoint(targetPose.getX());
-        yController.setSetpoint(targetPose.getY());
-        rotationController.setSetpoint(targetPose.getRotation().getRadians());
-    }
-
-    public Pose2d getPosition() {
-        return drivetrain.getPosition();
-    }
-
-    @Override
-    public void initialize() {
-        // Put all swerve modules in default position
-        drivetrain.toDefaultStates();
-    }
-
-    @Override
-    public void execute() {
-
-        // Get our current pose
-        final Pose2d measuredPosition = getPosition();
-
-        // Put current pose position on SmartDashboard
-        SmartDashboard.putNumber("PoseY", measuredPosition.getY());
-        SmartDashboard.putNumber("PoseX", measuredPosition.getX());
-		SmartDashboard.putNumber("PoseDegrees", measuredPosition.getRotation().getDegrees()); 
-
-        // Calculate our robot speeds with the PID controllers
-        final ChassisSpeeds speeds = new ChassisSpeeds(
-            xController.calculate(measuredPosition.getX()),
-            yController.calculate(measuredPosition.getY()),
-            rotationController.calculate(measuredPosition.getRotation().getRadians())
-        );
-
-        // Set those speeds
-        drivetrain.setDesiredState(speeds);
-    }
-
-    @Override
-    public boolean isFinished() {
-        // Finish once all controllers are within tolerance
-        return xController.atSetpoint() && yController.atSetpoint() && rotationController.atSetpoint();
-    }
-
-    @Override
-    public void end(boolean interrupted) {
-        // Stop all swerve modules at end
-        drivetrain.stop();
-    }
+	private final SwerveDrivetrain drivetrain;
+	private final PIDController xController, yController, rotationController;
+
+	/**
+	 * Create a new DriveToPose command. Tries to drive to a set Pose based on
+	 * odometry
+	 * 
+	 * <p>
+	 * This drives relative to the robot starting position, so a pose of +2x and +1y
+	 * will drive to the position 2 meters forward and 1 meter left of whether the
+	 * robot started,
+	 * where forward is whatever direction the robot started in
+	 * </p>
+	 * 
+	 * <p>
+	 * The last place the drivetrain position was reset counts as the starting
+	 * position
+	 * </p>
+	 * 
+	 * @param drivetrain the drivetrain of the robot
+	 */
+	public DriveToPoseBase(SwerveDrivetrain drivetrain) {
+
+		// Save drivetrain
+		this.drivetrain = drivetrain;
+
+		// Setup all PID controllers
+		xController = new PIDController(
+				RobotMovementConstants.TRANSLATION_PID_P,
+				RobotMovementConstants.TRANSLATION_PID_I,
+				RobotMovementConstants.TRANSLATION_PID_D);
+		xController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
+
+		yController = new PIDController(
+				RobotMovementConstants.TRANSLATION_PID_P,
+				RobotMovementConstants.TRANSLATION_PID_I,
+				RobotMovementConstants.TRANSLATION_PID_D);
+		yController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
+
+		rotationController = new PIDController(
+				RobotMovementConstants.ROTATION_PID_P,
+				RobotMovementConstants.ROTATION_PID_I,
+				RobotMovementConstants.ROTATION_PID_D);
+		rotationController.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
+
+		// Add drivetrain as a requirement so no other commands try to use it
+		addRequirements(this.drivetrain);
+	}
+
+	public void setDesiredPosition(Pose2d targetPose) {
+		xController.setSetpoint(targetPose.getX());
+		yController.setSetpoint(targetPose.getY());
+		rotationController.setSetpoint(targetPose.getRotation().getRadians());
+	}
+
+	public Pose2d getPosition() {
+		return drivetrain.getPosition();
+	}
+
+	@Override
+	public void initialize() {
+		// Put all swerve modules in default position
+		drivetrain.toDefaultStates();
+	}
+
+	@Override
+	public void execute() {
+
+		// Get our current pose
+		final Pose2d measuredPosition = getPosition();
+
+		// Put current pose position on SmartDashboard
+		SmartDashboard.putNumber("PoseY", measuredPosition.getY());
+		SmartDashboard.putNumber("PoseX", measuredPosition.getX());
+		SmartDashboard.putNumber("PoseDegrees", measuredPosition.getRotation().getDegrees());
+
+		// Calculate our robot speeds with the PID controllers
+		final ChassisSpeeds speeds = new ChassisSpeeds(
+				xController.calculate(measuredPosition.getX()),
+				yController.calculate(measuredPosition.getY()),
+				rotationController.calculate(measuredPosition.getRotation().getRadians()));
+
+		// Set those speeds
+		drivetrain.setDesiredState(speeds);
+	}
+
+	@Override
+	public boolean isFinished() {
+		// Finish once all controllers are within tolerance
+		return xController.atSetpoint() && yController.atSetpoint() && rotationController.atSetpoint();
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		// Stop all swerve modules at end
+		drivetrain.stop();
+	}
 }
diff --git a/src/main/java/frc/robot/commands/DriveTransform.java b/src/main/java/frc/robot/commands/DriveTransform.java
index c6d2913..8ca4c1c 100644
--- a/src/main/java/frc/robot/commands/DriveTransform.java
+++ b/src/main/java/frc/robot/commands/DriveTransform.java
@@ -1,6 +1,5 @@
 package frc.robot.commands;
 
-
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -8,38 +7,47 @@
 
 /** Command to automatically drive a certain transform */
 public class DriveTransform extends DriveToPoseBase {
-    private final Transform2d transform;
+	private final Transform2d transform;
+
+	/**
+	 * Create a new DriveToPose command. Tries to drive a certain transform using
+	 * the DriveToPose command.
+	 * 
+	 * <p>
+	 * This drives relative to the robot, so a transform of +2x and +1y will drive 2
+	 * meters forward and 1 meter left,
+	 * where forward is whatever direction the robot is currently facing
+	 * </p>
+	 * 
+	 * @param drivetrain the drivetrain of the robot
+	 * @param transform  target transform to drive to, will be added to current
+	 *                   position to get target pose
+	 */
+	public DriveTransform(SwerveDrivetrain drivetrain, Transform2d transform) {
+		super(drivetrain);
+		this.transform = transform;
+	}
 
-    /**
-     * Create a new DriveToPose command. Tries to drive a certain transform using the DriveToPose command.
-     * 
-     * <p>This drives relative to the robot, so a transform of +2x and +1y will drive 2 meters forward and 1 meter left,
-     * where forward is whatever direction the robot is currently facing</p>
-     * 
-     * @param drivetrain the drivetrain of the robot
-     * @param transform target transform to drive to, will be added to current position to get target pose
-     */
-    public DriveTransform(SwerveDrivetrain drivetrain, Transform2d transform) {
-        super(drivetrain);
-        this.transform = transform;
-    }
-    
-    /**
-     * Create a new DriveToPose command. Tries to drive a certain transform (a translation and a rotation) using the DriveToPose command
-     * 
-     * <p>This drives relative to the robot, so a transform of +2x and +1y will drive 2 meters forward and 1 meter left,
-     * where forward is whatever direction the robot is currently facing</p>
-     * 
-     * @param drivetrain The drivetrain of the robot
-     * @param translation Target transform to drive
-     * @param translation Target rotation to drive
-     */
-    public DriveTransform(SwerveDrivetrain drivetrain, Translation2d translation, Rotation2d rotation) {
-        this(drivetrain, new Transform2d(translation, rotation));
-    }    
+	/**
+	 * Create a new DriveToPose command. Tries to drive a certain transform (a
+	 * translation and a rotation) using the DriveToPose command
+	 * 
+	 * <p>
+	 * This drives relative to the robot, so a transform of +2x and +1y will drive 2
+	 * meters forward and 1 meter left,
+	 * where forward is whatever direction the robot is currently facing
+	 * </p>
+	 * 
+	 * @param drivetrain  The drivetrain of the robot
+	 * @param translation Target transform to drive
+	 * @param translation Target rotation to drive
+	 */
+	public DriveTransform(SwerveDrivetrain drivetrain, Translation2d translation, Rotation2d rotation) {
+		this(drivetrain, new Transform2d(translation, rotation));
+	}
 
-    @Override
-    public void initialize() {
-        setDesiredPosition(getPosition().plus(transform));
-    }
+	@Override
+	public void initialize() {
+		setDesiredPosition(getPosition().plus(transform));
+	}
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java
index 67c6ac7..958eee2 100644
--- a/src/main/java/frc/robot/commands/FollowTag.java
+++ b/src/main/java/frc/robot/commands/FollowTag.java
@@ -9,61 +9,65 @@
 
 /** Command to automatically drive a follow a tag a certain translation away */
 public class FollowTag extends DriveToPoseBase {
-    private final int tagID;
-    private final Transform2d targetDistance;
+	private final int tagID;
+	private final Transform2d targetDistance;
 
-    private final double loseTagAfterSeconds;
-    private double secondsSinceTagLastSeen;
+	private final double loseTagAfterSeconds;
+	private double secondsSinceTagLastSeen;
 
-    /**
-     * Create a new FollowTag command. Tries to follow a tag while staying a certain distance away.
-     * 
-     * @param drivetrain the drivetrain of the robot
-     * @param tagID the numerical ID of the the tag to follow
-     * @param targetDistanceToTag the target distance away from the tag to be
-     * @param loseTagAfterSeconds how long to wait before giving up on rediscover tag, set to -1 to never finish
-     */
-    public FollowTag(SwerveDrivetrain drivetrain, int tagID, Transform2d targetDistanceToTag, int loseTagAfterSeconds) {
-        super(drivetrain);
+	/**
+	 * Create a new FollowTag command. Tries to follow a tag while staying a certain
+	 * distance away.
+	 * 
+	 * @param drivetrain          the drivetrain of the robot
+	 * @param tagID               the numerical ID of the the tag to follow
+	 * @param targetDistanceToTag the target distance away from the tag to be
+	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover
+	 *                            tag, set to -1 to never finish
+	 */
+	public FollowTag(SwerveDrivetrain drivetrain, int tagID, Transform2d targetDistanceToTag, int loseTagAfterSeconds) {
+		super(drivetrain);
 
-        this.tagID = tagID;
-        this.targetDistance = targetDistanceToTag;
-        this.loseTagAfterSeconds = loseTagAfterSeconds;
-    }
-    
-    /**
-     * Create a new FollowTag command. Tries to follow a tag while staying a certain distance away.
-     * 
-     * @param drivetrain the drivetrain of the robot
-     * @param tagID the numerical ID of the the tag to follow
-     * @param targetDistanceToTag the target distance away from the tag to be
-     * @param loseTagAfterSeconds how long to wait before giving up on rediscover tag, set to -1 to never finish
-     */
-    public FollowTag(SwerveDrivetrain drivetrain, int tagID, Translation2d targetDistanceToTag, int loseTagAfterSeconds) {
-        this(drivetrain, tagID, new Transform2d(targetDistanceToTag, new Rotation2d()), loseTagAfterSeconds);
-    }
+		this.tagID = tagID;
+		this.targetDistance = targetDistanceToTag;
+		this.loseTagAfterSeconds = loseTagAfterSeconds;
+	}
 
-    @Override
-    public void execute() {
+	/**
+	 * Create a new FollowTag command. Tries to follow a tag while staying a certain
+	 * distance away.
+	 * 
+	 * @param drivetrain          the drivetrain of the robot
+	 * @param tagID               the numerical ID of the the tag to follow
+	 * @param targetDistanceToTag the target distance away from the tag to be
+	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover
+	 *                            tag, set to -1 to never finish
+	 */
+	public FollowTag(SwerveDrivetrain drivetrain, int tagID, Translation2d targetDistanceToTag,
+			int loseTagAfterSeconds) {
+		this(drivetrain, tagID, new Transform2d(targetDistanceToTag, new Rotation2d()), loseTagAfterSeconds);
+	}
 
-        // Sudo code, assume distance from front center of robot
-        final Transform3d tagPosition3d = new Transform3d(tagID * 5, 0, 0, new Rotation3d());
+	@Override
+	public void execute() {
 
-        // https://docs.photonvision.org/en/latest/docs/apriltag-pipelines/coordinate-systems.html
-        // https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/geometry/Rotation3d.html#getZ()
-        final Transform2d tagPosition = new Transform2d(
-            tagPosition3d.getZ(),
-            tagPosition3d.getX(),
-            Rotation2d.fromRadians(tagPosition3d.getRotation().getZ())
-        );
+		// Sudo code, assume distance from front center of robot
+		final Transform3d tagPosition3d = new Transform3d(tagID * 5, 0, 0, new Rotation3d());
 
-        final Transform2d driveTransform = tagPosition.plus(targetDistance.inverse());
+		// https://docs.photonvision.org/en/latest/docs/apriltag-pipelines/coordinate-systems.html
+		// https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/geometry/Rotation3d.html#getZ()
+		final Transform2d tagPosition = new Transform2d(
+				tagPosition3d.getZ(),
+				tagPosition3d.getX(),
+				Rotation2d.fromRadians(tagPosition3d.getRotation().getZ()));
 
-        setDesiredPosition(getPosition().plus(driveTransform));
-    }
+		final Transform2d driveTransform = tagPosition.plus(targetDistance.inverse());
 
-    @Override
-    public boolean isFinished() {
-        return (loseTagAfterSeconds != -1) && (secondsSinceTagLastSeen < loseTagAfterSeconds);
-    }
+		setDesiredPosition(getPosition().plus(driveTransform));
+	}
+
+	@Override
+	public boolean isFinished() {
+		return (loseTagAfterSeconds != -1) && (secondsSinceTagLastSeen < loseTagAfterSeconds);
+	}
 }
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/DriveXboxControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/DriveXboxControl.java
index 5326fde..f459cbf 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/DriveXboxControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/DriveXboxControl.java
@@ -12,60 +12,63 @@
 // https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/wpilibj2/command/button/CommandXboxController.html
 
 /**
- * This is the default command for the drivetrain, allowing for remote operation with xbox controller
+ * This is the default command for the drivetrain, allowing for remote operation
+ * with xbox controller
  */
 public class DriveXboxControl extends BaseControl {
-    private final OptionButton preciseModeButton;
-    private final OptionButton boostModeButton;
-    private final OptionButton fieldRelativeButton;
-
-    // private final OptionButton fieldRelativeButton;
-    
-    /**
-     * Creates a new SwerveDriveXboxControl Command.
-     *
-     * @param drivetrain           The drivetrain of the robot
-     * @param driverXboxController The xbox controller used to control drivetrain
-     */
-    public DriveXboxControl(SwerveDrivetrain drivetrain, CommandXboxController driverXboxController) {
-        super(drivetrain, driverXboxController);
-        
-        // Create and configure buttons
-        // OptionButton exampleToggleButton = new OptionButton(controller::a, ActivationMode.TOGGLE);
-        preciseModeButton = new OptionButton(driverXboxController::b, ActivationMode.TOGGLE);
-        boostModeButton = new OptionButton(driverXboxController::leftStick, ActivationMode.HOLD);
-        fieldRelativeButton = new OptionButton(driverXboxController::povUp, ActivationMode.TOGGLE);
-
-        // fieldRelativeButton = new OptionButton(driverXboxController::, ActivationMode.TOGGLE)
-
-        // Tell the command schedular we are using the drivetrain
-        addRequirements(drivetrain);
-    }
-
-    @Override
-    public void execute() {
-        final CommandXboxController xboxController = (CommandXboxController) controller;
-
-        final double leftX = applyJoystickDeadzone(xboxController.getLeftX(), DriverConstants.XBOX_DEAD_ZONE);
-        final double leftY = applyJoystickDeadzone(xboxController.getLeftY(), DriverConstants.XBOX_DEAD_ZONE);
-
-        final double rightX = -applyJoystickDeadzone(xboxController.getRightX(), DriverConstants.XBOX_DEAD_ZONE);
-
-        final boolean isFieldRelative = fieldRelativeButton.getState();
-
-        final int speedLevel = 1
-            - preciseModeButton.getStateAsInt()
-            + boostModeButton.getStateAsInt();
-
-        final ChassisSpeeds speeds = new ChassisSpeeds(
-            leftX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-            leftY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-            rightX * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
-
-        // Display relevant data on shuffleboard.
-        SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);        
-        SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
-
-        drivetrain.setDesiredState(speeds, isFieldRelative, true);
-    }
+	private final OptionButton preciseModeButton;
+	private final OptionButton boostModeButton;
+	private final OptionButton fieldRelativeButton;
+
+	// private final OptionButton fieldRelativeButton;
+
+	/**
+	 * Creates a new SwerveDriveXboxControl Command.
+	 *
+	 * @param drivetrain           The drivetrain of the robot
+	 * @param driverXboxController The xbox controller used to control drivetrain
+	 */
+	public DriveXboxControl(SwerveDrivetrain drivetrain, CommandXboxController driverXboxController) {
+		super(drivetrain, driverXboxController);
+
+		// Create and configure buttons
+		// OptionButton exampleToggleButton = new OptionButton(controller::a,
+		// ActivationMode.TOGGLE);
+		preciseModeButton = new OptionButton(driverXboxController::b, ActivationMode.TOGGLE);
+		boostModeButton = new OptionButton(driverXboxController::leftStick, ActivationMode.HOLD);
+		fieldRelativeButton = new OptionButton(driverXboxController::povUp, ActivationMode.TOGGLE);
+
+		// fieldRelativeButton = new OptionButton(driverXboxController::,
+		// ActivationMode.TOGGLE)
+
+		// Tell the command schedular we are using the drivetrain
+		addRequirements(drivetrain);
+	}
+
+	@Override
+	public void execute() {
+		final CommandXboxController xboxController = (CommandXboxController) controller;
+
+		final double leftX = applyJoystickDeadzone(xboxController.getLeftX(), DriverConstants.XBOX_DEAD_ZONE);
+		final double leftY = applyJoystickDeadzone(xboxController.getLeftY(), DriverConstants.XBOX_DEAD_ZONE);
+
+		final double rightX = -applyJoystickDeadzone(xboxController.getRightX(), DriverConstants.XBOX_DEAD_ZONE);
+
+		final boolean isFieldRelative = fieldRelativeButton.getState();
+
+		final int speedLevel = 1
+				- preciseModeButton.getStateAsInt()
+				+ boostModeButton.getStateAsInt();
+
+		final ChassisSpeeds speeds = new ChassisSpeeds(
+				leftX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				leftY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				rightX * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
+
+		// Display relevant data on shuffleboard.
+		SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);
+		SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
+
+		drivetrain.setDesiredState(speeds, isFieldRelative, true);
+	}
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java
index f702abe..f15a7c3 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java
@@ -9,57 +9,57 @@
 import frc.robot.utils.OptionButton.ActivationMode;
 
 /**
- * This is the default command for the drivetrain, allowing for remote operation with joystick
+ * This is the default command for the drivetrain, allowing for remote operation
+ * with joystick
  */
 public class JoystickControl extends BaseControl {
-    private final OptionButton preciseModeButton;
-    private final OptionButton boostModeButton;
-    private final OptionButton fieldRelativeButton;
+	private final OptionButton preciseModeButton;
+	private final OptionButton boostModeButton;
+	private final OptionButton fieldRelativeButton;
 
-    /**
+	/**
 	 * Creates a new SwerveDriveJoystickControl Command.
 	 *
-	 * @param drivetrain The drivetrain of the robot
+	 * @param drivetrain     The drivetrain of the robot
 	 * @param driverJoystick The joystick used to control drivetrain
 	 */
-    public JoystickControl(SwerveDrivetrain drivetrain, CommandJoystick driverJoystick) {
-        super(drivetrain, driverJoystick);
+	public JoystickControl(SwerveDrivetrain drivetrain, CommandJoystick driverJoystick) {
+		super(drivetrain, driverJoystick);
 
-        // Create and configure buttons
-        preciseModeButton = new OptionButton(driverJoystick, 2, ActivationMode.TOGGLE);
-        boostModeButton = new OptionButton(driverJoystick, 1, ActivationMode.HOLD);
-        fieldRelativeButton = new OptionButton(driverJoystick, 3, ActivationMode.TOGGLE);
-    }
+		// Create and configure buttons
+		preciseModeButton = new OptionButton(driverJoystick, 2, ActivationMode.TOGGLE);
+		boostModeButton = new OptionButton(driverJoystick, 1, ActivationMode.HOLD);
+		fieldRelativeButton = new OptionButton(driverJoystick, 3, ActivationMode.TOGGLE);
+	}
 
-    @Override
-    public void execute() {
-        super.execute();
+	@Override
+	public void execute() {
+		super.execute();
 
-        final CommandJoystick joystick = (CommandJoystick) controller;
+		final CommandJoystick joystick = (CommandJoystick) controller;
 
-        // Get joystick inputs
-        final double speedX = -applyJoystickDeadzone(joystick.getX(), DriverConstants.JOYSTICK_DEAD_ZONE);
+		// Get joystick inputs
+		final double speedX = -applyJoystickDeadzone(joystick.getX(), DriverConstants.JOYSTICK_DEAD_ZONE);
 		final double speedY = -applyJoystickDeadzone(joystick.getY(), DriverConstants.JOYSTICK_DEAD_ZONE);
-        final double speedR = -applyJoystickDeadzone(joystick.getTwist(), DriverConstants.JOYSTICK_DEAD_ZONE);
-        
-        // Level of speed from Precise, to Normal, to Boost
-        // Find our speed level, default is one (Normal)
-        final int speedLevel = 1
-            - preciseModeButton.getStateAsInt()
-            + boostModeButton.getStateAsInt();
+		final double speedR = -applyJoystickDeadzone(joystick.getTwist(), DriverConstants.JOYSTICK_DEAD_ZONE);
 
-        final boolean isFieldRelative = fieldRelativeButton.getState();
+		// Level of speed from Precise, to Normal, to Boost
+		// Find our speed level, default is one (Normal)
+		final int speedLevel = 1
+				- preciseModeButton.getStateAsInt()
+				+ boostModeButton.getStateAsInt();
 
-        final ChassisSpeeds speeds = new ChassisSpeeds(
-            speedY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-            speedX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-            speedR * DriverConstants.maxSpeedOptionsRotation[speedLevel]
-        );
+		final boolean isFieldRelative = fieldRelativeButton.getState();
 
-        // Display relevant data on shuffleboard.
-        SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);        
-        SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
+		final ChassisSpeeds speeds = new ChassisSpeeds(
+				speedY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				speedX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				speedR * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
 
-        drivetrain.setDesiredState(speeds, isFieldRelative, true);
-    }
+		// Display relevant data on shuffleboard.
+		SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);
+		SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
+
+		drivetrain.setDesiredState(speeds, isFieldRelative, true);
+	}
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java
index 040f355..c5dc71d 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java
@@ -9,51 +9,51 @@
 import frc.robot.utils.OptionButton.ActivationMode;
 
 /**
- * This is the default command for the drivetrain, allowing for remote operation with PS4 controller
+ * This is the default command for the drivetrain, allowing for remote operation
+ * with PS4 controller
  */
 public class PS4Control extends BaseControl {
-    private final OptionButton preciseModeButton;
-    private final OptionButton boostModeButton;
-
-    /**
-     * Creates a new SwerveDrivePS4Control Command
-     * 
-     * @param drivetrain The drive train of the robot
-     * @param driverPS4Controller The PS4 controller used to control the drive train
-     */
-    public PS4Control(SwerveDrivetrain drivetrain, CommandPS4Controller driverPS4Controller){
-        super(drivetrain, driverPS4Controller);
-
-        // Create and configure buttons
-        preciseModeButton = new OptionButton(driverPS4Controller::circle, ActivationMode.TOGGLE);
-        boostModeButton = new OptionButton(driverPS4Controller::triangle, ActivationMode.TOGGLE);
-
-        //Tell the command scheduler we are using the drivetrain.
-        addRequirements(drivetrain);
-    }
-
-    @Override
-    public void execute(){
-        final CommandPS4Controller ps4Controller = (CommandPS4Controller) controller;
-
-        final double leftX = ps4Controller.getLeftX();
-        final double leftY = ps4Controller.getLeftY();
-        final double rightX = ps4Controller.getRightX();
-
-        final int speedLevel = 1
-            - preciseModeButton.getStateAsInt()
-            + boostModeButton.getStateAsInt();
-
-        final ChassisSpeeds speeds = new ChassisSpeeds(
-            leftX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-            leftY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-            rightX * DriverConstants.maxSpeedOptionsRotation[speedLevel]
-        );
-
-        // Display relevant data on shuffleboard.
-        SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);        
-        SmartDashboard.putBoolean("Field Relieve", false);
-
-        drivetrain.setDesiredState(speeds, false, true);
-    }
+	private final OptionButton preciseModeButton;
+	private final OptionButton boostModeButton;
+
+	/**
+	 * Creates a new SwerveDrivePS4Control Command
+	 * 
+	 * @param drivetrain          The drive train of the robot
+	 * @param driverPS4Controller The PS4 controller used to control the drive train
+	 */
+	public PS4Control(SwerveDrivetrain drivetrain, CommandPS4Controller driverPS4Controller) {
+		super(drivetrain, driverPS4Controller);
+
+		// Create and configure buttons
+		preciseModeButton = new OptionButton(driverPS4Controller::circle, ActivationMode.TOGGLE);
+		boostModeButton = new OptionButton(driverPS4Controller::triangle, ActivationMode.TOGGLE);
+
+		// Tell the command scheduler we are using the drivetrain.
+		addRequirements(drivetrain);
+	}
+
+	@Override
+	public void execute() {
+		final CommandPS4Controller ps4Controller = (CommandPS4Controller) controller;
+
+		final double leftX = ps4Controller.getLeftX();
+		final double leftY = ps4Controller.getLeftY();
+		final double rightX = ps4Controller.getRightX();
+
+		final int speedLevel = 1
+				- preciseModeButton.getStateAsInt()
+				+ boostModeButton.getStateAsInt();
+
+		final ChassisSpeeds speeds = new ChassisSpeeds(
+				leftX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				leftY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				rightX * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
+
+		// Display relevant data on shuffleboard.
+		SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);
+		SmartDashboard.putBoolean("Field Relieve", false);
+
+		drivetrain.setDesiredState(speeds, false, true);
+	}
 }
diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index 79aab4a..15b5dd1 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -20,279 +20,316 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 /**
- * Subsystem for full drive train of robot. Contains 4 {@link SwerveModule} subsystems.
+ * Subsystem for full drive train of robot. Contains 4 {@link SwerveModule}
+ * subsystems.
  * 
  * @see <a href="https://youtu.be/X2UjzPi35gU">Swerve Drive Demo</a>
  */
 public class SwerveDrivetrain extends SubsystemBase {
-    /**
-     * The SwerveDriveKinematics class is a useful tool that converts between a ChassisSpeeds object
-     * and several SwerveModuleState objects, which contains velocities and angles for each swerve module of a swerve drive robot.
-     * @see https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html
-     */
-    private final SwerveDriveKinematics kinematics;
-
-    /**
-     * The SwerveDriveOdometry class can be used to track the position of a swerve drive robot on the field *
-     * @see https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-odometry.html
-     */
-    private final SwerveDriveOdometry odometry;
-
-    // Our 4 swerve Modules
-    private final SwerveModule moduleFL, moduleFR, moduleBL, moduleBR;
-    private final SwerveModule[] modules;
-
-    /**
-     * The Gyroscope on the robot. It gives data on Pitch, Yaw, and Roll of robot, as well as many other things
-     * @see https://www.kauailabs.com/public_files/navx-mxp/apidocs/java/com/kauailabs/navx/frc/AHRS.html
-     * @see https://ibb.co/dJrL259
-     */
-    private final AHRS gyro;
-
-    /**
-     * Pose of robot. The pose is the current the X, Y and Rotation position of the robot, relative to the last reset.
-     * @see https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/pose.html#pose
-     */
-    private Pose2d pose;
-
-    /**
-     * Constructor the drivetrain subsystem.
-     * 
-     * @param gyro Gyroscope on robot, should be physically located near center of robot
-     * @param swerveModuleFL Front left swerve module
-     * @param swerveModuleFR Front right swerve module
-     * @param swerveModuleBL Back left swerve module
-     * @param swerveModuleBR Back right swerve module
-     */
-    public SwerveDrivetrain(AHRS gyro, SwerveModule swerveModuleFL, SwerveModule swerveModuleFR, SwerveModule swerveModuleBL, SwerveModule swerveModuleBR) {
-
-        // save parameters
-        this.gyro = gyro;
-
-        moduleFL = swerveModuleFL;
-        moduleFR = swerveModuleFR;
-        moduleBL = swerveModuleBL;
-        moduleBR = swerveModuleBR;
-
-        modules = new SwerveModule[] {moduleFL, moduleFR, moduleBL, moduleBR};
-        
-        // create kinematics object using swerve module distance from center
-        kinematics = new SwerveDriveKinematics(
-            modulesMap(SwerveModule::getDistanceFromCenter, Translation2d[]::new)
-        );
-
-        // create starting state for odometry
-        odometry = new SwerveDriveOdometry(
-            kinematics,
-            getHeading(),
-            modulesMap(SwerveModule::getPosition, SwerveModulePosition[]::new)
-        );
-
-        // Set up name and children for sendable registry
-        setName(toString());
-        for (SwerveModule module : modules) {
-            addChild(module.getName(), module);
-        }
-    }
-
-    // --- Pose Related Methods ---
-
-    /**
-     * This is the periodic function of the swerve drivetrain.
-     * This method is called periodically by the CommandScheduler, about every 20ms.
-     */
-    @Override
-    public void periodic() {
-        // use odometry to update the estimated pose
-        pose = odometry.update(
-            getHeading(),
-            getWheelPositions()
-        );
-    }
-
-    /**
-     * Get the pose of robot, as calculated by odometry from gyro and distances swerve modules have traveled
-     * 
-     * @return The current positions of the robot, contains translational and rotational elements.
-     */
-    public Pose2d getPosition() {
-        return pose;
-    }
-
-    /**
-     * Resets the robot's pose on the field in software.
-     * Basically zeros out position.
-     */
-    public void resetPosition() {
-        odometry.resetPosition(
-            getHeading(),
-            getWheelPositions(),
-            pose
-        );
-    }
-
-
-    // --- Action Methods ---
-
-
-    /** Stop all swerve modules */
-    public void stop() {
-        modulesMap(SwerveModule::stop);
-    }
-
-    /** Put all swerve modules to default state, facing forward and staying still */
-    public void toDefaultStates() {
-        modulesMap(SwerveModule::toDefaultState);
-    }
-
-
-    // --- Chassis Speeds to Swerve Module States methods ---
-
-    /**
-     * Get field relative speeds of robot.
-     * 
-     * @return Speeds of drivetrain (from swerve modules)
-     */
-    public ChassisSpeeds getState() {
-        // get all module states and convert them into chassis speeds
-        return kinematics.toChassisSpeeds(
-            modulesMap(SwerveModule::getState, SwerveModuleState[]::new)
-        );
-    }
-
-    /**
-     * Get speeds of robot.
-     * 
-     * @param fieldRelative True if the robot is using a field relative coordinate system, false if using a robot relive coordinate system.
-     * @return Speeds of drivetrain (from swerve modules)
-     */
-    public ChassisSpeeds getState(boolean fieldRelative) {
-        ChassisSpeeds speeds = getState();
-        if (fieldRelative) speeds = ChassisSpeeds.fromRobotRelativeSpeeds(speeds, getHeading());
-        return speeds;
-    }
-
-    /**
-     * Set robot relative speeds of robot using default speeds units.
-     * 
-     * @param fieldRelative True if the robot is using a field relative coordinate system, false if using a robot relive coordinate syste
-     */
-    public void setDesiredState(ChassisSpeeds speeds) {
-        setDesiredState(speeds, false);
-    }
-
-    /**
-     * Set speeds of robot using default speeds units.
-     * 
-     * @param speeds Desired speeds of drivetrain (using swerve modules)
-     * @param fieldRelative True if the robot is using a field relative coordinate system, false if using a robot relive coordinate syste
-     */
-    public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative) {
-        setDesiredState(speeds, fieldRelative, false);
-    }
-
-    /**
-     * Set speeds of robot.
-     * 
-     * <p>
-     * Vx: the velocity of the robot in the x (forward) direction in meter per second.
-     * Vy: the velocity of the robot in the y (sideways) direction in meter per second. (Positive values mean the robot is moving to the left).
-     * Omega: the angular velocity of the robot in radians per second.
-     * 
-     * <p>
-     * If field relative, forward will be directly away from driver, no matter the rotation of the robot.
-     * If robot relative, forward will be whatever direction the robot is facing in.
-     * 
-     * <p>
-     * If power drive mode then speeds X, Y, and Omega are in motor powers from -1 to 1.
-     * If normal drive mode then X and Y are in meters per second and Omega is in radians per second
-     * 
-     * @param speeds Desired speeds of drivetrain (using swerve modules)
-     * @param fieldRelative True if the robot is using a field relative coordinate system, false if using a robot relive coordinate system.
-     * @param powerDriveMode True if in power drive mode with motor powers, false if in normal drive mode with default units
-     */
-    public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative, boolean powerDriveMode) {
-
-        if (fieldRelative) speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getHeading());
-
-        setDesiredState(speeds);
-
-        SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);
-        for (int i = 0; i < modules.length; i++) {
-            modules[i].setDesiredState(states[i], powerDriveMode);
-        }
-    }
-    
-    /**
-     * Get all the swerve module positions
-     * 
-     * @return a {@link SwerveDriveWheelPositions} object which contains an array of all swerve module positions
-     */
-    public SwerveDriveWheelPositions getWheelPositions() {
-        return new SwerveDriveWheelPositions(
-            modulesMap(SwerveModule::getPosition, SwerveModulePosition[]::new)
-        );
-    }
-
-
-    // --- Gyro methods ---
-
-
-    /**
-     * <p>Return the heading of the robot as a rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).</p>
-     * 
-     * <p>The angle is continuous, that is it will continue from 360 to 361 degrees. This allows algorithms that wouldn't want to see a discontinuity in the gyro output as it sweeps past from 360 to 0 on the second time around.</p>
-     * 
-     * <p>The angle is expected to increase as the gyro turns counterclockwise when looked at from the top.</p>
-     * It needs to follow the NWU axis convention.
-     * 
-     * @return the current heading of the robot as a {@link Rotation2d}.
-     * 
-     * @see https://ibb.co/dJrL259 NWU Axis Convention 
-     */
-    public Rotation2d getHeading() {
-        return gyro.getRotation2d();
-    }
-
-    /**
-     * <p>Return the heading of the robot in as a rotation in a 3D coordinate frame represented by a quaternion.</p>
-     * 
-     * @return the current heading of the robot as a {@link Rotation3d}.
-     */
-    public Rotation3d getHeading3d() {
-        return gyro.getRotation3d();
-    }
-
-    // --- Util ---
-
-    /**
-     * Utility method. Function to easily run a function on each swerve module
-     * 
-     * @param func function to run on each swerve module, takes one argument and returns nothing, operates via side effects.
-     */
-    private void modulesMap(Consumer<SwerveModule> func) {
-        Arrays.stream(modules).forEach(func);
-    }
-
-    /**
-     * Utility method. Function to easily run a function on each swerve module and collect results to array.
-     * Insures that we don't mix up order of swerve modules, as this could lead to hard to spot bugs.
-     * 
-     * @param <T> type that is returned by function and should be collected
-     * @param func function that gets some data off each swerve module
-     * @param arrayInitializer constructor function for array to collect results in, use T[]::new
-     * @return array of results from func.
-     */
-    private <T> T[] modulesMap(Function<? super SwerveModule, ? extends T> func, IntFunction<T[]> arrayInitializer) {
-        // Private method with template argument T. Returns array of type T.
-        // Takes in function that accepts a SwerveModule or something higher on the inheritance chain (For example: Object, SubsystemBase)
-        // and returns something of type T or something lower on the inheritance chain. (For example if T is Object: Integer)
-        // Also takes a T array initializer
-        return Arrays.stream(modules).map(func).toArray(arrayInitializer);
-    }  
-    
-    @Override
-    public String toString() {
-        return "SwerveDrivetrain";
-    }
+	/**
+	 * The SwerveDriveKinematics class is a useful tool that converts between a
+	 * ChassisSpeeds object
+	 * and several SwerveModuleState objects, which contains velocities and angles
+	 * for each swerve module of a swerve drive robot.
+	 * 
+	 * @see https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html
+	 */
+	private final SwerveDriveKinematics kinematics;
+
+	/**
+	 * The SwerveDriveOdometry class can be used to track the position of a swerve
+	 * drive robot on the field *
+	 * 
+	 * @see https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-odometry.html
+	 */
+	private final SwerveDriveOdometry odometry;
+
+	// Our 4 swerve Modules
+	private final SwerveModule moduleFL, moduleFR, moduleBL, moduleBR;
+	private final SwerveModule[] modules;
+
+	/**
+	 * The Gyroscope on the robot. It gives data on Pitch, Yaw, and Roll of robot,
+	 * as well as many other things
+	 * 
+	 * @see https://www.kauailabs.com/public_files/navx-mxp/apidocs/java/com/kauailabs/navx/frc/AHRS.html
+	 * @see https://ibb.co/dJrL259
+	 */
+	private final AHRS gyro;
+
+	/**
+	 * Pose of robot. The pose is the current the X, Y and Rotation position of the
+	 * robot, relative to the last reset.
+	 * 
+	 * @see https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/pose.html#pose
+	 */
+	private Pose2d pose;
+
+	/**
+	 * Constructor the drivetrain subsystem.
+	 * 
+	 * @param gyro           Gyroscope on robot, should be physically located near
+	 *                       center of robot
+	 * @param swerveModuleFL Front left swerve module
+	 * @param swerveModuleFR Front right swerve module
+	 * @param swerveModuleBL Back left swerve module
+	 * @param swerveModuleBR Back right swerve module
+	 */
+	public SwerveDrivetrain(AHRS gyro, SwerveModule swerveModuleFL, SwerveModule swerveModuleFR,
+			SwerveModule swerveModuleBL, SwerveModule swerveModuleBR) {
+
+		// save parameters
+		this.gyro = gyro;
+
+		moduleFL = swerveModuleFL;
+		moduleFR = swerveModuleFR;
+		moduleBL = swerveModuleBL;
+		moduleBR = swerveModuleBR;
+
+		modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR };
+
+		// create kinematics object using swerve module distance from center
+		kinematics = new SwerveDriveKinematics(
+				modulesMap(SwerveModule::getDistanceFromCenter, Translation2d[]::new));
+
+		// create starting state for odometry
+		odometry = new SwerveDriveOdometry(
+				kinematics,
+				getHeading(),
+				modulesMap(SwerveModule::getPosition, SwerveModulePosition[]::new));
+
+		// Set up name and children for sendable registry
+		setName(toString());
+		for (SwerveModule module : modules) {
+			addChild(module.getName(), module);
+		}
+	}
+
+	// --- Pose Related Methods ---
+
+	/**
+	 * This is the periodic function of the swerve drivetrain.
+	 * This method is called periodically by the CommandScheduler, about every 20ms.
+	 */
+	@Override
+	public void periodic() {
+		// use odometry to update the estimated pose
+		pose = odometry.update(
+				getHeading(),
+				getWheelPositions());
+	}
+
+	/**
+	 * Get the pose of robot, as calculated by odometry from gyro and distances
+	 * swerve modules have traveled
+	 * 
+	 * @return The current positions of the robot, contains translational and
+	 *         rotational elements.
+	 */
+	public Pose2d getPosition() {
+		return pose;
+	}
+
+	/**
+	 * Resets the robot's pose on the field in software.
+	 * Basically zeros out position.
+	 */
+	public void resetPosition() {
+		odometry.resetPosition(
+				getHeading(),
+				getWheelPositions(),
+				pose);
+	}
+
+	// --- Action Methods ---
+
+	/** Stop all swerve modules */
+	public void stop() {
+		modulesMap(SwerveModule::stop);
+	}
+
+	/** Put all swerve modules to default state, facing forward and staying still */
+	public void toDefaultStates() {
+		modulesMap(SwerveModule::toDefaultState);
+	}
+
+	// --- Chassis Speeds to Swerve Module States methods ---
+
+	/**
+	 * Get field relative speeds of robot.
+	 * 
+	 * @return Speeds of drivetrain (from swerve modules)
+	 */
+	public ChassisSpeeds getState() {
+		// get all module states and convert them into chassis speeds
+		return kinematics.toChassisSpeeds(
+				modulesMap(SwerveModule::getState, SwerveModuleState[]::new));
+	}
+
+	/**
+	 * Get speeds of robot.
+	 * 
+	 * @param fieldRelative True if the robot is using a field relative coordinate
+	 *                      system, false if using a robot relive coordinate system.
+	 * @return Speeds of drivetrain (from swerve modules)
+	 */
+	public ChassisSpeeds getState(boolean fieldRelative) {
+		ChassisSpeeds speeds = getState();
+		if (fieldRelative)
+			speeds = ChassisSpeeds.fromRobotRelativeSpeeds(speeds, getHeading());
+		return speeds;
+	}
+
+	/**
+	 * Set robot relative speeds of robot using default speeds units.
+	 * 
+	 * @param fieldRelative True if the robot is using a field relative coordinate
+	 *                      system, false if using a robot relive coordinate syste
+	 */
+	public void setDesiredState(ChassisSpeeds speeds) {
+		setDesiredState(speeds, false);
+	}
+
+	/**
+	 * Set speeds of robot using default speeds units.
+	 * 
+	 * @param speeds        Desired speeds of drivetrain (using swerve modules)
+	 * @param fieldRelative True if the robot is using a field relative coordinate
+	 *                      system, false if using a robot relive coordinate syste
+	 */
+	public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative) {
+		setDesiredState(speeds, fieldRelative, false);
+	}
+
+	/**
+	 * Set speeds of robot.
+	 * 
+	 * <p>
+	 * Vx: the velocity of the robot in the x (forward) direction in meter per
+	 * second.
+	 * Vy: the velocity of the robot in the y (sideways) direction in meter per
+	 * second. (Positive values mean the robot is moving to the left).
+	 * Omega: the angular velocity of the robot in radians per second.
+	 * 
+	 * <p>
+	 * If field relative, forward will be directly away from driver, no matter the
+	 * rotation of the robot.
+	 * If robot relative, forward will be whatever direction the robot is facing in.
+	 * 
+	 * <p>
+	 * If power drive mode then speeds X, Y, and Omega are in motor powers from -1
+	 * to 1.
+	 * If normal drive mode then X and Y are in meters per second and Omega is in
+	 * radians per second
+	 * 
+	 * @param speeds         Desired speeds of drivetrain (using swerve modules)
+	 * @param fieldRelative  True if the robot is using a field relative coordinate
+	 *                       system, false if using a robot relive coordinate
+	 *                       system.
+	 * @param powerDriveMode True if in power drive mode with motor powers, false if
+	 *                       in normal drive mode with default units
+	 */
+	public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative, boolean powerDriveMode) {
+
+		if (fieldRelative)
+			speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getHeading());
+
+		setDesiredState(speeds);
+
+		SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);
+		for (int i = 0; i < modules.length; i++) {
+			modules[i].setDesiredState(states[i], powerDriveMode);
+		}
+	}
+
+	/**
+	 * Get all the swerve module positions
+	 * 
+	 * @return a {@link SwerveDriveWheelPositions} object which contains an array of
+	 *         all swerve module positions
+	 */
+	public SwerveDriveWheelPositions getWheelPositions() {
+		return new SwerveDriveWheelPositions(
+				modulesMap(SwerveModule::getPosition, SwerveModulePosition[]::new));
+	}
+
+	// --- Gyro methods ---
+
+	/**
+	 * <p>
+	 * Return the heading of the robot as a rotation in a 2D coordinate frame
+	 * represented by a point on the unit circle (cosine and sine).
+	 * </p>
+	 * 
+	 * <p>
+	 * The angle is continuous, that is it will continue from 360 to 361 degrees.
+	 * This allows algorithms that wouldn't want to see a discontinuity in the gyro
+	 * output as it sweeps past from 360 to 0 on the second time around.
+	 * </p>
+	 * 
+	 * <p>
+	 * The angle is expected to increase as the gyro turns counterclockwise when
+	 * looked at from the top.
+	 * </p>
+	 * It needs to follow the NWU axis convention.
+	 * 
+	 * @return the current heading of the robot as a {@link Rotation2d}.
+	 * 
+	 * @see https://ibb.co/dJrL259 NWU Axis Convention
+	 */
+	public Rotation2d getHeading() {
+		return gyro.getRotation2d();
+	}
+
+	/**
+	 * <p>
+	 * Return the heading of the robot in as a rotation in a 3D coordinate frame
+	 * represented by a quaternion.
+	 * </p>
+	 * 
+	 * @return the current heading of the robot as a {@link Rotation3d}.
+	 */
+	public Rotation3d getHeading3d() {
+		return gyro.getRotation3d();
+	}
+
+	// --- Util ---
+
+	/**
+	 * Utility method. Function to easily run a function on each swerve module
+	 * 
+	 * @param func function to run on each swerve module, takes one argument and
+	 *             returns nothing, operates via side effects.
+	 */
+	private void modulesMap(Consumer<SwerveModule> func) {
+		Arrays.stream(modules).forEach(func);
+	}
+
+	/**
+	 * Utility method. Function to easily run a function on each swerve module and
+	 * collect results to array.
+	 * Insures that we don't mix up order of swerve modules, as this could lead to
+	 * hard to spot bugs.
+	 * 
+	 * @param <T>              type that is returned by function and should be
+	 *                         collected
+	 * @param func             function that gets some data off each swerve module
+	 * @param arrayInitializer constructor function for array to collect results in,
+	 *                         use T[]::new
+	 * @return array of results from func.
+	 */
+	private <T> T[] modulesMap(Function<? super SwerveModule, ? extends T> func, IntFunction<T[]> arrayInitializer) {
+		// Private method with template argument T. Returns array of type T.
+		// Takes in function that accepts a SwerveModule or something higher on the
+		// inheritance chain (For example: Object, SubsystemBase)
+		// and returns something of type T or something lower on the inheritance chain.
+		// (For example if T is Object: Integer)
+		// Also takes a T array initializer
+		return Arrays.stream(modules).map(func).toArray(arrayInitializer);
+	}
+
+	@Override
+	public String toString() {
+		return "SwerveDrivetrain";
+	}
 }
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index a859b90..a4655af 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -20,323 +20,355 @@
 import frc.robot.Constants.SwerveModuleConstants;
 
 /**
- * Subsystem for individual swerve module on robot. Each swerve module has one drive motor and one steering motor.
+ * Subsystem for individual swerve module on robot. Each swerve module has one
+ * drive motor and one steering motor.
  * 
- * @see <a href="https://www.swervedrivespecialties.com/products/mk4-swerve-module">Swerve Module Kit</a>
+ * @see <a href=
+ *      "https://www.swervedrivespecialties.com/products/mk4-swerve-module">Swerve
+ *      Module Kit</a>
  */
 public class SwerveModule extends SubsystemBase {
 
-    // the drive motor is the motor that spins the wheel making the robot move across the ground (aka wheel or velocity motor)
-    private final CANSparkMax driveMotor;
-    private final RelativeEncoder driveEncoder;
-    private final SparkPIDController drivePIDController;
-   
-    // the steering motor is the motor that changes the rotation of the wheel allowing the robot to drive in any direction (aka spin or angular motor)
-    // Also allows for spinning in place
-    private final CANSparkMax steeringMotor;
-    private final CANcoder steeringEncoder;
-    private final PIDController steeringPIDController;
-
-    /** Status Signal that gives steering encoders current position in rotations */
-    private final StatusSignal<Double> steeringPosition;
-
-    /** Default state, forward and still */
-    private final static SwerveModuleState defaultState = new SwerveModuleState();
-
-    /** Locations of the wheel relative to the physical center of the robot. */
-    private final Translation2d distanceFromCenter;
-
-    /** Whether swerve module is stopped */
-    private boolean stopped = false;
-
-    /**
-     * Constructor for an individual Swerve Module.
-     * Sets up both drive and angular motor for swerve module as well as systems to monitor and control them
-     * 
-     * @param velocityMotorDeviceID  device ID for drive motor
-     * @param steeringMotorDeviceId  device ID for steering motor
-     * @param angularEncoderDeviceID device ID for the angular motor's absolute encoder
-     * @param distanceFromCenter     distance from center of robot to center of swerve module
-     * @param steeringEncoderZero    the zero (forward) position for the angular motor's absolute encoder, in rotations
-     */
-    public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steeringAbsoluteEncoderId, double steeringEncoderZero, Translation2d distanceFromCenter) {
-        // --- Drive Motor ---
-        driveMotor = new CANSparkMax(driveMotorDeviceId, MotorType.kBrushless);
-
-        // You most restore factory defaults if you want to use velocity encoder.
-        // If you do not do this, everything will break and shake itself to death
-        driveMotor.restoreFactoryDefaults();
-
-        driveMotor.setIdleMode(IdleMode.kBrake);
-
-        // --- Drive Encoder ---
-        driveEncoder = driveMotor.getEncoder();
-
-        driveEncoder.setVelocityConversionFactor(SwerveModuleConstants.DRIVE_MOTOR_GEAR_RATIO);
-        driveEncoder.setPositionConversionFactor(SwerveModuleConstants.DRIVE_MOTOR_GEAR_RATIO);
-
-        // --- Drive PID ---
-        drivePIDController = driveMotor.getPIDController();
-        drivePIDController.setP(SwerveModuleConstants.DRIVE_PID_P);
-        drivePIDController.setI(SwerveModuleConstants.DRIVE_PID_I);
-        drivePIDController.setD(SwerveModuleConstants.DRIVE_PID_D);
-        drivePIDController.setFF(SwerveModuleConstants.DRIVE_PID_FF);
-        drivePIDController.setIAccum(SwerveModuleConstants.DRIVE_PID_MAX_I);
-        drivePIDController.setOutputRange(-1, 1);
-
-        // --- Steering Motor ---
-        steeringMotor = new CANSparkMax(steeringMotorDeviceId, MotorType.kBrushless);
-
-        steeringMotor.setIdleMode(IdleMode.kBrake);
-
-        // --- Steering Encoder ---
-        steeringEncoder = new CANcoder(steeringAbsoluteEncoderId);
-
-        // Use 0-1 for rotational
-        steeringEncoder.getConfigurator().apply(
-            new MagnetSensorConfigs()
-                .withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
-                .withMagnetOffset(steeringEncoderZero)
-        );
-        
-        // --- Steering PID ---
-        steeringPIDController = new PIDController(
-            SwerveModuleConstants.STEERING_PID_P,
-            SwerveModuleConstants.STEERING_PID_I,
-            SwerveModuleConstants.STEERING_PID_D
-        );
-        steeringPIDController.enableContinuousInput(0, 1);
-        
-        // --- Save other values ---
-        this.distanceFromCenter = distanceFromCenter;
-
-        steeringPosition = steeringEncoder.getAbsolutePosition();
-
-        setName(toString());
-    }
-
-    /**
-     * This is the periodic function of the swerve module.
-     * This method is called periodically by the CommandScheduler, about every 20ms.
-     * 
-     * We use it to constantly keep the 
-     */
-    @Override
-    public void periodic() {
-        if (!stopped) {
-            // Calculate how fast to spin the motor to get to the desired angle using our PID controller, then set the motor to spin at that speed
-            steeringMotor.set(steeringPIDController.calculate(getSteeringAngleRotations()));
-        }
-    }
-
-    // --- Direct control methods ---
-
-    /** Stop drive and steering motor of swerve module, module can be moved again by calling setDesiredState. */
-    public void stop() {
-        stopped = true;
-
-        // Manually stop both motors in swerve module
-        driveMotor.stopMotor();
-        steeringMotor.stopMotor();
-    }
-
-    /** 
-     * Set the desired state of the Swerve Module to the default/starting state.
-     * This should have the module facing forward and not spinning.
-     */
-    public void toDefaultState() {
-        setDesiredState(defaultState, false);
-    }
-
-    // --- Getters and setters for modules desired SwerveModuleState ---
-
-    /**
-     * Get the state of the swerve module.
-     * The state is the speed of our drive motor and angle of our steering motor.
-     * 
-     * @return Current state of swerve module, contains speed (in m/s) and angle as {@link Rotation2d}
-     */
-    public SwerveModuleState getState() {
-        return new SwerveModuleState(
-                getDriveSpeedMetersPerSecond(),
-                Rotation2d.fromRotations(getSteeringAngleRotations()));
-    }
-
-    /**
-     * Set the state of the swerve module. The state is the speed and angle of the swerve module.
-     * You can use {@code Rotation2d.fromDegrees()} to create angle.
-     * 
-     * @param state New state of swerve module, contains speed in meters per second and angle as {@link Rotation2d}
-     * @param powerDriveMode whether the SwerveModuleState is in meters per second (false) or motor power (true)
-     */
-    public void setDesiredState(SwerveModuleState state, boolean powerDriveMode) {
-
-        // If state is null, then stop robot and don't set any states
-        if (state == null) {
-            stop();
-            return;
-        }
-
-        // Optimize the reference state to avoid spinning further than 90 degrees
-        state = optimize(state, Rotation2d.fromRotations(getSteeringAngleRotations()));
-        
-        // --- Set steering motor ---
-        steeringPIDController.setSetpoint(state.angle.getRotations());
-        
-        // --- Set drive motor ---
-
-        if (state.speedMetersPerSecond == 0) {
-            // If our desired speed is 0, just use the builtin motor stop, no matter the mode.
-            
-            // Stops motor movement. Motor can be moved again by calling set without having to re-enable the motor.
-            driveMotor.stopMotor();
-        }
-        else if (powerDriveMode) {
-            // If we are in power drive mode just directly set power to our desired speed.
-
-            // This is a bit of an abuse of the SwerveModuleState object as we treat speeds as power values from -1 to 1.
-            // We do this because we don't want to have to deal with a PID controller when we are just driving, as a human driver does not care about they exact speed mapping.
-            driveMotor.set(state.speedMetersPerSecond);
-        }
-        else {
-            // If we are in normal drive mode use the motor controller to set our target velocity
-
-            // The CANSparkMaxes have a builtin PID controller on them we can use to set a target velocity.
-            // We first convert our speed from meters per second to rotations per minute, as that is the native unit of our devices
-            final double desiredDriveRotationsPerMinute = (state.speedMetersPerSecond * 60) / SwerveModuleConstants.WHEEL_CIRCUMFERENCE;
-            drivePIDController.setReference(desiredDriveRotationsPerMinute, ControlType.kVelocity);
-        }
-    }
-
-    // --- Public info getters ---
-
-    /**
-     * Get the position of the swerve module. The position is the distance traveled by the drive motor and angle of the drive motor.
-     * 
-     * @return Current position of swerve module, contains distance traveled by motor in meters and angle as {@link Rotation2d}
-     */
-    public SwerveModulePosition getPosition() {
-        return new SwerveModulePosition(
-                getDriveDistanceMeters(),
-                Rotation2d.fromRotations(getSteeringAngleRotations()));
-    }
-
-    /**
-     * Get locations of the wheel relative to the physical center of the robot.
-     * Useful for kinematics.
-     * 
-     * @return Translation2d representing distance from center of bot
-     */
-    public Translation2d getDistanceFromCenter() {
-        return distanceFromCenter;
-    }
-
-    // --- Private getters ---
-
-    /**
-     * Get the velocity of the drive motor.
-     * 
-     * @return Meters per second of the drive wheel
-     */
-    private double getDriveSpeedMetersPerSecond() {
-        return (SwerveModuleConstants.WHEEL_CIRCUMFERENCE * driveEncoder.getVelocity()) / 60;
-    }
-
-    /**
-     * Get the position of the drive motor.
-     * 
-     * @return Meters traveled by the drive wheel
-     */
-    private double getDriveDistanceMeters() {
-        return SwerveModuleConstants.WHEEL_CIRCUMFERENCE * driveEncoder.getPosition();
-    }
-
-    /**
-     * Get the angel of the steering motor.
-     * 
-     * @return Current position in rotations of the steering motor, accounting for offset
-     */
-    private double getSteeringAngleRotations() {
-        return steeringPosition.refresh().getValueAsDouble();
-    }
-
-    // --- Util ---
-
-    /**
-     * Optimize a swerve module state so that instead of suddenly rotating the wheel (with steering motor)
-     * to go a certain direction we can instead just turn a half as much and switch
-     * the speed of wheel to go in reverse.
-     * 
-     * @param desiredState The state you want the swerve module to be in
-     * @param currentAngle The current angle of the swerve module in degrees
-     * @return             An optimized version of desiredState
-     */
-    private static SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle) {
-        // find the target angle in the same 0-360 scope as the desired state
-        double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees());
-
-        // keep the same target speed
-        double targetSpeed = desiredState.speedMetersPerSecond;
-
-        // found how much we have to move to get to target angle
-        double delta = targetAngle - currentAngle.getDegrees();
-
-        // If we have to flip around more than 90 degrees than instead just reverse our direction
-        // and only turn enough so that we have the motor facing in the same direction, just the other way 
-        if (Math.abs(delta) > 90) {
-            targetSpeed = -targetSpeed;
-            targetAngle += delta > 0 ? -180 : 180;
-        }
-
-        return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle));
-    }
-
-    /**
-     * Utility method. Move an angle into the range of the reference. Finds the relative 0 and 360
-     * position for a scope reference and moves the new angle into that.
-     * Example: {@code placeInAppropriate0To360Scope(90, 370) = 10.0}
-     * {@code placeInAppropriate0To360Scope(720, 10) = 730.0}
-     * 
-     * @param scopeReference The reference to find which 0-360 scope we are in.
-     *                       For example {@code 10} is in {@code 0-360} scope while {@code 370} is in {@code 360-720} scope.
-     * @param newAngle       The angle we want to move into found scope.
-     *                       For example if the scope was {@code 0-360} and our angle was {@code 370} it would become {@code 10}
-     * @return               {@code newAngle} in the same scope as {@code scopeReference}
-     */
-    private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) {
-        double lowerBound;
-        double upperBound;
-        double lowerOffset = scopeReference % 360;
-        if (lowerOffset >= 0) {
-            lowerBound = scopeReference - lowerOffset;
-            upperBound = scopeReference + (360 - lowerOffset);
-        } else {
-            upperBound = scopeReference - lowerOffset;
-            lowerBound = scopeReference - (360 + lowerOffset);
-        }
-        while (newAngle < lowerBound) {
-            newAngle += 360;
-        }
-        while (newAngle > upperBound) {
-            newAngle -= 360;
-        }
-        if (newAngle - scopeReference > 180) {
-            newAngle -= 360;
-        } else if (newAngle - scopeReference < -180) {
-            newAngle += 360;
-        }
-        return newAngle;
-    }
-
-    @Override
-    public String toString() {
-        
-        final String[] yPositions = {"Back", "Middle", "Front"}; 
-        final String[] xPositions = {"Right", "Middle", "Left"};
-
-        final int ySignum = (int) Math.signum(distanceFromCenter.getY());
-        final int xSignum = (int) Math.signum(distanceFromCenter.getX());
-        
-        return xPositions[xSignum + 1] + yPositions[ySignum + 1] + "SwerveModule";
-    }
+	// the drive motor is the motor that spins the wheel making the robot move
+	// across the ground (aka wheel or velocity motor)
+	private final CANSparkMax driveMotor;
+	private final RelativeEncoder driveEncoder;
+	private final SparkPIDController drivePIDController;
+
+	// the steering motor is the motor that changes the rotation of the wheel
+	// allowing the robot to drive in any direction (aka spin or angular motor)
+	// Also allows for spinning in place
+	private final CANSparkMax steeringMotor;
+	private final CANcoder steeringEncoder;
+	private final PIDController steeringPIDController;
+
+	/** Status Signal that gives steering encoders current position in rotations */
+	private final StatusSignal<Double> steeringPosition;
+
+	/** Default state, forward and still */
+	private final static SwerveModuleState defaultState = new SwerveModuleState();
+
+	/** Locations of the wheel relative to the physical center of the robot. */
+	private final Translation2d distanceFromCenter;
+
+	/** Whether swerve module is stopped */
+	private boolean stopped = false;
+
+	/**
+	 * Constructor for an individual Swerve Module.
+	 * Sets up both drive and angular motor for swerve module as well as systems to
+	 * monitor and control them
+	 * 
+	 * @param velocityMotorDeviceID  device ID for drive motor
+	 * @param steeringMotorDeviceId  device ID for steering motor
+	 * @param angularEncoderDeviceID device ID for the angular motor's absolute
+	 *                               encoder
+	 * @param distanceFromCenter     distance from center of robot to center of
+	 *                               swerve module
+	 * @param steeringEncoderZero    the zero (forward) position for the angular
+	 *                               motor's absolute encoder, in rotations
+	 */
+	public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steeringAbsoluteEncoderId,
+			double steeringEncoderZero, Translation2d distanceFromCenter) {
+		// --- Drive Motor ---
+		driveMotor = new CANSparkMax(driveMotorDeviceId, MotorType.kBrushless);
+
+		// You most restore factory defaults if you want to use velocity encoder.
+		// If you do not do this, everything will break and shake itself to death
+		driveMotor.restoreFactoryDefaults();
+
+		driveMotor.setIdleMode(IdleMode.kBrake);
+
+		// --- Drive Encoder ---
+		driveEncoder = driveMotor.getEncoder();
+
+		driveEncoder.setVelocityConversionFactor(SwerveModuleConstants.DRIVE_MOTOR_GEAR_RATIO);
+		driveEncoder.setPositionConversionFactor(SwerveModuleConstants.DRIVE_MOTOR_GEAR_RATIO);
+
+		// --- Drive PID ---
+		drivePIDController = driveMotor.getPIDController();
+		drivePIDController.setP(SwerveModuleConstants.DRIVE_PID_P);
+		drivePIDController.setI(SwerveModuleConstants.DRIVE_PID_I);
+		drivePIDController.setD(SwerveModuleConstants.DRIVE_PID_D);
+		drivePIDController.setFF(SwerveModuleConstants.DRIVE_PID_FF);
+		drivePIDController.setIAccum(SwerveModuleConstants.DRIVE_PID_MAX_I);
+		drivePIDController.setOutputRange(-1, 1);
+
+		// --- Steering Motor ---
+		steeringMotor = new CANSparkMax(steeringMotorDeviceId, MotorType.kBrushless);
+
+		steeringMotor.setIdleMode(IdleMode.kBrake);
+
+		// --- Steering Encoder ---
+		steeringEncoder = new CANcoder(steeringAbsoluteEncoderId);
+
+		// Use 0-1 for rotational
+		steeringEncoder.getConfigurator().apply(
+				new MagnetSensorConfigs()
+						.withAbsoluteSensorRange(AbsoluteSensorRangeValue.Unsigned_0To1)
+						.withMagnetOffset(steeringEncoderZero));
+
+		// --- Steering PID ---
+		steeringPIDController = new PIDController(
+				SwerveModuleConstants.STEERING_PID_P,
+				SwerveModuleConstants.STEERING_PID_I,
+				SwerveModuleConstants.STEERING_PID_D);
+		steeringPIDController.enableContinuousInput(0, 1);
+
+		// --- Save other values ---
+		this.distanceFromCenter = distanceFromCenter;
+
+		steeringPosition = steeringEncoder.getAbsolutePosition();
+
+		setName(toString());
+	}
+
+	/**
+	 * This is the periodic function of the swerve module.
+	 * This method is called periodically by the CommandScheduler, about every 20ms.
+	 * 
+	 * We use it to constantly keep the
+	 */
+	@Override
+	public void periodic() {
+		if (!stopped) {
+			// Calculate how fast to spin the motor to get to the desired angle using our
+			// PID controller, then set the motor to spin at that speed
+			steeringMotor.set(steeringPIDController.calculate(getSteeringAngleRotations()));
+		}
+	}
+
+	// --- Direct control methods ---
+
+	/**
+	 * Stop drive and steering motor of swerve module, module can be moved again by
+	 * calling setDesiredState.
+	 */
+	public void stop() {
+		stopped = true;
+
+		// Manually stop both motors in swerve module
+		driveMotor.stopMotor();
+		steeringMotor.stopMotor();
+	}
+
+	/**
+	 * Set the desired state of the Swerve Module to the default/starting state.
+	 * This should have the module facing forward and not spinning.
+	 */
+	public void toDefaultState() {
+		setDesiredState(defaultState, false);
+	}
+
+	// --- Getters and setters for modules desired SwerveModuleState ---
+
+	/**
+	 * Get the state of the swerve module.
+	 * The state is the speed of our drive motor and angle of our steering motor.
+	 * 
+	 * @return Current state of swerve module, contains speed (in m/s) and angle as
+	 *         {@link Rotation2d}
+	 */
+	public SwerveModuleState getState() {
+		return new SwerveModuleState(
+				getDriveSpeedMetersPerSecond(),
+				Rotation2d.fromRotations(getSteeringAngleRotations()));
+	}
+
+	/**
+	 * Set the state of the swerve module. The state is the speed and angle of the
+	 * swerve module.
+	 * You can use {@code Rotation2d.fromDegrees()} to create angle.
+	 * 
+	 * @param state          New state of swerve module, contains speed in meters
+	 *                       per second and angle as {@link Rotation2d}
+	 * @param powerDriveMode whether the SwerveModuleState is in meters per second
+	 *                       (false) or motor power (true)
+	 */
+	public void setDesiredState(SwerveModuleState state, boolean powerDriveMode) {
+
+		// If state is null, then stop robot and don't set any states
+		if (state == null) {
+			stop();
+			return;
+		}
+
+		// Optimize the reference state to avoid spinning further than 90 degrees
+		state = optimize(state, Rotation2d.fromRotations(getSteeringAngleRotations()));
+
+		// --- Set steering motor ---
+		steeringPIDController.setSetpoint(state.angle.getRotations());
+
+		// --- Set drive motor ---
+
+		if (state.speedMetersPerSecond == 0) {
+			// If our desired speed is 0, just use the builtin motor stop, no matter the
+			// mode.
+
+			// Stops motor movement. Motor can be moved again by calling set without having
+			// to re-enable the motor.
+			driveMotor.stopMotor();
+		} else if (powerDriveMode) {
+			// If we are in power drive mode just directly set power to our desired speed.
+
+			// This is a bit of an abuse of the SwerveModuleState object as we treat speeds
+			// as power values from -1 to 1.
+			// We do this because we don't want to have to deal with a PID controller when
+			// we are just driving, as a human driver does not care about they exact speed
+			// mapping.
+			driveMotor.set(state.speedMetersPerSecond);
+		} else {
+			// If we are in normal drive mode use the motor controller to set our target
+			// velocity
+
+			// The CANSparkMaxes have a builtin PID controller on them we can use to set a
+			// target velocity.
+			// We first convert our speed from meters per second to rotations per minute, as
+			// that is the native unit of our devices
+			final double desiredDriveRotationsPerMinute = (state.speedMetersPerSecond * 60)
+					/ SwerveModuleConstants.WHEEL_CIRCUMFERENCE;
+			drivePIDController.setReference(desiredDriveRotationsPerMinute, ControlType.kVelocity);
+		}
+	}
+
+	// --- Public info getters ---
+
+	/**
+	 * Get the position of the swerve module. The position is the distance traveled
+	 * by the drive motor and angle of the drive motor.
+	 * 
+	 * @return Current position of swerve module, contains distance traveled by
+	 *         motor in meters and angle as {@link Rotation2d}
+	 */
+	public SwerveModulePosition getPosition() {
+		return new SwerveModulePosition(
+				getDriveDistanceMeters(),
+				Rotation2d.fromRotations(getSteeringAngleRotations()));
+	}
+
+	/**
+	 * Get locations of the wheel relative to the physical center of the robot.
+	 * Useful for kinematics.
+	 * 
+	 * @return Translation2d representing distance from center of bot
+	 */
+	public Translation2d getDistanceFromCenter() {
+		return distanceFromCenter;
+	}
+
+	// --- Private getters ---
+
+	/**
+	 * Get the velocity of the drive motor.
+	 * 
+	 * @return Meters per second of the drive wheel
+	 */
+	private double getDriveSpeedMetersPerSecond() {
+		return (SwerveModuleConstants.WHEEL_CIRCUMFERENCE * driveEncoder.getVelocity()) / 60;
+	}
+
+	/**
+	 * Get the position of the drive motor.
+	 * 
+	 * @return Meters traveled by the drive wheel
+	 */
+	private double getDriveDistanceMeters() {
+		return SwerveModuleConstants.WHEEL_CIRCUMFERENCE * driveEncoder.getPosition();
+	}
+
+	/**
+	 * Get the angel of the steering motor.
+	 * 
+	 * @return Current position in rotations of the steering motor, accounting for
+	 *         offset
+	 */
+	private double getSteeringAngleRotations() {
+		return steeringPosition.refresh().getValueAsDouble();
+	}
+
+	// --- Util ---
+
+	/**
+	 * Optimize a swerve module state so that instead of suddenly rotating the wheel
+	 * (with steering motor)
+	 * to go a certain direction we can instead just turn a half as much and switch
+	 * the speed of wheel to go in reverse.
+	 * 
+	 * @param desiredState The state you want the swerve module to be in
+	 * @param currentAngle The current angle of the swerve module in degrees
+	 * @return An optimized version of desiredState
+	 */
+	private static SwerveModuleState optimize(SwerveModuleState desiredState, Rotation2d currentAngle) {
+		// find the target angle in the same 0-360 scope as the desired state
+		double targetAngle = placeInAppropriate0To360Scope(currentAngle.getDegrees(), desiredState.angle.getDegrees());
+
+		// keep the same target speed
+		double targetSpeed = desiredState.speedMetersPerSecond;
+
+		// found how much we have to move to get to target angle
+		double delta = targetAngle - currentAngle.getDegrees();
+
+		// If we have to flip around more than 90 degrees than instead just reverse our
+		// direction
+		// and only turn enough so that we have the motor facing in the same direction,
+		// just the other way
+		if (Math.abs(delta) > 90) {
+			targetSpeed = -targetSpeed;
+			targetAngle += delta > 0 ? -180 : 180;
+		}
+
+		return new SwerveModuleState(targetSpeed, Rotation2d.fromDegrees(targetAngle));
+	}
+
+	/**
+	 * Utility method. Move an angle into the range of the reference. Finds the
+	 * relative 0 and 360
+	 * position for a scope reference and moves the new angle into that.
+	 * Example: {@code placeInAppropriate0To360Scope(90, 370) = 10.0}
+	 * {@code placeInAppropriate0To360Scope(720, 10) = 730.0}
+	 * 
+	 * @param scopeReference The reference to find which 0-360 scope we are in.
+	 *                       For example {@code 10} is in {@code 0-360} scope while
+	 *                       {@code 370} is in {@code 360-720} scope.
+	 * @param newAngle       The angle we want to move into found scope.
+	 *                       For example if the scope was {@code 0-360} and our
+	 *                       angle was {@code 370} it would become {@code 10}
+	 * @return {@code newAngle} in the same scope as {@code scopeReference}
+	 */
+	private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) {
+		double lowerBound;
+		double upperBound;
+		double lowerOffset = scopeReference % 360;
+		if (lowerOffset >= 0) {
+			lowerBound = scopeReference - lowerOffset;
+			upperBound = scopeReference + (360 - lowerOffset);
+		} else {
+			upperBound = scopeReference - lowerOffset;
+			lowerBound = scopeReference - (360 + lowerOffset);
+		}
+		while (newAngle < lowerBound) {
+			newAngle += 360;
+		}
+		while (newAngle > upperBound) {
+			newAngle -= 360;
+		}
+		if (newAngle - scopeReference > 180) {
+			newAngle -= 360;
+		} else if (newAngle - scopeReference < -180) {
+			newAngle += 360;
+		}
+		return newAngle;
+	}
+
+	@Override
+	public String toString() {
+
+		final String[] yPositions = { "Back", "Middle", "Front" };
+		final String[] xPositions = { "Right", "Middle", "Left" };
+
+		final int ySignum = (int) Math.signum(distanceFromCenter.getY());
+		final int xSignum = (int) Math.signum(distanceFromCenter.getX());
+
+		return xPositions[xSignum + 1] + yPositions[ySignum + 1] + "SwerveModule";
+	}
 }

From 2e3300e0422de2c168645fc196fec06956ce3c07 Mon Sep 17 00:00:00 2001
From: judeconnolly <judeconnolly@seattleacademy.org>
Date: Wed, 7 Feb 2024 15:36:38 -0800
Subject: [PATCH 31/99] Arm Subsystem Progression

---
 src/main/java/frc/robot/Constants.java        |  5 ++++
 src/main/java/frc/robot/RobotContainer.java   |  7 +++++
 .../commands/MoveArmJoystickControl.java      | 23 ++++++++++++++++
 .../SwerveDriveJoystickControl.java           |  4 +--
 src/main/java/frc/robot/subsystems/Arm.java   | 27 ++++++++++---------
 5 files changed, 52 insertions(+), 14 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/MoveArmJoystickControl.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 8b02bb4..1a3f6aa 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -100,6 +100,11 @@ public static class ArmConstants {
         public static final double ARM_AMP_SHOOTING_DEGREES = 0;
         public static final double ARM_SPEAKER_SHOOTING_DEGREES = 0;
         public static final double ARM_INTAKE_DEGREES = 0;
+
+        public static final int LEFT_MOTOR_ID = 0;
+        //public static final int LEFT_ENCODER_ID = 0;
+        public static final int RIGHT_MOTOR_ID = 0;
+        public static final int RIGHT_ENCODER_ID = 0;
     }
 }
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 04a8fd8..f4c7ce7 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,6 +4,8 @@
 import frc.robot.Constants.SwerveDrivetrainConstants;
 import frc.robot.Constants.SwerveModuleConstants;
 import frc.robot.commands.SwerveRemoteOperation.SwerveDriveJoystickControl;
+import frc.robot.Constants.ArmConstants;
+import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.SwerveModule;
 
@@ -47,6 +49,11 @@ public class RobotContainer {
 			SwerveModuleConstants.ANGULAR_MOTOR_ENCODER_OFFSET_BR,
             new Translation2d(-SwerveDrivetrainConstants.MODULE_LOCATION_X, -SwerveDrivetrainConstants.MODULE_LOCATION_Y));
 
+    private final Arm arm = new Arm(
+        ArmConstants.LEFT_MOTOR_ID,
+        ArmConstants.RIGHT_MOTOR_ID,
+        ArmConstants.RIGHT_ENCODER_ID);
+
     private final AHRS gyro = new AHRS(I2C.Port.kMXP);
 
     private final SwerveDrivetrain drivetrain = new SwerveDrivetrain(gyro, swerveModuleFL, swerveModuleFR, swerveModuleBL, swerveModuleBR);
diff --git a/src/main/java/frc/robot/commands/MoveArmJoystickControl.java b/src/main/java/frc/robot/commands/MoveArmJoystickControl.java
new file mode 100644
index 0000000..0da85dd
--- /dev/null
+++ b/src/main/java/frc/robot/commands/MoveArmJoystickControl.java
@@ -0,0 +1,23 @@
+package frc.robot.commands;
+
+import frc.robot.subsystems.Arm;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
+
+public class MoveArmJoystickControl extends Command {
+
+    public MoveArmJoystickControl(Arm arm, CommandJoystick joystick) {
+        
+
+    }
+
+    /**
+     * The main body of a command. Called repeatedly while the command is scheduled (Every 20 ms).
+     */
+    @Override
+    public void execute() {
+        double armSpeed = 0;
+
+        
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
index 9eaa933..7f72c88 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
+++ b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
@@ -49,12 +49,12 @@ public void initialize() {
         drivetrain.toDefaultStates();
     }
 
-    /**
+    
+/**
      * The main body of a command. Called repeatedly while the command is scheduled (Every 20 ms).
      */
     @Override
     public void execute() {
-
         // Get joystick inputs
         final double speedX = -applyJoystickDeadzone(joystick.getX(), DriverConstants.JOYSTICK_DEAD_ZONE);
 		final double speedY = -applyJoystickDeadzone(joystick.getY(), DriverConstants.JOYSTICK_DEAD_ZONE);
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 6184d82..0b6f7e2 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -2,14 +2,15 @@
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.ArmConstants;
 
 import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 import com.ctre.phoenix6.StatusSignal;
 import com.ctre.phoenix6.hardware.CANcoder;
+import com.revrobotics.CANSparkBase.IdleMode;
 
 // How to make Subsystem (ignore image instructions, code is out of date, just look at written general instructions): https://compendium.readthedocs.io/en/latest/tasks/subsystems/subsystems.html
 // Command based programming: https://docs.wpilib.org/en/stable/docs/software/commandbased/what-is-command-based.html
@@ -20,8 +21,10 @@
 public class Arm extends SubsystemBase {
 
     private final CANSparkMax leftArmMotor;
-    private final CANcoder leftArmEncoder;
-    //<p>leftArmEncoder may be useless, as the right arm equivelent is used for the position for both.</p>
+    //private final CANcoder leftArmEncoder;
+
+    //leftArmEncoder may be useless, as the right arm equivelent is used for the position for both.</p>
+
     private final CANSparkMax rightArmMotor;
     private final CANcoder rightArmEncoder;
 
@@ -33,11 +36,11 @@ public class Arm extends SubsystemBase {
 
     
 
-    /** Constructor. Creates a new ExampleSubsystem. */
-    public Arm(int leftMotorId, int leftEncoderId, int rightMotorId, int rightEncoderId) {
+    /** Constructor. Creates a new Arm Subsystem. */
+    public Arm(int leftMotorId, int rightMotorId, int rightEncoderId) {
 
         leftArmMotor = new CANSparkMax(leftMotorId,MotorType.kBrushless);
-        leftArmEncoder = new CANcoder(leftEncoderId);
+        //leftArmEncoder = new CANcoder(leftEncoderId);
 
         rightArmMotor = new CANSparkMax(rightMotorId,MotorType.kBrushless);
         rightArmEncoder = new CANcoder(rightEncoderId);
@@ -50,6 +53,9 @@ public Arm(int leftMotorId, int leftEncoderId, int rightMotorId, int rightEncode
 
         armPosition = rightArmEncoder.getAbsolutePosition();
 
+        leftArmMotor.setIdleMode(IdleMode.kBrake);
+        rightArmMotor.setIdleMode(IdleMode.kBrake);
+
     }
 
     public void setArmAngleRadians(double desiredDegree) {
@@ -73,18 +79,15 @@ public void setArmToIntakePosition() {
 
     /**
      * This method is called periodically by the CommandScheduler, about every 20ms.
-     * It should be used for updating subsystem-specific state that you don't want to offload to a Command.
-     * Try to avoid "doing to much" in this method (for example no driver control here).
      */
     @Override
     public void periodic() {
         // This method will be called once per scheduler run
 
         final double armSpeed = armRaisePIDController.calculate(armPosition.refresh().getValueAsDouble(),armRotation2d.getRotations());
-        if(armSpeed != 0){
-            leftArmMotor.set(armSpeed);
-            rightArmMotor.set(armSpeed);
-        }
+        leftArmMotor.set(armSpeed);
+        rightArmMotor.set(armSpeed);
+            
         
     }
 }

From 218b311b017aeca8f4583736b0ac5b0509fb9fad Mon Sep 17 00:00:00 2001
From: judeconnolly <judeconnolly@seattleacademy.org>
Date: Wed, 7 Feb 2024 17:55:12 -0800
Subject: [PATCH 32/99] ArmJoystickControl arm movment button controll :)

---
 .../robot/commands/ArmJoystickControl.java    | 44 +++++++++++++++++++
 .../commands/MoveArmJoystickControl.java      | 23 ----------
 src/main/java/frc/robot/subsystems/Arm.java   |  2 +-
 3 files changed, 45 insertions(+), 24 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/ArmJoystickControl.java
 delete mode 100644 src/main/java/frc/robot/commands/MoveArmJoystickControl.java

diff --git a/src/main/java/frc/robot/commands/ArmJoystickControl.java b/src/main/java/frc/robot/commands/ArmJoystickControl.java
new file mode 100644
index 0000000..8bccc2c
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ArmJoystickControl.java
@@ -0,0 +1,44 @@
+package frc.robot.commands;
+
+import frc.robot.subsystems.Arm;
+
+import frc.robot.utils.OptionButton;
+import frc.robot.utils.OptionButton.ActivationMode;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
+
+public class ArmJoystickControl extends Command {
+    private final Arm arm;
+    private final CommandJoystick joystick;
+
+    private final OptionButton raiseArmButton;
+    private final OptionButton lowerArmButton;
+
+    private double armPos;
+
+    public ArmJoystickControl(Arm arm, CommandJoystick joystick) {
+        this.arm = arm;
+        this.joystick = joystick;
+
+
+        lowerArmButton = new OptionButton(joystick, 101, ActivationMode.HOLD);
+        raiseArmButton = new OptionButton(joystick, 202, ActivationMode.HOLD);
+
+        addRequirements(arm);
+    }
+
+
+    /** 
+     * The main body of a command. Called repeatedly while the command is scheduled (Every 20 ms).
+     */
+    @Override
+    public void execute() {
+
+        armPos += raiseArmButton.getStateAsInt();
+        armPos -= lowerArmButton.getStateAsInt();
+        
+
+        arm.setArmAngleDegrees(armPos);
+    }
+}
diff --git a/src/main/java/frc/robot/commands/MoveArmJoystickControl.java b/src/main/java/frc/robot/commands/MoveArmJoystickControl.java
deleted file mode 100644
index 0da85dd..0000000
--- a/src/main/java/frc/robot/commands/MoveArmJoystickControl.java
+++ /dev/null
@@ -1,23 +0,0 @@
-package frc.robot.commands;
-
-import frc.robot.subsystems.Arm;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
-
-public class MoveArmJoystickControl extends Command {
-
-    public MoveArmJoystickControl(Arm arm, CommandJoystick joystick) {
-        
-
-    }
-
-    /**
-     * The main body of a command. Called repeatedly while the command is scheduled (Every 20 ms).
-     */
-    @Override
-    public void execute() {
-        double armSpeed = 0;
-
-        
-    }
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 0b6f7e2..2bece67 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -58,7 +58,7 @@ public Arm(int leftMotorId, int rightMotorId, int rightEncoderId) {
 
     }
 
-    public void setArmAngleRadians(double desiredDegree) {
+    public void setArmAngleDegrees(double desiredDegree) {
         //maximum should ALWAYS be a greater value then minimum
         if (desiredDegree < ArmConstants.MAXIMUM_ARM_DEGREES || ArmConstants.MINIMUM_ARM_DEGREES > desiredDegree) {
             armRotation2d = Rotation2d.fromDegrees(desiredDegree);

From 2455f4825b2c4764d3fb0ad51118ba0c7d20b80f Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Wed, 7 Feb 2024 21:20:38 -0800
Subject: [PATCH 33/99] Combine all controls into one unified using suppliers

---
 shuffleboard.json                             | 117 +++++++++++++++++-
 simgui.json                                   |   1 +
 src/main/java/frc/robot/Constants.java        |   7 +-
 src/main/java/frc/robot/RobotContainer.java   |  33 +++--
 .../BaseControl.java => DriverControl.java}   |  76 +++++++-----
 .../DriveXboxControl.java                     |  74 -----------
 .../JoystickControl.java                      |  65 ----------
 .../SwerveRemoteOperation/PS4Control.java     |  59 ---------
 .../robot/subsystems/SwerveDrivetrain.java    |   2 -
 .../frc/robot/utils/ChassisDriveInputs.java   |  59 +++++++++
 10 files changed, 247 insertions(+), 246 deletions(-)
 rename src/main/java/frc/robot/commands/{SwerveRemoteOperation/BaseControl.java => DriverControl.java} (57%)
 delete mode 100644 src/main/java/frc/robot/commands/SwerveRemoteOperation/DriveXboxControl.java
 delete mode 100644 src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java
 delete mode 100644 src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java
 create mode 100644 src/main/java/frc/robot/utils/ChassisDriveInputs.java

diff --git a/shuffleboard.json b/shuffleboard.json
index 86fb906..edc0351 100644
--- a/shuffleboard.json
+++ b/shuffleboard.json
@@ -103,17 +103,126 @@
               "Colors/Color when false": "#8B0000FF"
             }
           },
-          "9,2": {
+          "7,1": {
             "size": [
-              1,
+              2,
               1
             ],
             "content": {
               "_type": "Text View",
               "_source0": "network_table:///SmartDashboard/Bot Name",
               "_title": "Bot Name",
-              "_glyph": 148,
-              "_showGlyph": false
+              "_glyph": 474,
+              "_showGlyph": true
+            }
+          },
+          "1,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/PoseX",
+              "_title": "PoseX",
+              "_glyph": 114,
+              "_showGlyph": true
+            }
+          },
+          "0,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/PoseY",
+              "_title": "PoseY",
+              "_glyph": 115,
+              "_showGlyph": true
+            }
+          },
+          "2,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/PoseDegrees",
+              "_title": "PoseDegrees",
+              "_glyph": 419,
+              "_showGlyph": true
+            }
+          },
+          "7,0": {
+            "size": [
+              2,
+              1
+            ],
+            "content": {
+              "_type": "ComboBox Chooser",
+              "_source0": "network_table:///SmartDashboard/Auto Chooser",
+              "_title": "Auto Chooser",
+              "_glyph": 167,
+              "_showGlyph": true
+            }
+          },
+          "5,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Number Bar",
+              "_source0": "network_table:///SmartDashboard/SpeedX",
+              "_title": "SpeedX",
+              "_glyph": 114,
+              "_showGlyph": true,
+              "Range/Min": -1.0,
+              "Range/Max": 1.0,
+              "Range/Center": 0.0,
+              "Visuals/Num tick marks": 5,
+              "Visuals/Show text": true,
+              "Visuals/Orientation": "HORIZONTAL"
+            }
+          },
+          "4,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Number Bar",
+              "_source0": "network_table:///SmartDashboard/SpeedY",
+              "_title": "SpeedY",
+              "_glyph": 115,
+              "_showGlyph": true,
+              "Range/Min": -1.0,
+              "Range/Max": 1.0,
+              "Range/Center": 0.0,
+              "Visuals/Num tick marks": 5,
+              "Visuals/Show text": true,
+              "Visuals/Orientation": "HORIZONTAL"
+            }
+          },
+          "6,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Number Bar",
+              "_source0": "network_table:///SmartDashboard/Speed",
+              "_title": "Speed",
+              "_glyph": 419,
+              "_showGlyph": true,
+              "Range/Min": -1.0,
+              "Range/Max": 1.0,
+              "Range/Center": 0.0,
+              "Visuals/Num tick marks": 5,
+              "Visuals/Show text": true,
+              "Visuals/Orientation": "HORIZONTAL"
             }
           }
         }
diff --git a/simgui.json b/simgui.json
index f47076a..f1e1470 100644
--- a/simgui.json
+++ b/simgui.json
@@ -2,6 +2,7 @@
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Auto Chooser": "String Chooser",
       "/SmartDashboard/SendableChooser[0]": "String Chooser"
     }
   },
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 71c2aad..ea2f5de 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -31,7 +31,7 @@ public static enum Bot {
 	 * @author Aceius E.
 	 */
 	static {
-		serialNumber = RobotBase.isReal() ? RobotController.getSerialNumber() : "-default-";
+		serialNumber = RobotBase.isReal() ? RobotController.getSerialNumber() : "Simulation";
 
 		switch (serialNumber) {
 			case "03282B00": // Wood Bot Serial Number
@@ -42,7 +42,6 @@ public static enum Bot {
 				currentBot = Bot.PRACTICE_BOT;
 				break;
 
-			case "-default-": // Competition Bot Serial Number
 			default: // Also use competition bot as default
 				currentBot = Bot.COMPETITION_BOT;
 				break;
@@ -52,9 +51,7 @@ public static enum Bot {
 	public static class DriverConstants {
 		public static final int DRIVER_JOYSTICK_PORT = 0;
 
-		public static final double JOYSTICK_DEAD_ZONE = 0.25;
-		public static final double XBOX_DEAD_ZONE = 0.10;
-		public static final double PS4_DEAD_ZONE = 0.12;
+		public static final double DEAD_ZONE = 0.25;
 
 		// Names of options for displaying
 		public static final String[] maxSpeedOptionsNames = { "Precise", "Normal", "Boost" };
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 28b63a5..8de2bdf 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,11 +4,12 @@
 import frc.robot.Constants.SwerveDrivetrainConstants;
 import frc.robot.Constants.SwerveModuleConstants;
 import frc.robot.commands.Autos;
-import frc.robot.commands.SwerveRemoteOperation.BaseControl;
-import frc.robot.commands.SwerveRemoteOperation.JoystickControl;
-import frc.robot.commands.SwerveRemoteOperation.DriveXboxControl;
+import frc.robot.commands.DriverControl;
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.SwerveModule;
+import frc.robot.utils.ChassisDriveInputs;
+import frc.robot.utils.OptionButton;
+import frc.robot.utils.OptionButton.ActivationMode;
 
 import com.kauailabs.navx.frc.AHRS;
 
@@ -74,7 +75,7 @@ public class RobotContainer {
 	 */
 	public RobotContainer() {
 		autoChooser.setDefaultOption("Testing Auto", Autos.testingAuto(drivetrain));
-		SmartDashboard.putData(autoChooser);
+		SmartDashboard.putData("Auto Chooser", autoChooser);
 
 		configureBindings();
 
@@ -87,16 +88,32 @@ public void setUpDriveController() {
 		final HIDType genericHIDType = genericHID.getType();
 
 		SmartDashboard.putString("Drive Controller", genericHIDType.toString());
-		SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " " + Constants.serialNumber);
+		SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " - " + Constants.serialNumber);
 
 		drivetrain.removeDefaultCommand();
 
-		BaseControl control;
+		DriverControl control;
 
 		if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
-			control = new JoystickControl(drivetrain, new CommandJoystick(genericHID.getPort()));
+			final CommandJoystick joystick = new CommandJoystick(genericHID.getPort());
+			control = new DriverControl(drivetrain,
+				new ChassisDriveInputs(
+					joystick::getX, joystick::getY, joystick::getTwist,
+					-1, -1, Constants.DriverConstants.DEAD_ZONE),
+				new OptionButton(joystick, 2, ActivationMode.TOGGLE),
+				new OptionButton(joystick, 1, ActivationMode.HOLD),
+				new OptionButton(joystick, 3, ActivationMode.TOGGLE)
+			);
 		} else {
-			control = new DriveXboxControl(drivetrain, new CommandXboxController(genericHID.getPort()));
+			final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());
+			control = new DriverControl(drivetrain,
+				new ChassisDriveInputs(
+					xbox::getLeftX, xbox::getLeftY, xbox::getRightX,
+					+1, -1, Constants.DriverConstants.DEAD_ZONE),
+				new OptionButton(xbox::b, ActivationMode.TOGGLE),
+				new OptionButton(xbox::leftStick, ActivationMode.HOLD),
+				new OptionButton(xbox::povUp, ActivationMode.TOGGLE)
+			);
 		}
 
 		drivetrain.setDefaultCommand(control);
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/BaseControl.java b/src/main/java/frc/robot/commands/DriverControl.java
similarity index 57%
rename from src/main/java/frc/robot/commands/SwerveRemoteOperation/BaseControl.java
rename to src/main/java/frc/robot/commands/DriverControl.java
index 1a80d29..c0e2214 100644
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/BaseControl.java
+++ b/src/main/java/frc/robot/commands/DriverControl.java
@@ -1,19 +1,26 @@
-package frc.robot.commands.SwerveRemoteOperation;
+package frc.robot.commands;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
+import frc.robot.Constants.DriverConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
+import frc.robot.utils.ChassisDriveInputs;
+import frc.robot.utils.OptionButton;
 
 /**
  * This can be the default command for the drivetrain, allowing for remote
  * operation with a controller
  */
-public abstract class BaseControl extends Command {
+public class DriverControl extends Command {
 	protected final SwerveDrivetrain drivetrain;
-	protected final CommandGenericHID controller;
+
+	private final OptionButton preciseModeButton;
+	private final OptionButton boostModeButton;
+	private final OptionButton fieldRelativeButton;
+
+	private final ChassisDriveInputs chassisDriveInputs;
 
 	/**
 	 * Creates a new SwerveDriveBaseControl Command.
@@ -21,10 +28,15 @@ public abstract class BaseControl extends Command {
 	 * @param drivetrain       The drivetrain of the robot
 	 * @param driverController The device used to control drivetrain
 	 */
-	public BaseControl(SwerveDrivetrain drivetrain, CommandGenericHID driverController) {
+	public DriverControl(SwerveDrivetrain drivetrain, ChassisDriveInputs chassisDriveInputs,
+			OptionButton preciseModeButton, OptionButton boostModeButton, OptionButton fieldRelativeButton) {
+
+		this.chassisDriveInputs = chassisDriveInputs;
+
+		this.preciseModeButton = preciseModeButton;
+		this.boostModeButton = boostModeButton;
+		this.fieldRelativeButton = fieldRelativeButton;
 
-		// save parameters
-		this.controller = driverController;
 		this.drivetrain = drivetrain;
 
 		// Tell the command schedular we are using the drivetrain
@@ -50,15 +62,41 @@ public void initialize() {
 	 */
 	@Override
 	public void execute() {
+		final double speedX = chassisDriveInputs.getX();
+		final double speedY = chassisDriveInputs.getY();
+
+		final double speedRotation = chassisDriveInputs.getRotation();
+
+		final boolean isFieldRelative = fieldRelativeButton.getState();
+
+		final int speedLevel = 1
+				- preciseModeButton.getStateAsInt()
+				+ boostModeButton.getStateAsInt();
+
+		final ChassisSpeeds speeds = new ChassisSpeeds(
+				speedX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				speedY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				speedRotation * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
+
+
+		SmartDashboard.putNumber("SpeedX", speedX);
+		SmartDashboard.putNumber("SpeedY", speedY);
+		SmartDashboard.putNumber("Speed", speedRotation);
+
+		drivetrain.setDesiredState(speeds, isFieldRelative, true);
+
+		// Display relevant data on shuffleboard.
+		SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);
+		SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
 
 		// Position display
 		final Pose2d robotPosition = drivetrain.getPosition();
 
 		SmartDashboard.putNumber("PoseX", robotPosition.getX());
-		SmartDashboard.putNumber("PoseY", robotPosition.getX());
+		SmartDashboard.putNumber("PoseY", robotPosition.getY());
 		SmartDashboard.putNumber("PoseDegrees", robotPosition.getRotation().getDegrees());
 
-		// Speed degrees
+		// Speed and Heading
 		final ChassisSpeeds currentSpeeds = drivetrain.getState();
 		final double speedMetersPerSecond = Math
 				.sqrt(Math.pow(currentSpeeds.vxMetersPerSecond, 2) + Math.pow(currentSpeeds.vyMetersPerSecond, 2));
@@ -89,24 +127,4 @@ public void end(boolean interrupted) {
 
 		SmartDashboard.putBoolean("ControlActive", false);
 	}
-
-	// --- Util ---
-
-	/**
-	 * Utility method. Apply a deadzone to the joystick output to account for stick
-	 * drift and small bumps.
-	 * 
-	 * @param joystickValue Value in [-1, 1] from joystick axis
-	 * @return {@code 0} if {@code |joystickValue| <= deadzone}, else the
-	 *         {@code joystickValue} scaled to the new control area
-	 */
-	public static double applyJoystickDeadzone(double joystickValue, double deadzone) {
-		if (Math.abs(joystickValue) <= deadzone) {
-			// If the joystick |value| is in the deadzone than zero it out
-			return 0;
-		}
-
-		// scale value from the range [0, 1] to (deadzone, 1]
-		return joystickValue * (1 + deadzone) - Math.signum(joystickValue) * deadzone;
-	}
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/DriveXboxControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/DriveXboxControl.java
deleted file mode 100644
index f459cbf..0000000
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/DriveXboxControl.java
+++ /dev/null
@@ -1,74 +0,0 @@
-package frc.robot.commands.SwerveRemoteOperation;
-
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.Constants.DriverConstants;
-import frc.robot.subsystems.SwerveDrivetrain;
-import frc.robot.utils.OptionButton;
-import frc.robot.utils.OptionButton.ActivationMode;
-
-// Here is the documentation for the xbox controller code:
-// https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/wpilibj2/command/button/CommandXboxController.html
-
-/**
- * This is the default command for the drivetrain, allowing for remote operation
- * with xbox controller
- */
-public class DriveXboxControl extends BaseControl {
-	private final OptionButton preciseModeButton;
-	private final OptionButton boostModeButton;
-	private final OptionButton fieldRelativeButton;
-
-	// private final OptionButton fieldRelativeButton;
-
-	/**
-	 * Creates a new SwerveDriveXboxControl Command.
-	 *
-	 * @param drivetrain           The drivetrain of the robot
-	 * @param driverXboxController The xbox controller used to control drivetrain
-	 */
-	public DriveXboxControl(SwerveDrivetrain drivetrain, CommandXboxController driverXboxController) {
-		super(drivetrain, driverXboxController);
-
-		// Create and configure buttons
-		// OptionButton exampleToggleButton = new OptionButton(controller::a,
-		// ActivationMode.TOGGLE);
-		preciseModeButton = new OptionButton(driverXboxController::b, ActivationMode.TOGGLE);
-		boostModeButton = new OptionButton(driverXboxController::leftStick, ActivationMode.HOLD);
-		fieldRelativeButton = new OptionButton(driverXboxController::povUp, ActivationMode.TOGGLE);
-
-		// fieldRelativeButton = new OptionButton(driverXboxController::,
-		// ActivationMode.TOGGLE)
-
-		// Tell the command schedular we are using the drivetrain
-		addRequirements(drivetrain);
-	}
-
-	@Override
-	public void execute() {
-		final CommandXboxController xboxController = (CommandXboxController) controller;
-
-		final double leftX = applyJoystickDeadzone(xboxController.getLeftX(), DriverConstants.XBOX_DEAD_ZONE);
-		final double leftY = applyJoystickDeadzone(xboxController.getLeftY(), DriverConstants.XBOX_DEAD_ZONE);
-
-		final double rightX = -applyJoystickDeadzone(xboxController.getRightX(), DriverConstants.XBOX_DEAD_ZONE);
-
-		final boolean isFieldRelative = fieldRelativeButton.getState();
-
-		final int speedLevel = 1
-				- preciseModeButton.getStateAsInt()
-				+ boostModeButton.getStateAsInt();
-
-		final ChassisSpeeds speeds = new ChassisSpeeds(
-				leftX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-				leftY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-				rightX * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
-
-		// Display relevant data on shuffleboard.
-		SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);
-		SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
-
-		drivetrain.setDesiredState(speeds, isFieldRelative, true);
-	}
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java
deleted file mode 100644
index f15a7c3..0000000
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/JoystickControl.java
+++ /dev/null
@@ -1,65 +0,0 @@
-package frc.robot.commands.SwerveRemoteOperation;
-
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
-import frc.robot.Constants.DriverConstants;
-import frc.robot.subsystems.SwerveDrivetrain;
-import frc.robot.utils.OptionButton;
-import frc.robot.utils.OptionButton.ActivationMode;
-
-/**
- * This is the default command for the drivetrain, allowing for remote operation
- * with joystick
- */
-public class JoystickControl extends BaseControl {
-	private final OptionButton preciseModeButton;
-	private final OptionButton boostModeButton;
-	private final OptionButton fieldRelativeButton;
-
-	/**
-	 * Creates a new SwerveDriveJoystickControl Command.
-	 *
-	 * @param drivetrain     The drivetrain of the robot
-	 * @param driverJoystick The joystick used to control drivetrain
-	 */
-	public JoystickControl(SwerveDrivetrain drivetrain, CommandJoystick driverJoystick) {
-		super(drivetrain, driverJoystick);
-
-		// Create and configure buttons
-		preciseModeButton = new OptionButton(driverJoystick, 2, ActivationMode.TOGGLE);
-		boostModeButton = new OptionButton(driverJoystick, 1, ActivationMode.HOLD);
-		fieldRelativeButton = new OptionButton(driverJoystick, 3, ActivationMode.TOGGLE);
-	}
-
-	@Override
-	public void execute() {
-		super.execute();
-
-		final CommandJoystick joystick = (CommandJoystick) controller;
-
-		// Get joystick inputs
-		final double speedX = -applyJoystickDeadzone(joystick.getX(), DriverConstants.JOYSTICK_DEAD_ZONE);
-		final double speedY = -applyJoystickDeadzone(joystick.getY(), DriverConstants.JOYSTICK_DEAD_ZONE);
-		final double speedR = -applyJoystickDeadzone(joystick.getTwist(), DriverConstants.JOYSTICK_DEAD_ZONE);
-
-		// Level of speed from Precise, to Normal, to Boost
-		// Find our speed level, default is one (Normal)
-		final int speedLevel = 1
-				- preciseModeButton.getStateAsInt()
-				+ boostModeButton.getStateAsInt();
-
-		final boolean isFieldRelative = fieldRelativeButton.getState();
-
-		final ChassisSpeeds speeds = new ChassisSpeeds(
-				speedY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-				speedX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-				speedR * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
-
-		// Display relevant data on shuffleboard.
-		SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);
-		SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
-
-		drivetrain.setDesiredState(speeds, isFieldRelative, true);
-	}
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java
deleted file mode 100644
index c5dc71d..0000000
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/PS4Control.java
+++ /dev/null
@@ -1,59 +0,0 @@
-package frc.robot.commands.SwerveRemoteOperation;
-
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
-import frc.robot.Constants.DriverConstants;
-import frc.robot.subsystems.SwerveDrivetrain;
-import frc.robot.utils.OptionButton;
-import frc.robot.utils.OptionButton.ActivationMode;
-
-/**
- * This is the default command for the drivetrain, allowing for remote operation
- * with PS4 controller
- */
-public class PS4Control extends BaseControl {
-	private final OptionButton preciseModeButton;
-	private final OptionButton boostModeButton;
-
-	/**
-	 * Creates a new SwerveDrivePS4Control Command
-	 * 
-	 * @param drivetrain          The drive train of the robot
-	 * @param driverPS4Controller The PS4 controller used to control the drive train
-	 */
-	public PS4Control(SwerveDrivetrain drivetrain, CommandPS4Controller driverPS4Controller) {
-		super(drivetrain, driverPS4Controller);
-
-		// Create and configure buttons
-		preciseModeButton = new OptionButton(driverPS4Controller::circle, ActivationMode.TOGGLE);
-		boostModeButton = new OptionButton(driverPS4Controller::triangle, ActivationMode.TOGGLE);
-
-		// Tell the command scheduler we are using the drivetrain.
-		addRequirements(drivetrain);
-	}
-
-	@Override
-	public void execute() {
-		final CommandPS4Controller ps4Controller = (CommandPS4Controller) controller;
-
-		final double leftX = ps4Controller.getLeftX();
-		final double leftY = ps4Controller.getLeftY();
-		final double rightX = ps4Controller.getRightX();
-
-		final int speedLevel = 1
-				- preciseModeButton.getStateAsInt()
-				+ boostModeButton.getStateAsInt();
-
-		final ChassisSpeeds speeds = new ChassisSpeeds(
-				leftX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-				leftY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-				rightX * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
-
-		// Display relevant data on shuffleboard.
-		SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);
-		SmartDashboard.putBoolean("Field Relieve", false);
-
-		drivetrain.setDesiredState(speeds, false, true);
-	}
-}
diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index 15b5dd1..2cfd8fa 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -234,8 +234,6 @@ public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative, boolean
 		if (fieldRelative)
 			speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getHeading());
 
-		setDesiredState(speeds);
-
 		SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);
 		for (int i = 0; i < modules.length; i++) {
 			modules[i].setDesiredState(states[i], powerDriveMode);
diff --git a/src/main/java/frc/robot/utils/ChassisDriveInputs.java b/src/main/java/frc/robot/utils/ChassisDriveInputs.java
new file mode 100644
index 0000000..abf3736
--- /dev/null
+++ b/src/main/java/frc/robot/utils/ChassisDriveInputs.java
@@ -0,0 +1,59 @@
+package frc.robot.utils;
+
+import java.util.function.Supplier;
+
+public class ChassisDriveInputs {
+
+	private final Supplier<Double> xSupplier;
+	private final Supplier<Double> ySupplier;
+	private final Supplier<Double> rotationSupplier;
+
+	private final double deadzone;
+
+	private final double translationCoefficient;
+	private final double rotationCoefficient;
+
+	public ChassisDriveInputs(
+			Supplier<Double> getX, Supplier<Double> getY, Supplier<Double> getRotation,
+			double translationCoefficient, double rotationCoefficient, double deadzone) {
+
+		this.xSupplier = getX;
+		this.ySupplier = getY;
+		this.rotationSupplier = getRotation;
+
+		this.deadzone = deadzone;
+
+		this.translationCoefficient = translationCoefficient;
+		this.rotationCoefficient = rotationCoefficient;
+	}
+
+	public double getX() {
+		return applyJoystickDeadzone(xSupplier.get(), deadzone) * translationCoefficient;
+	}
+
+	public double getY() {
+		return applyJoystickDeadzone(ySupplier.get(), deadzone) * translationCoefficient;
+	}
+
+	public double getRotation() {
+		return applyJoystickDeadzone(rotationSupplier.get(), deadzone) * rotationCoefficient;
+	}
+
+	/**
+	 * Utility method. Apply a deadzone to the joystick output to account for stick
+	 * drift and small bumps.
+	 * 
+	 * @param joystickValue Value in [-1, 1] from joystick axis
+	 * @return {@code 0} if {@code |joystickValue| <= deadzone}, else the
+	 *         {@code joystickValue} scaled to the new control area
+	 */
+	private static double applyJoystickDeadzone(double joystickValue, double deadzone) {
+		if (Math.abs(joystickValue) <= deadzone) {
+			// If the joystick |value| is in the deadzone than zero it out
+			return 0;
+		}
+
+		// scale value from the range [0, 1] to (deadzone, 1]
+		return joystickValue * (1 + deadzone) - Math.signum(joystickValue) * deadzone;
+	}
+}

From d301203e0f6a2c3a5541f0eec224d6a85fde4604 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Thu, 8 Feb 2024 08:20:11 -0800
Subject: [PATCH 34/99] remember to call super

---
 src/main/java/frc/robot/commands/FollowTag.java | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java
index 958eee2..0fd9a82 100644
--- a/src/main/java/frc/robot/commands/FollowTag.java
+++ b/src/main/java/frc/robot/commands/FollowTag.java
@@ -64,6 +64,8 @@ public void execute() {
 		final Transform2d driveTransform = tagPosition.plus(targetDistance.inverse());
 
 		setDesiredPosition(getPosition().plus(driveTransform));
+
+		super.execute();
 	}
 
 	@Override

From cd84514db2ab6c00fe63893ec5e927f9ae4fe9e3 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Thu, 8 Feb 2024 08:20:27 -0800
Subject: [PATCH 35/99] add x mode breaking

---
 .../java/frc/robot/subsystems/SwerveDrivetrain.java    |  5 +++++
 src/main/java/frc/robot/subsystems/SwerveModule.java   | 10 ++++++++++
 2 files changed, 15 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index 2cfd8fa..6b48f69 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -153,6 +153,11 @@ public void toDefaultStates() {
 		modulesMap(SwerveModule::toDefaultState);
 	}
 
+	/** Put all swerve modules to inward state, making the swerve modules face in a X pattern. This prevents robot from slipping around. */
+	public void brakeMode() {
+		modulesMap(SwerveModule::toInwardPosition);
+	}
+
 	// --- Chassis Speeds to Swerve Module States methods ---
 
 	/**
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index a4655af..1715d6b 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -160,6 +160,16 @@ public void toDefaultState() {
 		setDesiredState(defaultState, false);
 	}
 
+	/**
+	 * 
+	 * 
+	 */
+	public void toInwardPosition() {
+		setDesiredState(new SwerveModuleState(
+			0, distanceFromCenter.getAngle()
+		), false);
+	}
+
 	// --- Getters and setters for modules desired SwerveModuleState ---
 
 	/**

From ef04238b812369212ff3e8a26f600db1f3bf7443 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Thu, 8 Feb 2024 16:00:51 -0800
Subject: [PATCH 36/99] clean up some comments

---
 src/main/java/frc/robot/Constants.java          |  6 ++----
 src/main/java/frc/robot/Main.java               |  6 ++----
 src/main/java/frc/robot/Robot.java              | 10 +++-------
 src/main/java/frc/robot/RobotContainer.java     | 17 ++++++++++++-----
 src/main/java/frc/robot/commands/Autos.java     |  4 +---
 .../frc/robot/utils/ChassisDriveInputs.java     | 11 +++++++++++
 6 files changed, 31 insertions(+), 23 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index ea2f5de..bd1cf85 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -20,13 +20,11 @@ public static enum Bot {
 	public static final String serialNumber;
 
 	/**
-	 * This code determines what bot is being deployed and sets constants
-	 * accordingly.
+	 * This code determines what bot is being deployed and sets constants accordingly.
 	 * 
 	 * Simulated bots cannot have a RoboRIO ID, so we must check if the bot is real.
 	 * If it isn't, load production config.
-	 * The production bot is always default, so if we do anything crazy to our bot
-	 * during the tourney like switch the RoboRIO the code works.
+	 * The production bot is always default, so if we do anything crazy to our bot during the tourney like switch the RoboRIO the code works.
 	 * 
 	 * @author Aceius E.
 	 */
diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java
index e5d3e96..04a9a16 100644
--- a/src/main/java/frc/robot/Main.java
+++ b/src/main/java/frc/robot/Main.java
@@ -4,10 +4,8 @@
 
 /**
  * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what
- * you are doing (which you do not), do not modify this file except to change
- * the parameter class to the startRobot
- * call.
+ * Unless you know what you are doing (which you do not), do not modify this file
+ * except to change the parameter class to the startRobot call.
  */
 public final class Main {
 	private Main() {
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index e19b167..c98c1d3 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -5,13 +5,9 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 /**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to
- * each mode, as described in the TimedRobot documentation. If you change the
- * name of this class or
- * the package after creating this project, you must also update the
- * build.gradle file in the
- * project.
+ * The VM is configured to automatically run this class,
+ * and to call the functions corresponding to each mode, as described in the TimedRobot documentation.
+ * If you change the name of this class or the package after creating this project, you must also update the build.gradle file in the project.
  */
 public class Robot extends TimedRobot {
 	private Command autonomousCommand;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 8de2bdf..04af3b4 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -20,16 +20,14 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
 /**
  * This class is where the bulk of the robot should be declared.
- * Since Command-based is a "declarative" paradigm, very little robot logic
- * should actually be handled in the {@link Robot} periodic methods (other than
- * the scheduler calls).
- * Instead, the structure of the robot (including subsystems, commands, and
- * trigger mappings) should be declared here.
+ * Since Command-based is a "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} periodic methods (other than the scheduler calls).
+ * Instead, the structure of the robot (including subsystems, commands, and trigger mappings) should be declared here.
  */
 public class RobotContainer {
 
@@ -97,23 +95,32 @@ public void setUpDriveController() {
 		if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
 			final CommandJoystick joystick = new CommandJoystick(genericHID.getPort());
 			control = new DriverControl(drivetrain,
+
 				new ChassisDriveInputs(
 					joystick::getX, joystick::getY, joystick::getTwist,
 					-1, -1, Constants.DriverConstants.DEAD_ZONE),
+
 				new OptionButton(joystick, 2, ActivationMode.TOGGLE),
 				new OptionButton(joystick, 1, ActivationMode.HOLD),
 				new OptionButton(joystick, 3, ActivationMode.TOGGLE)
 			);
+			
+			joystick.button(4).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
+
 		} else {
 			final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());
 			control = new DriverControl(drivetrain,
+
 				new ChassisDriveInputs(
 					xbox::getLeftX, xbox::getLeftY, xbox::getRightX,
 					+1, -1, Constants.DriverConstants.DEAD_ZONE),
+
 				new OptionButton(xbox::b, ActivationMode.TOGGLE),
 				new OptionButton(xbox::leftStick, ActivationMode.HOLD),
 				new OptionButton(xbox::povUp, ActivationMode.TOGGLE)
 			);
+
+
 		}
 
 		drivetrain.setDefaultCommand(control);
diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java
index 0e647ff..e0de80f 100644
--- a/src/main/java/frc/robot/commands/Autos.java
+++ b/src/main/java/frc/robot/commands/Autos.java
@@ -13,9 +13,7 @@ public final class Autos {
 	/** Example static factory for an autonomous command. */
 	public static Command testingAuto(SwerveDrivetrain drivetrain) {
 		return Commands.sequence(
-				new AutoDriveTo(drivetrain, new Translation2d(1, 0))
-		// new WaitCommand(1),
-		// new AutoDriveTo(drivetrain, new Translation2d(-1, 0))
+				new DriveTransform(drivetrain, new Translation2d(1, 0), new Rotation2d())
 		);
 	}
 
diff --git a/src/main/java/frc/robot/utils/ChassisDriveInputs.java b/src/main/java/frc/robot/utils/ChassisDriveInputs.java
index abf3736..04dfeff 100644
--- a/src/main/java/frc/robot/utils/ChassisDriveInputs.java
+++ b/src/main/java/frc/robot/utils/ChassisDriveInputs.java
@@ -2,6 +2,7 @@
 
 import java.util.function.Supplier;
 
+/** Class that stores supplies for main controls of ChassisSpeeds */
 public class ChassisDriveInputs {
 
 	private final Supplier<Double> xSupplier;
@@ -13,6 +14,16 @@ public class ChassisDriveInputs {
 	private final double translationCoefficient;
 	private final double rotationCoefficient;
 
+	/**
+	 * Create a new ChassisDriveInputs
+	 * 
+	 * @param getX Get the value mapped to forward
+	 * @param getY Get the value mapped to left
+	 * @param getRotation Get the value mapped to rotation
+	 * @param translationCoefficient Coefficient that forward (x) and left (Y) are multiped by
+	 * @param rotationCoefficient Coefficient that rotation is multiplied by
+	 * @param deadzone Deadzone for all axises
+	 */
 	public ChassisDriveInputs(
 			Supplier<Double> getX, Supplier<Double> getY, Supplier<Double> getRotation,
 			double translationCoefficient, double rotationCoefficient, double deadzone) {

From d9c10683004b00f1cc74c95c4b2ccdc30e6b11c6 Mon Sep 17 00:00:00 2001
From: Aceius E <aceiuselms@seattleacademy.org>
Date: Thu, 8 Feb 2024 17:35:46 -0800
Subject: [PATCH 37/99] New shuffleboard Layout

Co-Authored-By: MAIDGRENADE <156034961+MAIDGRENADE@users.noreply.github.com>
---
 shuffleboard.json | 32 ++++++++++++++++----------------
 simgui.json       |  8 ++++++++
 2 files changed, 24 insertions(+), 16 deletions(-)

diff --git a/shuffleboard.json b/shuffleboard.json
index edc0351..573cc3f 100644
--- a/shuffleboard.json
+++ b/shuffleboard.json
@@ -11,7 +11,7 @@
         "vgap": 16.0,
         "titleType": 0,
         "tiles": {
-          "0,0": {
+          "4,2": {
             "size": [
               1,
               1
@@ -56,7 +56,7 @@
               "Visuals/Counter clockwise": false
             }
           },
-          "9,1": {
+          "9,5": {
             "size": [
               1,
               1
@@ -69,7 +69,7 @@
               "_showGlyph": false
             }
           },
-          "3,0": {
+          "4,0": {
             "size": [
               1,
               2
@@ -88,7 +88,7 @@
               "Visuals/Orientation": "VERTICAL"
             }
           },
-          "9,0": {
+          "0,0": {
             "size": [
               1,
               1
@@ -103,7 +103,7 @@
               "Colors/Color when false": "#8B0000FF"
             }
           },
-          "7,1": {
+          "1,2": {
             "size": [
               2,
               1
@@ -116,7 +116,7 @@
               "_showGlyph": true
             }
           },
-          "1,3": {
+          "7,1": {
             "size": [
               1,
               1
@@ -129,7 +129,7 @@
               "_showGlyph": true
             }
           },
-          "0,3": {
+          "6,1": {
             "size": [
               1,
               1
@@ -142,7 +142,7 @@
               "_showGlyph": true
             }
           },
-          "2,3": {
+          "8,1": {
             "size": [
               1,
               1
@@ -155,7 +155,7 @@
               "_showGlyph": true
             }
           },
-          "7,0": {
+          "6,2": {
             "size": [
               2,
               1
@@ -168,7 +168,7 @@
               "_showGlyph": true
             }
           },
-          "5,3": {
+          "7,0": {
             "size": [
               1,
               1
@@ -187,7 +187,7 @@
               "Visuals/Orientation": "HORIZONTAL"
             }
           },
-          "4,3": {
+          "6,0": {
             "size": [
               1,
               1
@@ -206,7 +206,7 @@
               "Visuals/Orientation": "HORIZONTAL"
             }
           },
-          "6,3": {
+          "8,0": {
             "size": [
               1,
               1
@@ -243,9 +243,9 @@
     }
   ],
   "windowGeometry": {
-    "x": -7.199999809265137,
-    "y": -7.199999809265137,
-    "width": 1550.4000244140625,
-    "height": 830.4000244140625
+    "x": -7.0,
+    "y": -7.0,
+    "width": 1551.0,
+    "height": 831.0
   }
 }
\ No newline at end of file
diff --git a/simgui.json b/simgui.json
index f1e1470..89db2a0 100644
--- a/simgui.json
+++ b/simgui.json
@@ -6,6 +6,11 @@
       "/SmartDashboard/SendableChooser[0]": "String Chooser"
     }
   },
+  "NetworkTables": {
+    "Transitory Values": {
+      "open": false
+    }
+  },
   "NetworkTables Info": {
     "Connections": {
       "open": true
@@ -14,5 +19,8 @@
       "open": true
     },
     "visible": true
+  },
+  "NetworkTables View": {
+    "visible": false
   }
 }

From 37ca977331615fbe7c531b5a768411ae2c53469c Mon Sep 17 00:00:00 2001
From: judeconnolly <judeconnolly@seattleacademy.org>
Date: Thu, 8 Feb 2024 17:46:56 -0800
Subject: [PATCH 38/99] Arm Movement remake

---
 src/main/java/frc/robot/Constants.java        |  4 +++-
 .../robot/commands/ArmJoystickControl.java    | 21 ++++++++++++-------
 src/main/java/frc/robot/subsystems/Arm.java   | 17 +++++++++++----
 3 files changed, 30 insertions(+), 12 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 1a3f6aa..6b659bc 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -94,7 +94,7 @@ public static class SwerveDrivetrainConstants {
 
     public static class ArmConstants {
 
-        public static final double MAXIMUM_ARM_DEGREES = 3;
+        public static final double MAXIMUM_ARM_DEGREES = 1;
         public static final double MINIMUM_ARM_DEGREES = 0;
 
         public static final double ARM_AMP_SHOOTING_DEGREES = 0;
@@ -105,6 +105,8 @@ public static class ArmConstants {
         //public static final int LEFT_ENCODER_ID = 0;
         public static final int RIGHT_MOTOR_ID = 0;
         public static final int RIGHT_ENCODER_ID = 0;
+
+        public static final double DEGREES_PER_SECOND = 2.0;
     }
 }
 
diff --git a/src/main/java/frc/robot/commands/ArmJoystickControl.java b/src/main/java/frc/robot/commands/ArmJoystickControl.java
index 8bccc2c..1933559 100644
--- a/src/main/java/frc/robot/commands/ArmJoystickControl.java
+++ b/src/main/java/frc/robot/commands/ArmJoystickControl.java
@@ -1,11 +1,13 @@
 package frc.robot.commands;
 
+import frc.robot.Constants.ArmConstants;
 import frc.robot.subsystems.Arm;
 
 import frc.robot.utils.OptionButton;
 import frc.robot.utils.OptionButton.ActivationMode;
-
+import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
 
 public class ArmJoystickControl extends Command {
@@ -15,6 +17,8 @@ public class ArmJoystickControl extends Command {
     private final OptionButton raiseArmButton;
     private final OptionButton lowerArmButton;
 
+
+
     private double armPos;
 
     public ArmJoystickControl(Arm arm, CommandJoystick joystick) {
@@ -22,8 +26,12 @@ public ArmJoystickControl(Arm arm, CommandJoystick joystick) {
         this.joystick = joystick;
 
 
-        lowerArmButton = new OptionButton(joystick, 101, ActivationMode.HOLD);
-        raiseArmButton = new OptionButton(joystick, 202, ActivationMode.HOLD);
+        lowerArmButton = new OptionButton(joystick, 11, ActivationMode.HOLD);
+        raiseArmButton = new OptionButton(joystick, 12, ActivationMode.HOLD);
+
+        joystick.povUp().onTrue(Commands.run(arm::setArmToSpeakerPosition));
+        joystick.povRight().onTrue(Commands.run(arm::setArmToAmpPosition));
+        joystick.povDown().onTrue(Commands.run(arm::setArmToIntakePosition));
 
         addRequirements(arm);
     }
@@ -35,10 +43,9 @@ public ArmJoystickControl(Arm arm, CommandJoystick joystick) {
     @Override
     public void execute() {
 
-        armPos += raiseArmButton.getStateAsInt();
-        armPos -= lowerArmButton.getStateAsInt();
-        
 
-        arm.setArmAngleDegrees(armPos);
+        //two buttons determining the raising and lowering of the arm
+        arm.changeArmAngleDegreesBy(Double.valueOf(raiseArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
+        arm.changeArmAngleDegreesBy(Double.valueOf(-lowerArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 2bece67..ebc06f0 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -58,13 +58,20 @@ public Arm(int leftMotorId, int rightMotorId, int rightEncoderId) {
 
     }
 
-    public void setArmAngleDegrees(double desiredDegree) {
-        //maximum should ALWAYS be a greater value then minimum
-        if (desiredDegree < ArmConstants.MAXIMUM_ARM_DEGREES || ArmConstants.MINIMUM_ARM_DEGREES > desiredDegree) {
-            armRotation2d = Rotation2d.fromDegrees(desiredDegree);
+    // public void setArmAngleDegrees(double desiredDegree) {
+    //     //maximum should ALWAYS be a greater value then minimum
+    //     if (desiredDegree < ArmConstants.MAXIMUM_ARM_DEGREES || ArmConstants.MINIMUM_ARM_DEGREES > desiredDegree) {
+    //         armRotation2d = Rotation2d.fromDegrees(desiredDegree);
+    //     }
+    // }
+
+     public void changeArmAngleDegreesBy(double desiredDegrees) {
+        if (armRotation2d.getDegrees() < ArmConstants.MAXIMUM_ARM_DEGREES || ArmConstants.MINIMUM_ARM_DEGREES > armRotation2d.getDegrees()) {
+            armRotation2d = Rotation2d.fromDegrees(armRotation2d.getDegrees() + desiredDegrees);
         }
     }
 
+
     public void setArmToAmpPosition() {
         armRotation2d = Rotation2d.fromRadians(ArmConstants.ARM_AMP_SHOOTING_DEGREES);
     }
@@ -84,6 +91,8 @@ public void setArmToIntakePosition() {
     public void periodic() {
         // This method will be called once per scheduler run
 
+        
+
         final double armSpeed = armRaisePIDController.calculate(armPosition.refresh().getValueAsDouble(),armRotation2d.getRotations());
         leftArmMotor.set(armSpeed);
         rightArmMotor.set(armSpeed);

From 4cefc3947f02434da5a3f3df8d66a9235ce06925 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Thu, 8 Feb 2024 21:45:32 -0800
Subject: [PATCH 39/99] minor cleanup / comment fix

---
 .../java/frc/robot/commands/AutoDriveTo.java  |   4 -
 .../java/frc/robot/commands/AutoRotateTo.java |   5 -
 .../java/frc/robot/commands/DriveToPose.java  |   3 +-
 .../frc/robot/commands/DriveToPoseBase.java   |  19 +---
 .../frc/robot/commands/DriveTransform.java    |  10 +-
 .../frc/robot/commands/DriverControl.java     |   3 +-
 .../java/frc/robot/commands/FollowTag.java    |   6 +-
 .../robot/subsystems/ExampleSubsystem.java    |  11 +-
 .../robot/subsystems/SwerveDrivetrain.java    | 106 +++++++-----------
 .../frc/robot/subsystems/SwerveModule.java    | 103 ++++++-----------
 .../java/frc/robot/utils/OptionButton.java    |  29 ++---
 11 files changed, 100 insertions(+), 199 deletions(-)

diff --git a/src/main/java/frc/robot/commands/AutoDriveTo.java b/src/main/java/frc/robot/commands/AutoDriveTo.java
index 061e159..eefad5b 100644
--- a/src/main/java/frc/robot/commands/AutoDriveTo.java
+++ b/src/main/java/frc/robot/commands/AutoDriveTo.java
@@ -11,10 +11,6 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.RobotMovementConstants;
 
-// How to make Command (ignore image instructions, code is out of date, just look at written general instructions): https://compendium.readthedocs.io/en/latest/tasks/commands/commands.html
-// Command based programming: https://docs.wpilib.org/en/stable/docs/software/commandbased/what-is-command-based.html
-// Code documentations https://docs.wpilib.org/en/stable/docs/software/commandbased/commands.html 
-
 public class AutoDriveTo extends Command {
 	private final SwerveDrivetrain drivetrain;
 
diff --git a/src/main/java/frc/robot/commands/AutoRotateTo.java b/src/main/java/frc/robot/commands/AutoRotateTo.java
index d8cf9b3..8523011 100644
--- a/src/main/java/frc/robot/commands/AutoRotateTo.java
+++ b/src/main/java/frc/robot/commands/AutoRotateTo.java
@@ -9,11 +9,6 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import frc.robot.Constants.RobotMovementConstants;
 
-// How to make Command (ignore image instructions, code is out of date, just look at written general instructions): https://compendium.readthedocs.io/en/latest/tasks/commands/commands.html
-// Command based programming: https://docs.wpilib.org/en/stable/docs/software/commandbased/what-is-command-based.html
-// Code documentations https://docs.wpilib.org/en/stable/docs/software/commandbased/commands.html 
-
-/** An example command that uses an example subsystem. */
 public class AutoRotateTo extends Command {
 	private final SwerveDrivetrain drivetrain;
 
diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java
index e69574d..059c6e9 100644
--- a/src/main/java/frc/robot/commands/DriveToPose.java
+++ b/src/main/java/frc/robot/commands/DriveToPose.java
@@ -9,7 +9,8 @@ public class DriveToPose extends DriveToPoseBase {
     /**
      * Create a new DriveToPose command. Tries to drive to a set Pose based on odometry 
      * 
-     * <p>This drives relative to the robot starting position, so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
+     * <p>This drives relative to the robot starting position,
+	 * so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
      * where forward is whatever direction the robot started in</p>
      * 
      * <p>The last place the drivetrain position was reset counts as the starting position</p>
diff --git a/src/main/java/frc/robot/commands/DriveToPoseBase.java b/src/main/java/frc/robot/commands/DriveToPoseBase.java
index c2b3314..f302193 100644
--- a/src/main/java/frc/robot/commands/DriveToPoseBase.java
+++ b/src/main/java/frc/robot/commands/DriveToPoseBase.java
@@ -15,20 +15,13 @@ public class DriveToPoseBase extends Command {
 	private final PIDController xController, yController, rotationController;
 
 	/**
-	 * Create a new DriveToPose command. Tries to drive to a set Pose based on
-	 * odometry
+	 * Create a new DriveToPose command. Tries to drive to a set Pose based on odometry.
 	 * 
-	 * <p>
-	 * This drives relative to the robot starting position, so a pose of +2x and +1y
-	 * will drive to the position 2 meters forward and 1 meter left of whether the
-	 * robot started,
-	 * where forward is whatever direction the robot started in
-	 * </p>
-	 * 
-	 * <p>
-	 * The last place the drivetrain position was reset counts as the starting
-	 * position
-	 * </p>
+     * <p>This drives relative to the robot starting position,
+	 * so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
+     * where forward is whatever direction the robot started in</p>
+     * 
+     * <p>The last place the drivetrain position was reset counts as the starting position</p>
 	 * 
 	 * @param drivetrain the drivetrain of the robot
 	 */
diff --git a/src/main/java/frc/robot/commands/DriveTransform.java b/src/main/java/frc/robot/commands/DriveTransform.java
index 8ca4c1c..0f8ffb2 100644
--- a/src/main/java/frc/robot/commands/DriveTransform.java
+++ b/src/main/java/frc/robot/commands/DriveTransform.java
@@ -13,12 +13,10 @@ public class DriveTransform extends DriveToPoseBase {
 	 * Create a new DriveToPose command. Tries to drive a certain transform using
 	 * the DriveToPose command.
 	 * 
-	 * <p>
-	 * This drives relative to the robot, so a transform of +2x and +1y will drive 2
-	 * meters forward and 1 meter left,
-	 * where forward is whatever direction the robot is currently facing
-	 * </p>
-	 * 
+	 * <p>This drives relative to the robot starting position,
+	 * so a transform of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
+     * where forward is whatever direction the robot is currently facing</p>
+     *  
 	 * @param drivetrain the drivetrain of the robot
 	 * @param transform  target transform to drive to, will be added to current
 	 *                   position to get target pose
diff --git a/src/main/java/frc/robot/commands/DriverControl.java b/src/main/java/frc/robot/commands/DriverControl.java
index c0e2214..be36840 100644
--- a/src/main/java/frc/robot/commands/DriverControl.java
+++ b/src/main/java/frc/robot/commands/DriverControl.java
@@ -10,8 +10,7 @@
 import frc.robot.utils.OptionButton;
 
 /**
- * This can be the default command for the drivetrain, allowing for remote
- * operation with a controller
+ * This can be the default command for the drivetrain, allowing for remote operation with a controller.
  */
 public class DriverControl extends Command {
 	protected final SwerveDrivetrain drivetrain;
diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java
index 0fd9a82..3f68adc 100644
--- a/src/main/java/frc/robot/commands/FollowTag.java
+++ b/src/main/java/frc/robot/commands/FollowTag.java
@@ -16,8 +16,7 @@ public class FollowTag extends DriveToPoseBase {
 	private double secondsSinceTagLastSeen;
 
 	/**
-	 * Create a new FollowTag command. Tries to follow a tag while staying a certain
-	 * distance away.
+	 * Create a new FollowTag command. Tries to follow a tag while staying a certain distance away.
 	 * 
 	 * @param drivetrain          the drivetrain of the robot
 	 * @param tagID               the numerical ID of the the tag to follow
@@ -34,8 +33,7 @@ public FollowTag(SwerveDrivetrain drivetrain, int tagID, Transform2d targetDista
 	}
 
 	/**
-	 * Create a new FollowTag command. Tries to follow a tag while staying a certain
-	 * distance away.
+	 * Create a new FollowTag command. Tries to follow a tag while staying a certain distance away.
 	 * 
 	 * @param drivetrain          the drivetrain of the robot
 	 * @param tagID               the numerical ID of the the tag to follow
diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java
index d59c5ae..64915b8 100644
--- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java
@@ -6,11 +6,6 @@
 // Command based programming: https://docs.wpilib.org/en/stable/docs/software/commandbased/what-is-command-based.html
 // Subsystem Documentation documentation: https://docs.wpilib.org/en/stable/docs/software/commandbased/subsystems.html
 
-/**
- * How to make a Subsystem:
- * 1. Copy this file, remember that class name has to match
- */
-
 public class ExampleSubsystem extends SubsystemBase {
 	/** Constructor. Creates a new ExampleSubsystem. */
 	public ExampleSubsystem() {
@@ -19,10 +14,8 @@ public ExampleSubsystem() {
 
 	/**
 	 * This method is called periodically by the CommandScheduler, about every 20ms.
-	 * It should be used for updating subsystem-specific state that you don't want
-	 * to offload to a Command.
-	 * Try to avoid "doing to much" in this method (for example no driver control
-	 * here).
+	 * It should be used for updating subsystem-specific state that you don't want to offload to a Command.
+	 * Try to avoid "doing to much" in this method (for example no driver control here).
 	 */
 	@Override
 	public void periodic() {
diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index 6b48f69..9fffd52 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -27,30 +27,26 @@
  */
 public class SwerveDrivetrain extends SubsystemBase {
 	/**
-	 * The SwerveDriveKinematics class is a useful tool that converts between a
-	 * ChassisSpeeds object
-	 * and several SwerveModuleState objects, which contains velocities and angles
-	 * for each swerve module of a swerve drive robot.
+	 * The SwerveDriveKinematics class is a useful tool that converts between a ChassisSpeeds object
+	 * and several SwerveModuleState objects, which contains velocities and angles for each swerve module of a swerve drive robot.
 	 * 
 	 * @see https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html
 	 */
 	private final SwerveDriveKinematics kinematics;
 
 	/**
-	 * The SwerveDriveOdometry class can be used to track the position of a swerve
-	 * drive robot on the field *
+	 * The SwerveDriveOdometry class can be used to track the position of a swerve drive robot on the field
 	 * 
 	 * @see https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-odometry.html
 	 */
 	private final SwerveDriveOdometry odometry;
 
-	// Our 4 swerve Modules
+	/** Swerve module */
 	private final SwerveModule moduleFL, moduleFR, moduleBL, moduleBR;
 	private final SwerveModule[] modules;
 
 	/**
-	 * The Gyroscope on the robot. It gives data on Pitch, Yaw, and Roll of robot,
-	 * as well as many other things
+	 * The Gyroscope on the robot. It gives data on Pitch, Yaw, and Roll of robot, as well as many other things
 	 * 
 	 * @see https://www.kauailabs.com/public_files/navx-mxp/apidocs/java/com/kauailabs/navx/frc/AHRS.html
 	 * @see https://ibb.co/dJrL259
@@ -58,8 +54,8 @@ public class SwerveDrivetrain extends SubsystemBase {
 	private final AHRS gyro;
 
 	/**
-	 * Pose of robot. The pose is the current the X, Y and Rotation position of the
-	 * robot, relative to the last reset.
+	 * Pose of robot. The pose is the current the X, Y and Rotation position of the robot, relative to the last reset.
+	 * It is updated every 20ms in periodic.
 	 * 
 	 * @see https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/pose.html#pose
 	 */
@@ -68,15 +64,15 @@ public class SwerveDrivetrain extends SubsystemBase {
 	/**
 	 * Constructor the drivetrain subsystem.
 	 * 
-	 * @param gyro           Gyroscope on robot, should be physically located near
-	 *                       center of robot
+	 * @param gyro           Gyroscope on robot, should be physically located near center of robot
 	 * @param swerveModuleFL Front left swerve module
 	 * @param swerveModuleFR Front right swerve module
 	 * @param swerveModuleBL Back left swerve module
 	 * @param swerveModuleBR Back right swerve module
 	 */
-	public SwerveDrivetrain(AHRS gyro, SwerveModule swerveModuleFL, SwerveModule swerveModuleFR,
-			SwerveModule swerveModuleBL, SwerveModule swerveModuleBR) {
+	public SwerveDrivetrain(AHRS gyro,
+		SwerveModule swerveModuleFL, SwerveModule swerveModuleFR,
+		SwerveModule swerveModuleBL, SwerveModule swerveModuleBR) {
 
 		// save parameters
 		this.gyro = gyro;
@@ -98,7 +94,7 @@ public SwerveDrivetrain(AHRS gyro, SwerveModule swerveModuleFL, SwerveModule swe
 				getHeading(),
 				modulesMap(SwerveModule::getPosition, SwerveModulePosition[]::new));
 
-		// Set up name and children for sendable registry
+		// set up name and children for sendable registry
 		setName(toString());
 		for (SwerveModule module : modules) {
 			addChild(module.getName(), module);
@@ -120,11 +116,9 @@ public void periodic() {
 	}
 
 	/**
-	 * Get the pose of robot, as calculated by odometry from gyro and distances
-	 * swerve modules have traveled
+	 * Get the pose of robot, as calculated by odometry from gyro and distances swerve modules have traveled
 	 * 
-	 * @return The current positions of the robot, contains translational and
-	 *         rotational elements.
+	 * @return The current positions of the robot, contains translational and rotational elements.
 	 */
 	public Pose2d getPosition() {
 		return pose;
@@ -174,22 +168,19 @@ public ChassisSpeeds getState() {
 	/**
 	 * Get speeds of robot.
 	 * 
-	 * @param fieldRelative True if the robot is using a field relative coordinate
-	 *                      system, false if using a robot relive coordinate system.
+	 * @param fieldRelative True if the robot is using a field relative coordinate system,
+	 * 						false if using a robot relive coordinate system.
 	 * @return Speeds of drivetrain (from swerve modules)
 	 */
 	public ChassisSpeeds getState(boolean fieldRelative) {
-		ChassisSpeeds speeds = getState();
-		if (fieldRelative)
-			speeds = ChassisSpeeds.fromRobotRelativeSpeeds(speeds, getHeading());
-		return speeds;
+		if (fieldRelative) return ChassisSpeeds.fromRobotRelativeSpeeds(getState(), getHeading());
+		return getState();
 	}
 
 	/**
 	 * Set robot relative speeds of robot using default speeds units.
 	 * 
-	 * @param fieldRelative True if the robot is using a field relative coordinate
-	 *                      system, false if using a robot relive coordinate syste
+	 * @param speeds Desired speeds of drivetrain (using swerve modules)
 	 */
 	public void setDesiredState(ChassisSpeeds speeds) {
 		setDesiredState(speeds, false);
@@ -199,8 +190,8 @@ public void setDesiredState(ChassisSpeeds speeds) {
 	 * Set speeds of robot using default speeds units.
 	 * 
 	 * @param speeds        Desired speeds of drivetrain (using swerve modules)
-	 * @param fieldRelative True if the robot is using a field relative coordinate
-	 *                      system, false if using a robot relive coordinate syste
+	 * @param fieldRelative True if the robot is using a field relative coordinate system, 
+	 * 						false if using a robot relive coordinate system
 	 */
 	public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative) {
 		setDesiredState(speeds, fieldRelative, false);
@@ -210,29 +201,23 @@ public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative) {
 	 * Set speeds of robot.
 	 * 
 	 * <p>
-	 * Vx: the velocity of the robot in the x (forward) direction in meter per
-	 * second.
-	 * Vy: the velocity of the robot in the y (sideways) direction in meter per
-	 * second. (Positive values mean the robot is moving to the left).
+	 * Vx: the velocity of the robot in the x (forward) direction in meter per second.
+	 * Vy: the velocity of the robot in the y (sideways) direction in meter per second. (Positive values mean the robot is moving to the left).
 	 * Omega: the angular velocity of the robot in radians per second.
 	 * 
 	 * <p>
-	 * If field relative, forward will be directly away from driver, no matter the
-	 * rotation of the robot.
+	 * If field relative, forward will be directly away from driver, no matter the rotation of the robot.
 	 * If robot relative, forward will be whatever direction the robot is facing in.
 	 * 
 	 * <p>
-	 * If power drive mode then speeds X, Y, and Omega are in motor powers from -1
-	 * to 1.
-	 * If normal drive mode then X and Y are in meters per second and Omega is in
-	 * radians per second
+	 * If power drive mode then speeds X, Y, and Omega are in motor powers from -1 to 1.
+	 * If normal drive mode then X and Y are in meters per second and Omega is in radians per second
 	 * 
 	 * @param speeds         Desired speeds of drivetrain (using swerve modules)
-	 * @param fieldRelative  True if the robot is using a field relative coordinate
-	 *                       system, false if using a robot relive coordinate
-	 *                       system.
-	 * @param powerDriveMode True if in power drive mode with motor powers, false if
-	 *                       in normal drive mode with default units
+	 * @param fieldRelative  True if the robot is using a field relative coordinate system, 
+	 * 						 false if using a robot relive coordinate system
+	 * @param powerDriveMode True if in power drive mode with motor powers,
+	 * 					     false if in normal drive mode with default units
 	 */
 	public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative, boolean powerDriveMode) {
 
@@ -262,19 +247,17 @@ public SwerveDriveWheelPositions getWheelPositions() {
 	 * <p>
 	 * Return the heading of the robot as a rotation in a 2D coordinate frame
 	 * represented by a point on the unit circle (cosine and sine).
-	 * </p>
 	 * 
 	 * <p>
 	 * The angle is continuous, that is it will continue from 360 to 361 degrees.
 	 * This allows algorithms that wouldn't want to see a discontinuity in the gyro
 	 * output as it sweeps past from 360 to 0 on the second time around.
-	 * </p>
 	 * 
 	 * <p>
 	 * The angle is expected to increase as the gyro turns counterclockwise when
 	 * looked at from the top.
-	 * </p>
-	 * It needs to follow the NWU axis convention.
+	 * 
+	 * It follows the NWU axis convention.
 	 * 
 	 * @return the current heading of the robot as a {@link Rotation2d}.
 	 * 
@@ -301,33 +284,26 @@ public Rotation3d getHeading3d() {
 	/**
 	 * Utility method. Function to easily run a function on each swerve module
 	 * 
-	 * @param func function to run on each swerve module, takes one argument and
-	 *             returns nothing, operates via side effects.
+	 * @param func function to run on each swerve module, takes one argument and returns nothing, operates via side effects.
 	 */
 	private void modulesMap(Consumer<SwerveModule> func) {
 		Arrays.stream(modules).forEach(func);
 	}
 
 	/**
-	 * Utility method. Function to easily run a function on each swerve module and
-	 * collect results to array.
-	 * Insures that we don't mix up order of swerve modules, as this could lead to
-	 * hard to spot bugs.
+	 * Utility method. Function to easily run a function on each swerve module and collect results to array.
+	 * Insures that we don't mix up order of swerve modules, as this could lead to hard to spot bugs.
 	 * 
-	 * @param <T>              type that is returned by function and should be
-	 *                         collected
+	 * @param <T>              type that is returned by function and should be collected
 	 * @param func             function that gets some data off each swerve module
-	 * @param arrayInitializer constructor function for array to collect results in,
-	 *                         use T[]::new
-	 * @return array of results from func.
+	 * @param arrayInitializer constructor function for array to collect results in, use T[]::new
+	 * @return 				   array of results from func.
 	 */
 	private <T> T[] modulesMap(Function<? super SwerveModule, ? extends T> func, IntFunction<T[]> arrayInitializer) {
 		// Private method with template argument T. Returns array of type T.
-		// Takes in function that accepts a SwerveModule or something higher on the
-		// inheritance chain (For example: Object, SubsystemBase)
-		// and returns something of type T or something lower on the inheritance chain.
-		// (For example if T is Object: Integer)
-		// Also takes a T array initializer
+		// Takes in function that accepts a SwerveModule or something higher on the inheritance chain (For example: Object, SubsystemBase)
+		// and returns something of type T or something lower on the inheritance chain. (For example if T is Object: Integer)
+		// Also takes a T array initializer (T[]::new)
 		return Arrays.stream(modules).map(func).toArray(arrayInitializer);
 	}
 
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 1715d6b..995cef4 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -29,14 +29,12 @@
  */
 public class SwerveModule extends SubsystemBase {
 
-	// the drive motor is the motor that spins the wheel making the robot move
-	// across the ground (aka wheel or velocity motor)
+	// the drive motor is the motor that spins the wheel making the robot move across the ground (aka wheel or velocity motor)
 	private final CANSparkMax driveMotor;
 	private final RelativeEncoder driveEncoder;
 	private final SparkPIDController drivePIDController;
 
-	// the steering motor is the motor that changes the rotation of the wheel
-	// allowing the robot to drive in any direction (aka spin or angular motor)
+	// the steering motor is the motor that changes the rotation of the wheel allowing the robot to drive in any direction (aka spin or angular motor)
 	// Also allows for spinning in place
 	private final CANSparkMax steeringMotor;
 	private final CANcoder steeringEncoder;
@@ -61,15 +59,11 @@ public class SwerveModule extends SubsystemBase {
 	 * 
 	 * @param velocityMotorDeviceID  device ID for drive motor
 	 * @param steeringMotorDeviceId  device ID for steering motor
-	 * @param angularEncoderDeviceID device ID for the angular motor's absolute
-	 *                               encoder
-	 * @param distanceFromCenter     distance from center of robot to center of
-	 *                               swerve module
-	 * @param steeringEncoderZero    the zero (forward) position for the angular
-	 *                               motor's absolute encoder, in rotations
+	 * @param angularEncoderDeviceID device ID for the angular motor's absolute encoder
+	 * @param distanceFromCenter     distance from center of robot to center of swerve module
+	 * @param steeringEncoderZero    the zero (forward) position for the angular motor's absolute encoder, in rotations
 	 */
-	public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steeringAbsoluteEncoderId,
-			double steeringEncoderZero, Translation2d distanceFromCenter) {
+	public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steeringAbsoluteEncoderId, double steeringEncoderZero, Translation2d distanceFromCenter) {
 		// --- Drive Motor ---
 		driveMotor = new CANSparkMax(driveMotorDeviceId, MotorType.kBrushless);
 
@@ -132,8 +126,8 @@ public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steer
 	@Override
 	public void periodic() {
 		if (!stopped) {
-			// Calculate how fast to spin the motor to get to the desired angle using our
-			// PID controller, then set the motor to spin at that speed
+			// Calculate how fast to spin the motor to get to the desired angle using our PID controller,
+			// then set the motor to spin at that speed
 			steeringMotor.set(steeringPIDController.calculate(getSteeringAngleRotations()));
 		}
 	}
@@ -141,8 +135,7 @@ public void periodic() {
 	// --- Direct control methods ---
 
 	/**
-	 * Stop drive and steering motor of swerve module, module can be moved again by
-	 * calling setDesiredState.
+	 * Stop drive and steering motor of swerve module, module can be moved again by calling setDesiredState.
 	 */
 	public void stop() {
 		stopped = true;
@@ -161,8 +154,7 @@ public void toDefaultState() {
 	}
 
 	/**
-	 * 
-	 * 
+	 * Turn module to facing inward, directly towards center of robot, useful for braking
 	 */
 	public void toInwardPosition() {
 		setDesiredState(new SwerveModuleState(
@@ -186,14 +178,11 @@ public SwerveModuleState getState() {
 	}
 
 	/**
-	 * Set the state of the swerve module. The state is the speed and angle of the
-	 * swerve module.
-	 * You can use {@code Rotation2d.fromDegrees()} to create angle.
+	 * Set the state of the swerve module. The state is the speed and angle of the swerve module.
+	 * You can use {@code Rotation2d.fromDegrees()} to create an angle.
 	 * 
-	 * @param state          New state of swerve module, contains speed in meters
-	 *                       per second and angle as {@link Rotation2d}
-	 * @param powerDriveMode whether the SwerveModuleState is in meters per second
-	 *                       (false) or motor power (true)
+	 * @param state          New state of swerve module, contains speed in meters per second and angle as {@link Rotation2d}
+	 * @param powerDriveMode Whether the SwerveModuleState is in meters per second (false) or motor power (true)
 	 */
 	public void setDesiredState(SwerveModuleState state, boolean powerDriveMode) {
 
@@ -212,31 +201,22 @@ public void setDesiredState(SwerveModuleState state, boolean powerDriveMode) {
 		// --- Set drive motor ---
 
 		if (state.speedMetersPerSecond == 0) {
-			// If our desired speed is 0, just use the builtin motor stop, no matter the
-			// mode.
+			// If our desired speed is 0, just use the builtin motor stop, no matter the mode.
 
-			// Stops motor movement. Motor can be moved again by calling set without having
-			// to re-enable the motor.
+			// Stops motor movement. Motor can be moved again by calling set without having to re-enable the motor.
 			driveMotor.stopMotor();
 		} else if (powerDriveMode) {
 			// If we are in power drive mode just directly set power to our desired speed.
 
-			// This is a bit of an abuse of the SwerveModuleState object as we treat speeds
-			// as power values from -1 to 1.
-			// We do this because we don't want to have to deal with a PID controller when
-			// we are just driving, as a human driver does not care about they exact speed
-			// mapping.
+			// This is a bit of an abuse of the SwerveModuleState object as we treat speeds as power values from -1 to 1.
+			// We do this because we don't want to have to deal with a PID controller when we are just driving, as a human driver does not care about they exact speed mapping.
 			driveMotor.set(state.speedMetersPerSecond);
 		} else {
-			// If we are in normal drive mode use the motor controller to set our target
-			// velocity
-
-			// The CANSparkMaxes have a builtin PID controller on them we can use to set a
-			// target velocity.
-			// We first convert our speed from meters per second to rotations per minute, as
-			// that is the native unit of our devices
-			final double desiredDriveRotationsPerMinute = (state.speedMetersPerSecond * 60)
-					/ SwerveModuleConstants.WHEEL_CIRCUMFERENCE;
+			// If we are in normal drive mode use the motor controller to set our target velocity.
+
+			// The CANSparkMaxes have a builtin PID controller on them we can use to set a target velocity.
+			// We first convert our speed from meters per second to rotations per minute, as that is the native unit of our devices
+			final double desiredDriveRotationsPerMinute = (state.speedMetersPerSecond * 60) / SwerveModuleConstants.WHEEL_CIRCUMFERENCE;
 			drivePIDController.setReference(desiredDriveRotationsPerMinute, ControlType.kVelocity);
 		}
 	}
@@ -244,11 +224,9 @@ public void setDesiredState(SwerveModuleState state, boolean powerDriveMode) {
 	// --- Public info getters ---
 
 	/**
-	 * Get the position of the swerve module. The position is the distance traveled
-	 * by the drive motor and angle of the drive motor.
+	 * Get the position of the swerve module. The position is the distance traveled by the drive motor and angle of the drive motor.
 	 * 
-	 * @return Current position of swerve module, contains distance traveled by
-	 *         motor in meters and angle as {@link Rotation2d}
+	 * @return Current position of swerve module, contains distance traveled by motor in meters and angle as {@link Rotation2d}
 	 */
 	public SwerveModulePosition getPosition() {
 		return new SwerveModulePosition(
@@ -257,8 +235,7 @@ public SwerveModulePosition getPosition() {
 	}
 
 	/**
-	 * Get locations of the wheel relative to the physical center of the robot.
-	 * Useful for kinematics.
+	 * Get locations of the wheel relative to the physical center of the robot. Useful for kinematics.
 	 * 
 	 * @return Translation2d representing distance from center of bot
 	 */
@@ -289,8 +266,7 @@ private double getDriveDistanceMeters() {
 	/**
 	 * Get the angel of the steering motor.
 	 * 
-	 * @return Current position in rotations of the steering motor, accounting for
-	 *         offset
+	 * @return Current position in rotations of the steering motor, accounting for offset
 	 */
 	private double getSteeringAngleRotations() {
 		return steeringPosition.refresh().getValueAsDouble();
@@ -299,10 +275,8 @@ private double getSteeringAngleRotations() {
 	// --- Util ---
 
 	/**
-	 * Optimize a swerve module state so that instead of suddenly rotating the wheel
-	 * (with steering motor)
-	 * to go a certain direction we can instead just turn a half as much and switch
-	 * the speed of wheel to go in reverse.
+	 * Optimize a swerve module state so that instead of suddenly rotating the wheel (with steering motor)
+	 * to go a certain direction we can instead just turn a half as much and switch the speed of wheel to go in reverse.
 	 * 
 	 * @param desiredState The state you want the swerve module to be in
 	 * @param currentAngle The current angle of the swerve module in degrees
@@ -318,10 +292,8 @@ private static SwerveModuleState optimize(SwerveModuleState desiredState, Rotati
 		// found how much we have to move to get to target angle
 		double delta = targetAngle - currentAngle.getDegrees();
 
-		// If we have to flip around more than 90 degrees than instead just reverse our
-		// direction
-		// and only turn enough so that we have the motor facing in the same direction,
-		// just the other way
+		// If we have to flip around more than 90 degrees than instead just reverse our direction
+		// and only turn enough so that we have the motor facing in the same direction, just the other way
 		if (Math.abs(delta) > 90) {
 			targetSpeed = -targetSpeed;
 			targetAngle += delta > 0 ? -180 : 180;
@@ -331,18 +303,13 @@ private static SwerveModuleState optimize(SwerveModuleState desiredState, Rotati
 	}
 
 	/**
-	 * Utility method. Move an angle into the range of the reference. Finds the
-	 * relative 0 and 360
-	 * position for a scope reference and moves the new angle into that.
-	 * Example: {@code placeInAppropriate0To360Scope(90, 370) = 10.0}
-	 * {@code placeInAppropriate0To360Scope(720, 10) = 730.0}
+	 * Utility method. Move an angle into the range of the reference. Finds the relative 0 and 360 position for a scope reference and moves the new angle into that.
+	 * Example:
+	 * <li>{@code placeInAppropriate0To360Scope(90, 370) = 10.0}, since 90 is in the 0-360 scope we move 370 into that scope
+	 * <li>{@code placeInAppropriate0To360Scope(720, 10) = 730.0} since 720 is in the 720-1080 scope we move 10 to be in that scope
 	 * 
 	 * @param scopeReference The reference to find which 0-360 scope we are in.
-	 *                       For example {@code 10} is in {@code 0-360} scope while
-	 *                       {@code 370} is in {@code 360-720} scope.
 	 * @param newAngle       The angle we want to move into found scope.
-	 *                       For example if the scope was {@code 0-360} and our
-	 *                       angle was {@code 370} it would become {@code 10}
 	 * @return {@code newAngle} in the same scope as {@code scopeReference}
 	 */
 	private static double placeInAppropriate0To360Scope(double scopeReference, double newAngle) {
diff --git a/src/main/java/frc/robot/utils/OptionButton.java b/src/main/java/frc/robot/utils/OptionButton.java
index 19e614d..9a67d65 100644
--- a/src/main/java/frc/robot/utils/OptionButton.java
+++ b/src/main/java/frc/robot/utils/OptionButton.java
@@ -51,8 +51,7 @@ public OptionButton(CommandGenericHID controller, int button, ActivationMode mod
 	 * 
 	 * @param commandDevice device that button is on
 	 * @param button        button number
-	 * @param mode          whether or not we want button to act as toggle or hold
-	 *                      button
+	 * @param mode          whether or not we want button to act as toggle or hold button
 	 */
 	public OptionButton(Supplier<Trigger> button, ActivationMode mode) {
 		this.button = button;
@@ -81,35 +80,21 @@ public void toggleOff() {
 	}
 
 	/**
-	 * <p>
 	 * Get the state of the button
-	 * </p>
 	 * 
-	 * <p>
-	 * for TAP buttons this returns true if the button has been tapped
-	 * </p>
+	 * <li>for TAP buttons this returns true if the button has been tapped
 	 * 
-	 * <p>
-	 * for HOLD buttons this returns true if the button is currently being held down
-	 * </p>
+	 * <li>for HOLD buttons this returns true if the button is currently being held down
 	 * 
-	 * <p>
-	 * for TOGGLE buttons this returns true if the button has been tapped an odd
-	 * number of times
-	 * </p>
+	 * <li> for TOGGLE buttons this returns true if the button has been tapped an odd number of times
 	 * 
 	 * @return boolean with true being the button is active
 	 */
 	public boolean getState() {
 		switch (mode) {
-			case HOLD:
-				return button.get().getAsBoolean();
-			case TAP:
-				return isToggled;
-			case TOGGLE:
-				return isToggled;
-			default:
-				return false;
+			case HOLD: return button.get().getAsBoolean();
+			case TAP: case TOGGLE: return isToggled;
+			default: return false;
 		}
 	}
 

From cd22023059d8430a9b6791277f0e2f9c5d142223 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Sat, 10 Feb 2024 10:38:14 -0800
Subject: [PATCH 40/99] fix some minor bugs and cleanup comments

---
 src/main/java/frc/robot/commands/DriveToPose.java   |  1 +
 .../java/frc/robot/commands/DriveToPoseBase.java    | 13 +++++++++----
 src/main/java/frc/robot/commands/FollowTag.java     |  2 +-
 3 files changed, 11 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java
index 059c6e9..8dee1ed 100644
--- a/src/main/java/frc/robot/commands/DriveToPose.java
+++ b/src/main/java/frc/robot/commands/DriveToPose.java
@@ -21,6 +21,7 @@ public class DriveToPose extends DriveToPoseBase {
     public DriveToPose(SwerveDrivetrain drivetrain, Pose2d targetPosition) {
         super(drivetrain);
 
+		// Set desired pose so once we initialize we go there
         setDesiredPosition(targetPosition);
     }
 }
diff --git a/src/main/java/frc/robot/commands/DriveToPoseBase.java b/src/main/java/frc/robot/commands/DriveToPoseBase.java
index f302193..594702a 100644
--- a/src/main/java/frc/robot/commands/DriveToPoseBase.java
+++ b/src/main/java/frc/robot/commands/DriveToPoseBase.java
@@ -9,7 +9,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.RobotMovementConstants;
 
-/** Base Command for automatically driving to a certain Pose on the field */
+/** Base Command for child commands that need to automatically drive to a certain Pose on the field */
 public class DriveToPoseBase extends Command {
 	private final SwerveDrivetrain drivetrain;
 	private final PIDController xController, yController, rotationController;
@@ -53,6 +53,11 @@ public DriveToPoseBase(SwerveDrivetrain drivetrain) {
 		addRequirements(this.drivetrain);
 	}
 
+	/**
+	 * 
+	 * 
+	 * @param targetPose Pose that robot will drive to once command is scheduled
+	 */
 	public void setDesiredPosition(Pose2d targetPose) {
 		xController.setSetpoint(targetPose.getX());
 		yController.setSetpoint(targetPose.getY());
@@ -63,9 +68,9 @@ public Pose2d getPosition() {
 		return drivetrain.getPosition();
 	}
 
+	/** Put all swerve modules in default positions */
 	@Override
 	public void initialize() {
-		// Put all swerve modules in default position
 		drivetrain.toDefaultStates();
 	}
 
@@ -90,15 +95,15 @@ public void execute() {
 		drivetrain.setDesiredState(speeds);
 	}
 
+	/** Finish once all controllers are within tolerance */
 	@Override
 	public boolean isFinished() {
-		// Finish once all controllers are within tolerance
 		return xController.atSetpoint() && yController.atSetpoint() && rotationController.atSetpoint();
 	}
 
+	/** Stop all swerve modules at end */
 	@Override
 	public void end(boolean interrupted) {
-		// Stop all swerve modules at end
 		drivetrain.stop();
 	}
 }
diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java
index 3f68adc..c3a767a 100644
--- a/src/main/java/frc/robot/commands/FollowTag.java
+++ b/src/main/java/frc/robot/commands/FollowTag.java
@@ -57,7 +57,7 @@ public void execute() {
 		final Transform2d tagPosition = new Transform2d(
 				tagPosition3d.getZ(),
 				tagPosition3d.getX(),
-				Rotation2d.fromRadians(tagPosition3d.getRotation().getZ()));
+				Rotation2d.fromRadians(-tagPosition3d.getRotation().getZ()));
 
 		final Transform2d driveTransform = tagPosition.plus(targetDistance.inverse());
 

From 4278680a65958a87ad3db5c62caaf9a202e4d62b Mon Sep 17 00:00:00 2001
From: DigitalFork <113265535+DigitalFork@users.noreply.github.com>
Date: Sat, 10 Feb 2024 13:47:30 -0800
Subject: [PATCH 41/99] Squashed commit of the following:

commit e8f587211f3846ad5cef366dc51678cd3f9ca8b5
Merge: 9d2afd0 9b3b533
Author: DigitalFork <113265535+DigitalFork@users.noreply.github.com>
Date:   Sat Feb 10 11:53:01 2024 -0800

    Merge branch 'main' into vision

commit 9d2afd0389eb62be272a461a5e52da90f23a5b4c
Author: DigitalFork <113265535+DigitalFork@users.noreply.github.com>
Date:   Sat Feb 10 11:42:59 2024 -0800

    Re-add commented lines (for merge)

commit eb7c2c1a87d7a1963a8dc3a5170461c5f67387a0
Author: DigitalFork <113265535+DigitalFork@users.noreply.github.com>
Date:   Sat Feb 10 11:34:40 2024 -0800

    Add camera name constant and port forwarder for USB port on RoboRIO
    https://docs.photonvision.org/en/latest/docs/installation/networking.html#port-forwarding

commit 91d13c34e427cd716fd1ce4a1e8a79319a1f73ec
Author: DigitalFork <113265535+DigitalFork@users.noreply.github.com>
Date:   Wed Feb 7 21:00:11 2024 -0800

    Update PhotonLib version
    Fix SmartDashboard put statement
    Add camera offset transform constant

commit bce9f628b776321886977f7c944afbd72ea52188
Author: DigitalFork <113265535+DigitalFork@users.noreply.github.com>
Date:   Sat Feb 3 21:57:36 2024 -0800

    Add new post statements for testing
    Need to get 3D AprilTags working

commit 00ddc23776d051a9eb17bf2a23d606034028fac8
Author: DigitalFork <113265535+DigitalFork@users.noreply.github.com>
Date:   Sat Feb 3 14:11:35 2024 -0800

    VISION PIPELINE WORKS!
    Update photonlib vendordep
    Vision cameraName is Arducam name

commit 9996b906592e8c4db0e8018c79bdf68d21ad468c
Author: DigitalFork <113265535+DigitalFork@users.noreply.github.com>
Date:   Sat Feb 3 10:27:24 2024 -0800

    Add Initial Full Vision Pipeline Test
    Add getDist methods for tag detection in bot.
    Add ShuffleBoard field for posting tag numbers. (Not working, need to test)

commit 2e38015b6a248c65732e0afbbc08cd5e80ae5f09
Author: DigitalFork <113265535+DigitalFork@users.noreply.github.com>
Date:   Wed Jan 24 15:50:17 2024 -0800

    Add initial files for vision
---
 src/main/java/frc/robot/Constants.java        |  14 ++-
 src/main/java/frc/robot/RobotContainer.java   |  46 +++++---
 .../java/frc/robot/subsystems/Vision.java     | 106 ++++++++++++++++++
 vendordeps/photonlib.json                     |  57 ++++++++++
 4 files changed, 203 insertions(+), 20 deletions(-)
 create mode 100644 src/main/java/frc/robot/subsystems/Vision.java
 create mode 100644 vendordeps/photonlib.json

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index bd1cf85..7f643b4 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -1,5 +1,7 @@
 package frc.robot;
 
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.RobotController;
@@ -20,11 +22,13 @@ public static enum Bot {
 	public static final String serialNumber;
 
 	/**
-	 * This code determines what bot is being deployed and sets constants accordingly.
+	 * This code determines what bot is being deployed and sets constants
+	 * accordingly.
 	 * 
 	 * Simulated bots cannot have a RoboRIO ID, so we must check if the bot is real.
 	 * If it isn't, load production config.
-	 * The production bot is always default, so if we do anything crazy to our bot during the tourney like switch the RoboRIO the code works.
+	 * The production bot is always default, so if we do anything crazy to our bot
+	 * during the tourney like switch the RoboRIO the code works.
 	 * 
 	 * @author Aceius E.
 	 */
@@ -220,4 +224,10 @@ public static class SwerveDrivetrainConstants {
 		public static final double MODULE_LOCATION_Y;
 		public static final double MODULE_LOCATION_X;
 	}
+
+	public static class VisionConstants {
+
+		public static final Transform3d CAMERA_POSE = new Transform3d(0.5, 0, 0.25, new Rotation3d());
+		public static final String CAMERA_NAME = "Arducam_OV9281_USB_Camera";
+	}
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 04af3b4..15862d4 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -1,8 +1,13 @@
 package frc.robot;
 
 import frc.robot.Constants.DriverConstants;
+import frc.robot.Constants.OperatorConstants;
 import frc.robot.Constants.SwerveDrivetrainConstants;
 import frc.robot.Constants.SwerveModuleConstants;
+import frc.robot.Constants.VisionConstants;
+import frc.robot.subsystems.SwerveDrivetrain;
+import frc.robot.subsystems.SwerveModule;
+import frc.robot.subsystems.Vision;
 import frc.robot.commands.Autos;
 import frc.robot.commands.DriverControl;
 import frc.robot.subsystems.SwerveDrivetrain;
@@ -19,6 +24,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.net.PortForwarder;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
@@ -26,8 +32,11 @@
 
 /**
  * This class is where the bulk of the robot should be declared.
- * Since Command-based is a "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot} periodic methods (other than the scheduler calls).
- * Instead, the structure of the robot (including subsystems, commands, and trigger mappings) should be declared here.
+ * Since Command-based is a "declarative" paradigm, very little robot logic
+ * should actually be handled in the {@link Robot} periodic methods (other than
+ * the scheduler calls).
+ * Instead, the structure of the robot (including subsystems, commands, and
+ * trigger mappings) should be declared here.
  */
 public class RobotContainer {
 
@@ -68,6 +77,8 @@ public class RobotContainer {
 
 	private final SendableChooser<Command> autoChooser = new SendableChooser<Command>();
 
+	private final Vision vision = new Vision(VisionConstants.CAMERA_NAME);
+
 	/**
 	 * The container for the robot. Contains subsystems, OI devices, and commands.
 	 */
@@ -78,6 +89,8 @@ public RobotContainer() {
 		configureBindings();
 
 		setUpDriveController();
+
+		PortForwarder.add(5800, "photonvision.local", 5800);
 	}
 
 	public void setUpDriveController() {
@@ -96,30 +109,27 @@ public void setUpDriveController() {
 			final CommandJoystick joystick = new CommandJoystick(genericHID.getPort());
 			control = new DriverControl(drivetrain,
 
-				new ChassisDriveInputs(
-					joystick::getX, joystick::getY, joystick::getTwist,
-					-1, -1, Constants.DriverConstants.DEAD_ZONE),
+					new ChassisDriveInputs(
+							joystick::getX, joystick::getY, joystick::getTwist,
+							-1, -1, Constants.DriverConstants.DEAD_ZONE),
+
+					new OptionButton(joystick, 2, ActivationMode.TOGGLE),
+					new OptionButton(joystick, 1, ActivationMode.HOLD),
+					new OptionButton(joystick, 3, ActivationMode.TOGGLE));
 
-				new OptionButton(joystick, 2, ActivationMode.TOGGLE),
-				new OptionButton(joystick, 1, ActivationMode.HOLD),
-				new OptionButton(joystick, 3, ActivationMode.TOGGLE)
-			);
-			
 			joystick.button(4).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
 
 		} else {
 			final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());
 			control = new DriverControl(drivetrain,
 
-				new ChassisDriveInputs(
-					xbox::getLeftX, xbox::getLeftY, xbox::getRightX,
-					+1, -1, Constants.DriverConstants.DEAD_ZONE),
-
-				new OptionButton(xbox::b, ActivationMode.TOGGLE),
-				new OptionButton(xbox::leftStick, ActivationMode.HOLD),
-				new OptionButton(xbox::povUp, ActivationMode.TOGGLE)
-			);
+					new ChassisDriveInputs(
+							xbox::getLeftX, xbox::getLeftY, xbox::getRightX,
+							+1, -1, Constants.DriverConstants.DEAD_ZONE),
 
+					new OptionButton(xbox::b, ActivationMode.TOGGLE),
+					new OptionButton(xbox::leftStick, ActivationMode.HOLD),
+					new OptionButton(xbox::povUp, ActivationMode.TOGGLE));
 
 		}
 
diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java
new file mode 100644
index 0000000..a6c7d14
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Vision.java
@@ -0,0 +1,106 @@
+package frc.robot.subsystems;
+
+import java.util.List;
+
+import org.photonvision.PhotonCamera;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+// How to make Subsystem (ignore image instructions, code is out of date, just look at written general instructions): https://compendium.readthedocs.io/en/latest/tasks/subsystems/subsystems.html
+// Command based programming: https://docs.wpilib.org/en/stable/docs/software/commandbased/what-is-command-based.html
+// Subsystem Documentation documentation: https://docs.wpilib.org/en/stable/docs/software/commandbased/subsystems.html
+
+/**
+ * 
+ */
+
+public class Vision extends SubsystemBase {
+    /** Constructor. Creates a new ExampleSubsystem. */
+    PhotonCamera camera;
+    
+    
+    PhotonTrackedTarget bestTag = null;
+    List<PhotonTrackedTarget> targets = null;
+    
+    public Vision(String cameraName) {
+        camera = new PhotonCamera(cameraName);
+
+    }
+
+    /**
+     * Gives Transform3d from the robot center to the desired target
+     * @return
+     */
+    public Transform3d getDistToTag() {
+        return bestTag.getBestCameraToTarget();
+    }
+
+    /**
+     * Gives Transform3d from robot center to the desired target
+     * @param tagID The fiducial ID of the desired April Tag
+     * @return Returns null if the tag cannot be found
+     * 
+     */
+    public Transform3d getDistToTag(int tagID) {
+        Transform3d dist = null;
+        for(int i = 0; i < targets.size(); i++)
+        {
+            if (targets.get(i).getFiducialId() == tagID) {
+                dist = targets.get(i).getBestCameraToTarget();
+                break;
+            }
+        }
+        
+        return dist;
+    }
+    /**
+     * This method is called periodically by the CommandScheduler, about every 20ms.
+     * It should be used for updating subsystem-specific state that you don't want to offload to a Command.
+     * Try to avoid "doing to much" in this method (for example no driver control here).
+     */
+    @Override
+    public void periodic() {
+        PhotonPipelineResult result = camera.getLatestResult();
+        // This method will be called once per scheduler run
+        bestTag = result.getBestTarget();
+        targets = result.getTargets();
+        if(bestTag != null) {
+            SmartDashboard.putNumber("Tag ID", bestTag.getFiducialId());
+            Transform3d tagPose = bestTag.getBestCameraToTarget();
+            Rotation3d tagRot = tagPose.getRotation();
+            // SmartDashboard.putNumberArray("Tag X Y Z", new double[] {tagPose.getX(), tagPose.getY(), tagPose.getZ()});
+            // SmartDashboard.putNumberArray("Tag Yaw, Pitch, Roll", new double[] {
+            //     Units.radiansToDegrees(tagRot.getZ()),
+            //     Units.radiansToDegrees(tagRot.getY()),
+            //     Units.radiansToDegrees(tagRot.getX())});
+            SmartDashboard.putNumber("Tag Pose X", tagPose.getX());
+            SmartDashboard.putNumber("Tag Pose Y", tagPose.getY());
+            SmartDashboard.putNumber("Tag Pose Z", tagPose.getZ());
+            SmartDashboard.putNumber("Tag Pose Yaw", tagRot.getZ());
+            SmartDashboard.putNumber("Tag Pose Pitch", tagRot.getY());
+            SmartDashboard.putNumber("Tag Pose Roll", tagRot.getX());
+            SmartDashboard.putNumber("Tag Yaw", bestTag.getYaw());
+            SmartDashboard.putNumber("Tag Pitch", bestTag.getPitch());
+        } else {
+            SmartDashboard.putNumber("Tag ID", -1);
+            SmartDashboard.putNumber("Tag Pose X", -1);
+            SmartDashboard.putNumber("Tag Pose Y", -1);
+            SmartDashboard.putNumber("Tag Pose Z", -1);
+            SmartDashboard.putNumber("Tag Pose Yaw", -1);
+            SmartDashboard.putNumber("Tag Pose Pitch", -1);
+            SmartDashboard.putNumber("Tag Pose Roll", -1);
+            SmartDashboard.putNumber("Tag Yaw", 0);
+            SmartDashboard.putNumber("Tag Pitch", 0);
+            // SmartDashboard.putNumberArray("Tag X Y Z", new double[] {0, 0, 0});
+            // SmartDashboard.putNumberArray("Tag Yaw, Pitch, Roll", new double[] {0, 0, 0});
+        }
+
+        
+    }
+}
diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json
new file mode 100644
index 0000000..46211fc
--- /dev/null
+++ b/vendordeps/photonlib.json
@@ -0,0 +1,57 @@
+{
+    "fileName": "photonlib.json",
+    "name": "photonlib",
+    "version": "v2024.2.4",
+    "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+    "frcYear": "2024",
+    "mavenUrls": [
+        "https://maven.photonvision.org/repository/internal",
+        "https://maven.photonvision.org/repository/snapshots"
+    ],
+    "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json",
+    "jniDependencies": [],
+    "cppDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photonlib-cpp",
+            "version": "v2024.2.4",
+            "libName": "photonlib",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-cpp",
+            "version": "v2024.2.4",
+            "libName": "photontargeting",
+            "headerClassifier": "headers",
+            "sharedLibrary": true,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxathena",
+                "linuxx86-64",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "javaDependencies": [
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photonlib-java",
+            "version": "v2024.2.4"
+        },
+        {
+            "groupId": "org.photonvision",
+            "artifactId": "photontargeting-java",
+            "version": "v2024.2.4"
+        }
+    ]
+}
\ No newline at end of file

From 943a30c40952d69adda15278ab3ee4b5a6000af5 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Sat, 10 Feb 2024 15:17:46 -0800
Subject: [PATCH 42/99] switch x and y to fix it

---
 src/main/java/frc/robot/RobotContainer.java   |  4 +--
 .../frc/robot/commands/DriveToPoseBase.java   | 17 ++++++++++-
 .../java/frc/robot/commands/FollowTag.java    | 30 +++++++++++--------
 3 files changed, 35 insertions(+), 16 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 04af3b4..ed4aff4 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -97,7 +97,7 @@ public void setUpDriveController() {
 			control = new DriverControl(drivetrain,
 
 				new ChassisDriveInputs(
-					joystick::getX, joystick::getY, joystick::getTwist,
+					joystick::getY, joystick::getX, joystick::getTwist,
 					-1, -1, Constants.DriverConstants.DEAD_ZONE),
 
 				new OptionButton(joystick, 2, ActivationMode.TOGGLE),
@@ -112,7 +112,7 @@ public void setUpDriveController() {
 			control = new DriverControl(drivetrain,
 
 				new ChassisDriveInputs(
-					xbox::getLeftX, xbox::getLeftY, xbox::getRightX,
+					xbox::getLeftY, xbox::getLeftX, xbox::getRightX,
 					+1, -1, Constants.DriverConstants.DEAD_ZONE),
 
 				new OptionButton(xbox::b, ActivationMode.TOGGLE),
diff --git a/src/main/java/frc/robot/commands/DriveToPoseBase.java b/src/main/java/frc/robot/commands/DriveToPoseBase.java
index 594702a..82a40f3 100644
--- a/src/main/java/frc/robot/commands/DriveToPoseBase.java
+++ b/src/main/java/frc/robot/commands/DriveToPoseBase.java
@@ -14,6 +14,8 @@ public class DriveToPoseBase extends Command {
 	private final SwerveDrivetrain drivetrain;
 	private final PIDController xController, yController, rotationController;
 
+	private boolean stopped = false;
+
 	/**
 	 * Create a new DriveToPose command. Tries to drive to a set Pose based on odometry.
 	 * 
@@ -54,20 +56,32 @@ public DriveToPoseBase(SwerveDrivetrain drivetrain) {
 	}
 
 	/**
-	 * 
+	 * Set PID controller setpoints to get ot pose
 	 * 
 	 * @param targetPose Pose that robot will drive to once command is scheduled
 	 */
 	public void setDesiredPosition(Pose2d targetPose) {
+		stopped = false;
 		xController.setSetpoint(targetPose.getX());
 		yController.setSetpoint(targetPose.getY());
 		rotationController.setSetpoint(targetPose.getRotation().getRadians());
 	}
 
+	/**
+	 * Get current drivetrain pose
+	 * 
+	 * @return current drivetrain pose
+	 */
 	public Pose2d getPosition() {
 		return drivetrain.getPosition();
 	}
 
+	/** Stop from setting new states, can be started by calling setDesiredPositions again */
+	public void stop() {
+		stopped = true;
+		drivetrain.stop();
+	}
+
 	/** Put all swerve modules in default positions */
 	@Override
 	public void initialize() {
@@ -76,6 +90,7 @@ public void initialize() {
 
 	@Override
 	public void execute() {
+		if (stopped) return;
 
 		// Get our current pose
 		final Pose2d measuredPosition = getPosition();
diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java
index c3a767a..c6d0193 100644
--- a/src/main/java/frc/robot/commands/FollowTag.java
+++ b/src/main/java/frc/robot/commands/FollowTag.java
@@ -41,8 +41,7 @@ public FollowTag(SwerveDrivetrain drivetrain, int tagID, Transform2d targetDista
 	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover
 	 *                            tag, set to -1 to never finish
 	 */
-	public FollowTag(SwerveDrivetrain drivetrain, int tagID, Translation2d targetDistanceToTag,
-			int loseTagAfterSeconds) {
+	public FollowTag(SwerveDrivetrain drivetrain, int tagID, Translation2d targetDistanceToTag, int loseTagAfterSeconds) {
 		this(drivetrain, tagID, new Transform2d(targetDistanceToTag, new Rotation2d()), loseTagAfterSeconds);
 	}
 
@@ -50,18 +49,23 @@ public FollowTag(SwerveDrivetrain drivetrain, int tagID, Translation2d targetDis
 	public void execute() {
 
 		// Sudo code, assume distance from front center of robot
-		final Transform3d tagPosition3d = new Transform3d(tagID * 5, 0, 0, new Rotation3d());
+		final Transform3d tagPosition3d = new Transform3d();
 
-		// https://docs.photonvision.org/en/latest/docs/apriltag-pipelines/coordinate-systems.html
-		// https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/geometry/Rotation3d.html#getZ()
-		final Transform2d tagPosition = new Transform2d(
-				tagPosition3d.getZ(),
-				tagPosition3d.getX(),
-				Rotation2d.fromRadians(-tagPosition3d.getRotation().getZ()));
-
-		final Transform2d driveTransform = tagPosition.plus(targetDistance.inverse());
-
-		setDesiredPosition(getPosition().plus(driveTransform));
+		if (tagPosition3d != null) {
+			// https://docs.photonvision.org/en/latest/docs/apriltag-pipelines/coordinate-systems.html
+			// https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/geometry/Rotation3d.html#getZ()
+			final Transform2d tagPosition = new Transform2d(
+					tagPosition3d.getZ(),
+					tagPosition3d.getX(),
+					Rotation2d.fromRadians(-tagPosition3d.getRotation().getZ()));
+	
+			final Transform2d driveTransform = tagPosition.plus(targetDistance.inverse());
+	
+			setDesiredPosition(getPosition().plus(driveTransform));
+		}
+		else {
+			stop();
+		}
 
 		super.execute();
 	}

From 54f75807f599ad7b3dd0c272fc683ea64d965b27 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Sat, 10 Feb 2024 15:42:24 -0800
Subject: [PATCH 43/99] start using vision subsystem

---
 src/main/java/frc/robot/commands/FollowTag.java | 13 ++++++++-----
 src/main/java/frc/robot/subsystems/Vision.java  |  1 -
 2 files changed, 8 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java
index c6d0193..6cdcaea 100644
--- a/src/main/java/frc/robot/commands/FollowTag.java
+++ b/src/main/java/frc/robot/commands/FollowTag.java
@@ -1,14 +1,15 @@
 package frc.robot.commands;
 
 import frc.robot.subsystems.SwerveDrivetrain;
+import frc.robot.subsystems.Vision;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
 
 /** Command to automatically drive a follow a tag a certain translation away */
 public class FollowTag extends DriveToPoseBase {
+	private final Vision camera;
 	private final int tagID;
 	private final Transform2d targetDistance;
 
@@ -24,10 +25,12 @@ public class FollowTag extends DriveToPoseBase {
 	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover
 	 *                            tag, set to -1 to never finish
 	 */
-	public FollowTag(SwerveDrivetrain drivetrain, int tagID, Transform2d targetDistanceToTag, int loseTagAfterSeconds) {
+	public FollowTag(SwerveDrivetrain drivetrain, Vision camera, int tagID, Transform2d targetDistanceToTag, int loseTagAfterSeconds) {
 		super(drivetrain);
 
+		this.camera = camera;
 		this.tagID = tagID;
+
 		this.targetDistance = targetDistanceToTag;
 		this.loseTagAfterSeconds = loseTagAfterSeconds;
 	}
@@ -41,15 +44,15 @@ public FollowTag(SwerveDrivetrain drivetrain, int tagID, Transform2d targetDista
 	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover
 	 *                            tag, set to -1 to never finish
 	 */
-	public FollowTag(SwerveDrivetrain drivetrain, int tagID, Translation2d targetDistanceToTag, int loseTagAfterSeconds) {
-		this(drivetrain, tagID, new Transform2d(targetDistanceToTag, new Rotation2d()), loseTagAfterSeconds);
+	public FollowTag(SwerveDrivetrain drivetrain, Vision camera, int tagID, Translation2d targetDistanceToTag, int loseTagAfterSeconds) {
+		this(drivetrain, camera, tagID, new Transform2d(targetDistanceToTag, new Rotation2d()), loseTagAfterSeconds);
 	}
 
 	@Override
 	public void execute() {
 
 		// Sudo code, assume distance from front center of robot
-		final Transform3d tagPosition3d = new Transform3d();
+		final Transform3d tagPosition3d = camera.getDistToTag(tagID);
 
 		if (tagPosition3d != null) {
 			// https://docs.photonvision.org/en/latest/docs/apriltag-pipelines/coordinate-systems.html
diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java
index a6c7d14..cfd8671 100644
--- a/src/main/java/frc/robot/subsystems/Vision.java
+++ b/src/main/java/frc/robot/subsystems/Vision.java
@@ -8,7 +8,6 @@
 
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 

From fb3cbe1c23edda1cefa3e5dd30387229835ab9e3 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Sun, 11 Feb 2024 11:28:00 -0800
Subject: [PATCH 44/99] clean up ChassisDriveInputs

---
 src/main/java/frc/robot/RobotContainer.java   | 12 ++++--
 .../frc/robot/utils/ChassisDriveInputs.java   | 41 +++++++++++--------
 2 files changed, 31 insertions(+), 22 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index f11a289..a39371c 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -107,8 +107,10 @@ public void setUpDriveController() {
 			control = new DriverControl(drivetrain,
 
 				new ChassisDriveInputs(
-					joystick::getY, joystick::getX, joystick::getTwist,
-					-1, -1, Constants.DriverConstants.DEAD_ZONE),
+					joystick::getY, -1,
+					joystick::getX, -1,
+					joystick::getTwist, -1,
+					Constants.DriverConstants.DEAD_ZONE),
 
 					new OptionButton(joystick, 2, ActivationMode.TOGGLE),
 					new OptionButton(joystick, 1, ActivationMode.HOLD),
@@ -121,8 +123,10 @@ public void setUpDriveController() {
 			control = new DriverControl(drivetrain,
 
 				new ChassisDriveInputs(
-					xbox::getLeftY, xbox::getLeftX, xbox::getRightX,
-					+1, -1, Constants.DriverConstants.DEAD_ZONE),
+					xbox::getLeftY, +1,
+					xbox::getLeftX, +1,
+					xbox::getRightX, -1,
+					Constants.DriverConstants.DEAD_ZONE),
 
 				new OptionButton(xbox::b, ActivationMode.TOGGLE),
 				new OptionButton(xbox::leftStick, ActivationMode.HOLD),
diff --git a/src/main/java/frc/robot/utils/ChassisDriveInputs.java b/src/main/java/frc/robot/utils/ChassisDriveInputs.java
index 04dfeff..5f249d5 100644
--- a/src/main/java/frc/robot/utils/ChassisDriveInputs.java
+++ b/src/main/java/frc/robot/utils/ChassisDriveInputs.java
@@ -5,45 +5,50 @@
 /** Class that stores supplies for main controls of ChassisSpeeds */
 public class ChassisDriveInputs {
 
-	private final Supplier<Double> xSupplier;
-	private final Supplier<Double> ySupplier;
-	private final Supplier<Double> rotationSupplier;
+	private final Supplier<Double> xSupplier, ySupplier, rotationSupplier;
+
+	private final double xCoefficient, yCoefficient, rotationCoefficient;
 
 	private final double deadzone;
 
-	private final double translationCoefficient;
-	private final double rotationCoefficient;
 
 	/**
 	 * Create a new ChassisDriveInputs
 	 * 
-	 * @param getX Get the value mapped to forward
-	 * @param getY Get the value mapped to left
-	 * @param getRotation Get the value mapped to rotation
-	 * @param translationCoefficient Coefficient that forward (x) and left (Y) are multiped by
+	 * @param getForward Get the value mapped to X, -1 full backward to +1 full forward
+	 * @param forwardCoefficient Coefficient that forward (X) multiped by
+	 * 
+	 * @param getLeft Get the value mapped to Y, -1 full right to +1 full left
+	 * @param leftCoefficient Coefficient that forward left (Y) are multiped by
+	 * 
+	 * @param getRotation Get the value mapped to rotation, -1 full clock
 	 * @param rotationCoefficient Coefficient that rotation is multiplied by
+	 * 
 	 * @param deadzone Deadzone for all axises
 	 */
 	public ChassisDriveInputs(
-			Supplier<Double> getX, Supplier<Double> getY, Supplier<Double> getRotation,
-			double translationCoefficient, double rotationCoefficient, double deadzone) {
+			Supplier<Double> getForward, double forwardCoefficient, 
+			Supplier<Double> getLeft, double leftCoefficient,
+			Supplier<Double> getRotation, double rotationCoefficient,
+			double deadzone) {
 
-		this.xSupplier = getX;
-		this.ySupplier = getY;
+		this.ySupplier = getForward;
+		this.xSupplier = getLeft;
 		this.rotationSupplier = getRotation;
 
-		this.deadzone = deadzone;
-
-		this.translationCoefficient = translationCoefficient;
+		this.yCoefficient = forwardCoefficient;
+		this.xCoefficient = leftCoefficient;
 		this.rotationCoefficient = rotationCoefficient;
+
+		this.deadzone = deadzone;
 	}
 
 	public double getX() {
-		return applyJoystickDeadzone(xSupplier.get(), deadzone) * translationCoefficient;
+		return applyJoystickDeadzone(xSupplier.get(), deadzone) * xCoefficient;
 	}
 
 	public double getY() {
-		return applyJoystickDeadzone(ySupplier.get(), deadzone) * translationCoefficient;
+		return applyJoystickDeadzone(ySupplier.get(), deadzone) * yCoefficient;
 	}
 
 	public double getRotation() {

From b8316e7f13370920a46aa2ee19671498604b55d0 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Mon, 12 Feb 2024 14:05:50 -0800
Subject: [PATCH 45/99] update how drivetrain gets to position to account for
 vision

---
 src/main/java/frc/robot/Constants.java        |   2 +-
 src/main/java/frc/robot/RobotContainer.java   |   6 +-
 src/main/java/frc/robot/commands/Autos.java   |   6 +-
 .../java/frc/robot/commands/DriveToPose.java  |  27 ++-
 .../frc/robot/commands/DriveToPoseBase.java   | 124 ------------
 .../frc/robot/commands/DriveTransform.java    |  19 +-
 .../java/frc/robot/commands/FollowTag.java    |  70 ++++---
 .../robot/subsystems/SwerveDrivetrain.java    |  70 ++++++-
 .../java/frc/robot/subsystems/Vision.java     | 178 +++++++++---------
 9 files changed, 250 insertions(+), 252 deletions(-)
 delete mode 100644 src/main/java/frc/robot/commands/DriveToPoseBase.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 7f643b4..6e15b83 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -33,7 +33,7 @@ public static enum Bot {
 	 * @author Aceius E.
 	 */
 	static {
-		serialNumber = RobotBase.isReal() ? RobotController.getSerialNumber() : "Simulation";
+		serialNumber = RobotBase.isReal() ? RobotController.getSerialNumber() : "simulation";
 
 		switch (serialNumber) {
 			case "03282B00": // Wood Bot Serial Number
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index a39371c..02eb759 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -74,13 +74,14 @@ public class RobotContainer {
 
 	private final SendableChooser<Command> autoChooser = new SendableChooser<Command>();
 
-	private final Vision vision = new Vision(VisionConstants.CAMERA_NAME);
+	private final Vision vision = new Vision(VisionConstants.CAMERA_NAME, Constants.VisionConstants.CAMERA_POSE);
 
 	/**
 	 * The container for the robot. Contains subsystems, OI devices, and commands.
 	 */
 	public RobotContainer() {
 		autoChooser.setDefaultOption("Testing Auto", Autos.testingAuto(drivetrain));
+		autoChooser.addOption("Follow Tag", Autos.tagFollowAuto(drivetrain, vision, 1));
 		SmartDashboard.putData("Auto Chooser", autoChooser);
 
 		configureBindings();
@@ -116,7 +117,8 @@ public void setUpDriveController() {
 					new OptionButton(joystick, 1, ActivationMode.HOLD),
 					new OptionButton(joystick, 3, ActivationMode.TOGGLE));
 
-			joystick.button(4).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
+			joystick.button(10).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
+			joystick.button(11).onTrue(Commands.run(drivetrain::toDefaultStates, drivetrain));
 
 		} else {
 			final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());
diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java
index e0de80f..270c6ca 100644
--- a/src/main/java/frc/robot/commands/Autos.java
+++ b/src/main/java/frc/robot/commands/Autos.java
@@ -1,7 +1,7 @@
 package frc.robot.commands;
 
 import frc.robot.subsystems.SwerveDrivetrain;
-
+import frc.robot.subsystems.Vision;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -17,6 +17,10 @@ public static Command testingAuto(SwerveDrivetrain drivetrain) {
 		);
 	}
 
+	public static Command tagFollowAuto(SwerveDrivetrain drivetrain, Vision camera, Integer tagId) {
+		return new FollowTag(drivetrain, camera, new Translation2d(1, 0), tagId, null);
+	}
+
 	public static Command startingAuto(Arm arm, SwerveDrivetrain drivetrain, boolean invertY) {
 
 		// assumes start position in corner
diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java
index 8dee1ed..e70ec3f 100644
--- a/src/main/java/frc/robot/commands/DriveToPose.java
+++ b/src/main/java/frc/robot/commands/DriveToPose.java
@@ -1,10 +1,13 @@
 package frc.robot.commands;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.SwerveDrivetrain;
 
 /** Command to automatically drive to a certain Pose on the field */
-public class DriveToPose extends DriveToPoseBase {
+public class DriveToPose extends Command {
+	private final SwerveDrivetrain drivetrain;
+	private final Pose2d targetPosition;
 
     /**
      * Create a new DriveToPose command. Tries to drive to a set Pose based on odometry 
@@ -19,9 +22,23 @@ public class DriveToPose extends DriveToPoseBase {
      * @param targetPosition the pose the robot is trying to achieve
      */
     public DriveToPose(SwerveDrivetrain drivetrain, Pose2d targetPosition) {
-        super(drivetrain);
-
-		// Set desired pose so once we initialize we go there
-        setDesiredPosition(targetPosition);
+		this.drivetrain = drivetrain;
+		this.targetPosition = targetPosition;
     }
+
+	@Override
+	public void initialize() {
+		drivetrain.setDesiredPosition(targetPosition);
+	}
+
+	@Override
+	public boolean isFinished() {
+		return drivetrain.isAtDesiredPosition();
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		drivetrain.clearDesiredPosition();
+		drivetrain.stop();
+	}
 }
diff --git a/src/main/java/frc/robot/commands/DriveToPoseBase.java b/src/main/java/frc/robot/commands/DriveToPoseBase.java
deleted file mode 100644
index 82a40f3..0000000
--- a/src/main/java/frc/robot/commands/DriveToPoseBase.java
+++ /dev/null
@@ -1,124 +0,0 @@
-package frc.robot.commands;
-
-import frc.robot.subsystems.SwerveDrivetrain;
-import edu.wpi.first.wpilibj2.command.Command;
-
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.Constants.RobotMovementConstants;
-
-/** Base Command for child commands that need to automatically drive to a certain Pose on the field */
-public class DriveToPoseBase extends Command {
-	private final SwerveDrivetrain drivetrain;
-	private final PIDController xController, yController, rotationController;
-
-	private boolean stopped = false;
-
-	/**
-	 * Create a new DriveToPose command. Tries to drive to a set Pose based on odometry.
-	 * 
-     * <p>This drives relative to the robot starting position,
-	 * so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
-     * where forward is whatever direction the robot started in</p>
-     * 
-     * <p>The last place the drivetrain position was reset counts as the starting position</p>
-	 * 
-	 * @param drivetrain the drivetrain of the robot
-	 */
-	public DriveToPoseBase(SwerveDrivetrain drivetrain) {
-
-		// Save drivetrain
-		this.drivetrain = drivetrain;
-
-		// Setup all PID controllers
-		xController = new PIDController(
-				RobotMovementConstants.TRANSLATION_PID_P,
-				RobotMovementConstants.TRANSLATION_PID_I,
-				RobotMovementConstants.TRANSLATION_PID_D);
-		xController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
-
-		yController = new PIDController(
-				RobotMovementConstants.TRANSLATION_PID_P,
-				RobotMovementConstants.TRANSLATION_PID_I,
-				RobotMovementConstants.TRANSLATION_PID_D);
-		yController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
-
-		rotationController = new PIDController(
-				RobotMovementConstants.ROTATION_PID_P,
-				RobotMovementConstants.ROTATION_PID_I,
-				RobotMovementConstants.ROTATION_PID_D);
-		rotationController.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
-
-		// Add drivetrain as a requirement so no other commands try to use it
-		addRequirements(this.drivetrain);
-	}
-
-	/**
-	 * Set PID controller setpoints to get ot pose
-	 * 
-	 * @param targetPose Pose that robot will drive to once command is scheduled
-	 */
-	public void setDesiredPosition(Pose2d targetPose) {
-		stopped = false;
-		xController.setSetpoint(targetPose.getX());
-		yController.setSetpoint(targetPose.getY());
-		rotationController.setSetpoint(targetPose.getRotation().getRadians());
-	}
-
-	/**
-	 * Get current drivetrain pose
-	 * 
-	 * @return current drivetrain pose
-	 */
-	public Pose2d getPosition() {
-		return drivetrain.getPosition();
-	}
-
-	/** Stop from setting new states, can be started by calling setDesiredPositions again */
-	public void stop() {
-		stopped = true;
-		drivetrain.stop();
-	}
-
-	/** Put all swerve modules in default positions */
-	@Override
-	public void initialize() {
-		drivetrain.toDefaultStates();
-	}
-
-	@Override
-	public void execute() {
-		if (stopped) return;
-
-		// Get our current pose
-		final Pose2d measuredPosition = getPosition();
-
-		// Put current pose position on SmartDashboard
-		SmartDashboard.putNumber("PoseY", measuredPosition.getY());
-		SmartDashboard.putNumber("PoseX", measuredPosition.getX());
-		SmartDashboard.putNumber("PoseDegrees", measuredPosition.getRotation().getDegrees());
-
-		// Calculate our robot speeds with the PID controllers
-		final ChassisSpeeds speeds = new ChassisSpeeds(
-				xController.calculate(measuredPosition.getX()),
-				yController.calculate(measuredPosition.getY()),
-				rotationController.calculate(measuredPosition.getRotation().getRadians()));
-
-		// Set those speeds
-		drivetrain.setDesiredState(speeds);
-	}
-
-	/** Finish once all controllers are within tolerance */
-	@Override
-	public boolean isFinished() {
-		return xController.atSetpoint() && yController.atSetpoint() && rotationController.atSetpoint();
-	}
-
-	/** Stop all swerve modules at end */
-	@Override
-	public void end(boolean interrupted) {
-		drivetrain.stop();
-	}
-}
diff --git a/src/main/java/frc/robot/commands/DriveTransform.java b/src/main/java/frc/robot/commands/DriveTransform.java
index 0f8ffb2..938be08 100644
--- a/src/main/java/frc/robot/commands/DriveTransform.java
+++ b/src/main/java/frc/robot/commands/DriveTransform.java
@@ -3,10 +3,12 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.SwerveDrivetrain;
 
 /** Command to automatically drive a certain transform */
-public class DriveTransform extends DriveToPoseBase {
+public class DriveTransform extends Command {
+	private final SwerveDrivetrain drivetrain;
 	private final Transform2d transform;
 
 	/**
@@ -22,7 +24,7 @@ public class DriveTransform extends DriveToPoseBase {
 	 *                   position to get target pose
 	 */
 	public DriveTransform(SwerveDrivetrain drivetrain, Transform2d transform) {
-		super(drivetrain);
+		this.drivetrain = drivetrain;
 		this.transform = transform;
 	}
 
@@ -46,6 +48,17 @@ public DriveTransform(SwerveDrivetrain drivetrain, Translation2d translation, Ro
 
 	@Override
 	public void initialize() {
-		setDesiredPosition(getPosition().plus(transform));
+		drivetrain.setDesiredPosition(drivetrain.getPosition().plus(transform));
+	}
+
+	@Override
+	public boolean isFinished() {
+		return drivetrain.isAtDesiredPosition();
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		drivetrain.clearDesiredPosition();
+		drivetrain.stop();
 	}
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java
index 6cdcaea..298d6f9 100644
--- a/src/main/java/frc/robot/commands/FollowTag.java
+++ b/src/main/java/frc/robot/commands/FollowTag.java
@@ -2,79 +2,97 @@
 
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.Vision;
+
+import org.photonvision.targeting.PhotonTrackedTarget;
+
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
 
 /** Command to automatically drive a follow a tag a certain translation away */
-public class FollowTag extends DriveToPoseBase {
+public class FollowTag extends Command {
+	private final SwerveDrivetrain drivetrain;
+
 	private final Vision camera;
-	private final int tagID;
+	private final Integer tagID;
 	private final Transform2d targetDistance;
 
-	private final double loseTagAfterSeconds;
+	private final Double loseTagAfterSeconds;
 	private double secondsSinceTagLastSeen;
 
 	/**
 	 * Create a new FollowTag command. Tries to follow a tag while staying a certain distance away.
 	 * 
 	 * @param drivetrain          the drivetrain of the robot
-	 * @param tagID               the numerical ID of the the tag to follow
+	 * @param camera          	  the vision subsystem of the robot
+	 * @param tagID               the numerical ID of the the tag to follow, null for whatever best is
 	 * @param targetDistanceToTag the target distance away from the tag to be
-	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover
-	 *                            tag, set to -1 to never finish
+	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover tag, set to null to never finish
 	 */
-	public FollowTag(SwerveDrivetrain drivetrain, Vision camera, int tagID, Transform2d targetDistanceToTag, int loseTagAfterSeconds) {
-		super(drivetrain);
+	public FollowTag(SwerveDrivetrain drivetrain, Vision camera, Transform2d targetDistanceToTag, Integer tagID, Double loseTagAfterSeconds) {
+		this.drivetrain = drivetrain;
 
 		this.camera = camera;
 		this.tagID = tagID;
 
 		this.targetDistance = targetDistanceToTag;
 		this.loseTagAfterSeconds = loseTagAfterSeconds;
+
+		addRequirements(drivetrain);
 	}
 
 	/**
 	 * Create a new FollowTag command. Tries to follow a tag while staying a certain distance away.
 	 * 
 	 * @param drivetrain          the drivetrain of the robot
-	 * @param tagID               the numerical ID of the the tag to follow
+	 * @param camera          	  the vision subsystem of the robot
+	 * @param tagID               the numerical ID of the the tag to follow, null for whatever best is
 	 * @param targetDistanceToTag the target distance away from the tag to be
-	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover
-	 *                            tag, set to -1 to never finish
+	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover tag, set to null to never finish
 	 */
-	public FollowTag(SwerveDrivetrain drivetrain, Vision camera, int tagID, Translation2d targetDistanceToTag, int loseTagAfterSeconds) {
-		this(drivetrain, camera, tagID, new Transform2d(targetDistanceToTag, new Rotation2d()), loseTagAfterSeconds);
+	public FollowTag(SwerveDrivetrain drivetrain, Vision camera, Translation2d targetDistanceToTag, Integer tagID, Double loseTagAfterSeconds) {
+		this(drivetrain, camera, new Transform2d(targetDistanceToTag, new Rotation2d()), tagID, loseTagAfterSeconds);
+	}
+
+	@Override
+	public void initialize() {
+		secondsSinceTagLastSeen = 0;
+		drivetrain.toDefaultStates();
 	}
 
 	@Override
 	public void execute() {
 
-		// Sudo code, assume distance from front center of robot
-		final Transform3d tagPosition3d = camera.getDistToTag(tagID);
+		final PhotonTrackedTarget tag = (tagID == null) ? camera.getDistToTag() : camera.getDistToTag(tagID);
+
+		if (tag == null) {
+			secondsSinceTagLastSeen += TimedRobot.kDefaultPeriod;
+
+			drivetrain.stop();
+		} else {
+			final Transform3d tagPosition3d = camera.getDistanceToTarget(tag);
+
+			secondsSinceTagLastSeen = 0;
 
-		if (tagPosition3d != null) {
 			// https://docs.photonvision.org/en/latest/docs/apriltag-pipelines/coordinate-systems.html
 			// https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/geometry/Rotation3d.html#getZ()
 			final Transform2d tagPosition = new Transform2d(
 					tagPosition3d.getZ(),
 					tagPosition3d.getX(),
-					Rotation2d.fromRadians(-tagPosition3d.getRotation().getZ()));
-	
+					Rotation2d.fromRadians(-tag.getYaw()));
+
 			final Transform2d driveTransform = tagPosition.plus(targetDistance.inverse());
-	
-			setDesiredPosition(getPosition().plus(driveTransform));
-		}
-		else {
-			stop();
-		}
 
-		super.execute();
+			drivetrain.setDesiredPosition(drivetrain.getPosition().plus(driveTransform));
+		}
 	}
 
 	@Override
 	public boolean isFinished() {
-		return (loseTagAfterSeconds != -1) && (secondsSinceTagLastSeen < loseTagAfterSeconds);
+		drivetrain.clearDesiredPosition();
+		return (loseTagAfterSeconds != null) && (secondsSinceTagLastSeen < loseTagAfterSeconds);
 	}
 }
diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index 9fffd52..8169f84 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -7,6 +7,7 @@
 
 import com.kauailabs.navx.frc.AHRS;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
@@ -18,6 +19,7 @@
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.RobotMovementConstants;
 
 /**
  * Subsystem for full drive train of robot. Contains 4 {@link SwerveModule}
@@ -61,6 +63,17 @@ public class SwerveDrivetrain extends SubsystemBase {
 	 */
 	private Pose2d pose;
 
+	/**
+	 * Desired pose of robot. The desired pose is the the X, Y and Rotation the robot wants to be in, relative to the last reset.
+	 * It can be set to null to not have any desired pose.
+	 * 
+	 * @see https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/pose.html#pose
+	 */
+	private Pose2d desiredPose;
+
+	/** The PID controller to get robot to desired pose */
+	private final PIDController xController, yController, rotationController;
+
 	/**
 	 * Constructor the drivetrain subsystem.
 	 * 
@@ -99,6 +112,16 @@ public SwerveDrivetrain(AHRS gyro,
 		for (SwerveModule module : modules) {
 			addChild(module.getName(), module);
 		}
+
+		// set up PID controllers for desired pose
+		xController = new PIDController(RobotMovementConstants.TRANSLATION_PID_P, RobotMovementConstants.TRANSLATION_PID_I, RobotMovementConstants.TRANSLATION_PID_D);
+		xController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
+
+		yController = new PIDController(RobotMovementConstants.TRANSLATION_PID_P, RobotMovementConstants.TRANSLATION_PID_I, RobotMovementConstants.TRANSLATION_PID_D);
+		yController.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
+
+		rotationController = new PIDController(RobotMovementConstants.ROTATION_PID_P, RobotMovementConstants.ROTATION_PID_I, RobotMovementConstants.ROTATION_PID_D);
+		rotationController.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
 	}
 
 	// --- Pose Related Methods ---
@@ -113,6 +136,17 @@ public void periodic() {
 		pose = odometry.update(
 				getHeading(),
 				getWheelPositions());
+		
+		if (desiredPose != null) {
+			// Calculate our robot speeds with the PID controllers
+			final ChassisSpeeds speeds = new ChassisSpeeds(
+					xController.calculate(pose.getX(), desiredPose.getX()),
+					yController.calculate(pose.getY(), desiredPose.getY()),
+					rotationController.calculate(pose.getRotation().getRadians(), desiredPose.getRotation().getRadians()));
+
+			// Set those speeds
+			setDesiredState(speeds);
+		}
 	}
 
 	/**
@@ -135,20 +169,50 @@ public void resetPosition() {
 				pose);
 	}
 
+	/**
+     * <p>This drives relative to the robot starting position,
+	 * so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
+     * where forward is whatever direction the robot started in</p>
+     * 
+     * <p>The last place the drivetrain position was reset counts as the starting position</p>
+	 *
+	 * @param desiredPose the pose the robot will try to drive to
+	 */
+	public void setDesiredPosition(Pose2d desiredPose) {
+		this.desiredPose = desiredPose;
+	}
+
+	/** Sets desired position to null, stops robot from continue to try and get to the last set pose  */
+	public void clearDesiredPosition() {
+		setDesiredPosition(null);
+	}
+
+	/**
+	 * Checks whether drivetrain is at the desired pose
+	 * 
+	 * @return are all drive PID controllers within tolerance of their setpoints
+	 */
+	public boolean isAtDesiredPosition() {
+		return xController.atSetpoint() && yController.atSetpoint() && rotationController.atSetpoint();
+	}
+	
 	// --- Action Methods ---
 
-	/** Stop all swerve modules */
+	/** Stop all swerve modules, clears desired position */
 	public void stop() {
+		clearDesiredPosition();
 		modulesMap(SwerveModule::stop);
 	}
 
-	/** Put all swerve modules to default state, facing forward and staying still */
+	/** Put all swerve modules to default state, facing forward and staying still. Also clears desired position. */
 	public void toDefaultStates() {
+		clearDesiredPosition();
 		modulesMap(SwerveModule::toDefaultState);
 	}
 
-	/** Put all swerve modules to inward state, making the swerve modules face in a X pattern. This prevents robot from slipping around. */
+	/** Put all swerve modules to inward state, making the swerve modules face in a X pattern. This prevents robot from slipping around. Also clears desired position */
 	public void brakeMode() {
+		clearDesiredPosition();
 		modulesMap(SwerveModule::toInwardPosition);
 	}
 
diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java
index cfd8671..cc0ce85 100644
--- a/src/main/java/frc/robot/subsystems/Vision.java
+++ b/src/main/java/frc/robot/subsystems/Vision.java
@@ -1,6 +1,6 @@
 package frc.robot.subsystems;
 
-import java.util.List;
+import java.util.ArrayList;
 
 import org.photonvision.PhotonCamera;
 import org.photonvision.targeting.PhotonPipelineResult;
@@ -8,6 +8,7 @@
 
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
@@ -15,91 +16,94 @@
 // Command based programming: https://docs.wpilib.org/en/stable/docs/software/commandbased/what-is-command-based.html
 // Subsystem Documentation documentation: https://docs.wpilib.org/en/stable/docs/software/commandbased/subsystems.html
 
-/**
- * 
- */
-
+/** Vision subsystem */
 public class Vision extends SubsystemBase {
-    /** Constructor. Creates a new ExampleSubsystem. */
-    PhotonCamera camera;
-    
-    
-    PhotonTrackedTarget bestTag = null;
-    List<PhotonTrackedTarget> targets = null;
-    
-    public Vision(String cameraName) {
-        camera = new PhotonCamera(cameraName);
-
-    }
-
-    /**
-     * Gives Transform3d from the robot center to the desired target
-     * @return
-     */
-    public Transform3d getDistToTag() {
-        return bestTag.getBestCameraToTarget();
-    }
-
-    /**
-     * Gives Transform3d from robot center to the desired target
-     * @param tagID The fiducial ID of the desired April Tag
-     * @return Returns null if the tag cannot be found
-     * 
-     */
-    public Transform3d getDistToTag(int tagID) {
-        Transform3d dist = null;
-        for(int i = 0; i < targets.size(); i++)
-        {
-            if (targets.get(i).getFiducialId() == tagID) {
-                dist = targets.get(i).getBestCameraToTarget();
-                break;
-            }
-        }
-        
-        return dist;
-    }
-    /**
-     * This method is called periodically by the CommandScheduler, about every 20ms.
-     * It should be used for updating subsystem-specific state that you don't want to offload to a Command.
-     * Try to avoid "doing to much" in this method (for example no driver control here).
-     */
-    @Override
-    public void periodic() {
-        PhotonPipelineResult result = camera.getLatestResult();
-        // This method will be called once per scheduler run
-        bestTag = result.getBestTarget();
-        targets = result.getTargets();
-        if(bestTag != null) {
-            SmartDashboard.putNumber("Tag ID", bestTag.getFiducialId());
-            Transform3d tagPose = bestTag.getBestCameraToTarget();
-            Rotation3d tagRot = tagPose.getRotation();
-            // SmartDashboard.putNumberArray("Tag X Y Z", new double[] {tagPose.getX(), tagPose.getY(), tagPose.getZ()});
-            // SmartDashboard.putNumberArray("Tag Yaw, Pitch, Roll", new double[] {
-            //     Units.radiansToDegrees(tagRot.getZ()),
-            //     Units.radiansToDegrees(tagRot.getY()),
-            //     Units.radiansToDegrees(tagRot.getX())});
-            SmartDashboard.putNumber("Tag Pose X", tagPose.getX());
-            SmartDashboard.putNumber("Tag Pose Y", tagPose.getY());
-            SmartDashboard.putNumber("Tag Pose Z", tagPose.getZ());
-            SmartDashboard.putNumber("Tag Pose Yaw", tagRot.getZ());
-            SmartDashboard.putNumber("Tag Pose Pitch", tagRot.getY());
-            SmartDashboard.putNumber("Tag Pose Roll", tagRot.getX());
-            SmartDashboard.putNumber("Tag Yaw", bestTag.getYaw());
-            SmartDashboard.putNumber("Tag Pitch", bestTag.getPitch());
-        } else {
-            SmartDashboard.putNumber("Tag ID", -1);
-            SmartDashboard.putNumber("Tag Pose X", -1);
-            SmartDashboard.putNumber("Tag Pose Y", -1);
-            SmartDashboard.putNumber("Tag Pose Z", -1);
-            SmartDashboard.putNumber("Tag Pose Yaw", -1);
-            SmartDashboard.putNumber("Tag Pose Pitch", -1);
-            SmartDashboard.putNumber("Tag Pose Roll", -1);
-            SmartDashboard.putNumber("Tag Yaw", 0);
-            SmartDashboard.putNumber("Tag Pitch", 0);
-            // SmartDashboard.putNumberArray("Tag X Y Z", new double[] {0, 0, 0});
-            // SmartDashboard.putNumberArray("Tag Yaw, Pitch, Roll", new double[] {0, 0, 0});
-        }
-
-        
-    }
+
+	private final static boolean DEBUG_INFO = true;
+
+	final PhotonCamera camera;
+	final Transform3d cameraToFrontCenter;
+
+	/**
+	 * Create new PhotonCamera subsystem
+	 * 
+	 * @param cameraName          name of PhotonCamera
+	 * @param cameraToFrontCenter distance from the camera to the front center point
+	 *                            of the robot
+	 */
+	public Vision(String cameraName, Transform3d cameraToFrontCenter) {
+		camera = new PhotonCamera(cameraName);
+		this.cameraToFrontCenter = cameraToFrontCenter;
+	}
+
+	/**
+	 * Get best april tag target
+	 * 
+	 * @return Object of best target
+	 */
+	public PhotonTrackedTarget getDistToTag() {
+		return camera
+				.getLatestResult()
+				.getBestTarget();
+	}
+
+	/**
+	 * Gives Transform3d from robot center to the desired target
+	 * 
+	 * @param tagID The fiducial ID of the desired April Tag
+	 * @return returns first tag with matching ID, null if None are found
+	 */
+	public PhotonTrackedTarget getDistToTag(int tagID) {
+		for (PhotonTrackedTarget target : camera.getLatestResult().getTargets()) {
+			if (target.getFiducialId() == tagID)
+				return target;
+		}
+		return null;
+	}
+
+	public Transform3d getDistanceToTarget(PhotonTrackedTarget tag) {
+		return tag
+				.getBestCameraToTarget()
+				.plus(cameraToFrontCenter);
+	}
+
+	/**
+	 * This method is called periodically by the CommandScheduler, about every 20ms.
+	 * It should be used for updating subsystem-specific state that you don't want
+	 * to offload to a Command.
+	 * Try to avoid "doing to much" in this method (for example no driver control
+	 * here).
+	 */
+	@Override
+	public void periodic() {
+		if (!DEBUG_INFO)
+			return;
+
+		PhotonPipelineResult result = camera.getLatestResult();
+
+		PhotonTrackedTarget bestTag = result.getBestTarget();
+
+		if (bestTag == null) {
+			bestTag = new PhotonTrackedTarget(-1, -1, -1, -1, -1,
+					new Transform3d(new Translation3d(-1, -1, -1), new Rotation3d(-1, -1, -1)), null, 0,
+					new ArrayList<>(), new ArrayList<>());
+		}
+
+		SmartDashboard.putNumber("Tag ID", bestTag.getFiducialId());
+		SmartDashboard.putNumber("Tag Yaw", bestTag.getYaw());
+		SmartDashboard.putNumber("Tag Pitch", bestTag.getPitch());
+		SmartDashboard.putNumber("Tag Skew", bestTag.getSkew());
+
+		Transform3d tagPose = bestTag.getBestCameraToTarget();
+
+		SmartDashboard.putNumber("Tag Pose X", tagPose.getX());
+		SmartDashboard.putNumber("Tag Pose Y", tagPose.getY());
+		SmartDashboard.putNumber("Tag Pose Z", tagPose.getZ());
+
+		Rotation3d tagPoseRotation = tagPose.getRotation();
+
+		SmartDashboard.putNumber("Tag Pose Yaw", tagPoseRotation.getZ());
+		SmartDashboard.putNumber("Tag Pose Pitch", tagPoseRotation.getY());
+		SmartDashboard.putNumber("Tag Pose Roll", tagPoseRotation.getX());
+	}
 }

From bb476a2f3c2537a8011c32666544b53600fa3ece Mon Sep 17 00:00:00 2001
From: judeconnolly <judeconnolly@seattleacademy.org>
Date: Mon, 12 Feb 2024 16:01:54 -0800
Subject: [PATCH 46/99] Arm Xbox control creation and attribution of buttons

teehee my commit names make me feel smart
---
 .../robot/commands/ArmJoystickControl.java    |  3 --
 .../frc/robot/commands/ArmXboxControl.java    | 48 +++++++++++++++++++
 2 files changed, 48 insertions(+), 3 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/ArmXboxControl.java

diff --git a/src/main/java/frc/robot/commands/ArmJoystickControl.java b/src/main/java/frc/robot/commands/ArmJoystickControl.java
index 1933559..b8e3d33 100644
--- a/src/main/java/frc/robot/commands/ArmJoystickControl.java
+++ b/src/main/java/frc/robot/commands/ArmJoystickControl.java
@@ -18,9 +18,6 @@ public class ArmJoystickControl extends Command {
     private final OptionButton lowerArmButton;
 
 
-
-    private double armPos;
-
     public ArmJoystickControl(Arm arm, CommandJoystick joystick) {
         this.arm = arm;
         this.joystick = joystick;
diff --git a/src/main/java/frc/robot/commands/ArmXboxControl.java b/src/main/java/frc/robot/commands/ArmXboxControl.java
new file mode 100644
index 0000000..afa77e5
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ArmXboxControl.java
@@ -0,0 +1,48 @@
+package frc.robot.commands;
+
+import frc.robot.Constants.ArmConstants;
+import frc.robot.subsystems.Arm;
+
+import frc.robot.utils.OptionButton;
+import frc.robot.utils.OptionButton.ActivationMode;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+
+public class ArmXboxControl extends Command {
+    private final Arm arm;
+    private final CommandXboxController xbox;
+
+    private final OptionButton raiseArmButton;
+    private final OptionButton lowerArmButton;
+
+
+    public ArmXboxControl(Arm arm, CommandXboxController xbox ) {
+        this.arm = arm;
+        this.xbox = xbox;
+
+
+        lowerArmButton = new OptionButton(xbox, 5, ActivationMode.HOLD);
+        raiseArmButton = new OptionButton(xbox, 6, ActivationMode.HOLD);
+
+        xbox.povUp().onTrue(Commands.run(arm::setArmToSpeakerPosition));
+        xbox.povRight().onTrue(Commands.run(arm::setArmToAmpPosition));
+        xbox.povDown().onTrue(Commands.run(arm::setArmToIntakePosition));
+
+        addRequirements(arm);
+    }
+
+
+    /** 
+     * The main body of a command. Called repeatedly while the command is scheduled (Every 20 ms).
+     */
+    @Override
+    public void execute() {
+
+
+        //two buttons determining the raising and lowering of the arm
+        arm.changeArmAngleDegreesBy(Double.valueOf(raiseArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
+        arm.changeArmAngleDegreesBy(Double.valueOf(-lowerArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
+    }
+}

From 432203419202c253aa8f9a2f0b389b4a7bb0baa0 Mon Sep 17 00:00:00 2001
From: Aceius E <aceiuselms@seattleacademy.org>
Date: Mon, 12 Feb 2024 17:29:21 -0800
Subject: [PATCH 47/99] Switch to shuffleboard API in DriverControl command

---
 .../frc/robot/commands/DriverControl.java     | 110 +++++++++++-------
 1 file changed, 67 insertions(+), 43 deletions(-)

diff --git a/src/main/java/frc/robot/commands/DriverControl.java b/src/main/java/frc/robot/commands/DriverControl.java
index c0e2214..b84710f 100644
--- a/src/main/java/frc/robot/commands/DriverControl.java
+++ b/src/main/java/frc/robot/commands/DriverControl.java
@@ -2,12 +2,16 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.DriverConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.utils.ChassisDriveInputs;
 import frc.robot.utils.OptionButton;
+import java.util.function.Supplier;
 
 /**
  * This can be the default command for the drivetrain, allowing for remote
@@ -22,15 +26,31 @@ public class DriverControl extends Command {
 
 	private final ChassisDriveInputs chassisDriveInputs;
 
+	private final Supplier<Double> speedX;
+	private final Supplier<Double> speedY;
+	private final Supplier<Double> speedRotation;
+	private final Supplier<Boolean> isFieldRelative;
+	private final Supplier<Integer> speedLevel;
+	private final Supplier<ChassisSpeeds> speeds;
+
+	private ShuffleboardTab driverTab;
+	private GenericEntry widgetSpeedX;
+	private GenericEntry widgetSpeedY;
+	private GenericEntry widgetSpeed;
+	private GenericEntry widgetSpeedMode;
+	private GenericEntry widgetFieldRelative;
+	private GenericEntry widgetPoseX;
+	private GenericEntry widgetPoseY;
+	private GenericEntry widgetPoseDegrees;
+	private ShuffleboardTab developerTab;
+	private GenericEntry widgetTotalSpeed;
+	private GenericEntry widgetHeading;
+
 	/**
-	 * Creates a new SwerveDriveBaseControl Command.
-	 *
-	 * @param drivetrain       The drivetrain of the robot
-	 * @param driverController The device used to control drivetrain
+	 * Creates a new DriverControl Command.
 	 */
 	public DriverControl(SwerveDrivetrain drivetrain, ChassisDriveInputs chassisDriveInputs,
 			OptionButton preciseModeButton, OptionButton boostModeButton, OptionButton fieldRelativeButton) {
-
 		this.chassisDriveInputs = chassisDriveInputs;
 
 		this.preciseModeButton = preciseModeButton;
@@ -39,62 +59,66 @@ public DriverControl(SwerveDrivetrain drivetrain, ChassisDriveInputs chassisDriv
 
 		this.drivetrain = drivetrain;
 
-		// Tell the command schedular we are using the drivetrain
+		speedX = chassisDriveInputs::getX;
+		speedY = chassisDriveInputs::getY;
+
+		speedRotation = chassisDriveInputs::getRotation;
+		isFieldRelative = fieldRelativeButton::getState;
+
+		speedLevel = () -> 1
+				- preciseModeButton.getStateAsInt()
+				+ boostModeButton.getStateAsInt();
+
+		speeds = () -> new ChassisSpeeds(
+				speedX.get() * DriverConstants.maxSpeedOptionsTranslation[speedLevel.get()],
+				speedY.get() * DriverConstants.maxSpeedOptionsTranslation[speedLevel.get()],
+				speedRotation.get() * DriverConstants.maxSpeedOptionsRotation[speedLevel.get()]);
+
 		addRequirements(drivetrain);
 	}
 
 	/**
-	 * The initial subroutine of a command. Called once when the command is
-	 * initially scheduled.
 	 * Puts all swerve modules to the default state, staying still and facing
-	 * forwards.
+	 * forwards. Also turns control activity indicator green.
 	 */
 	@Override
 	public void initialize() {
 		drivetrain.toDefaultStates();
 
-		SmartDashboard.putBoolean("ControlActive", true);
+		driverTab = Shuffleboard.getTab("Driver Control");
+		driverTab.add("Control Active", true);
+		widgetTotalSpeed = driverTab.add("Robot Speed", 0).getEntry();
+		widgetHeading = driverTab.add("Heading", 0).getEntry();
+
+		developerTab = Shuffleboard.getTab("Debug");
+		widgetSpeedX = developerTab.add("SpeedX", speedX.get()).getEntry();
+		widgetSpeedY = developerTab.add("SpeedY", speedY.get()).getEntry();
+		widgetSpeed = developerTab.add("SpeedRot", speedRotation.get()).getEntry();
+		widgetSpeedMode = developerTab.add("Speed Mode", "-").getEntry();
+		widgetFieldRelative = developerTab.add("Field Relative", false).getEntry();
+		widgetPoseX = developerTab.add("PoseX", 0).getEntry();
+		widgetPoseY = developerTab.add("PoseY", 0).getEntry();
+		widgetPoseDegrees = developerTab.add("PoseDegrees", 0).getEntry();
 	}
 
-	/**
-	 * The main body of a command. Called repeatedly while the command is scheduled
-	 * (Every 20 ms).
-	 */
 	@Override
 	public void execute() {
-		final double speedX = chassisDriveInputs.getX();
-		final double speedY = chassisDriveInputs.getY();
-
-		final double speedRotation = chassisDriveInputs.getRotation();
-
-		final boolean isFieldRelative = fieldRelativeButton.getState();
-
-		final int speedLevel = 1
-				- preciseModeButton.getStateAsInt()
-				+ boostModeButton.getStateAsInt();
-
-		final ChassisSpeeds speeds = new ChassisSpeeds(
-				speedX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-				speedY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-				speedRotation * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
-
-
-		SmartDashboard.putNumber("SpeedX", speedX);
-		SmartDashboard.putNumber("SpeedY", speedY);
-		SmartDashboard.putNumber("Speed", speedRotation);
+		drivetrain.setDesiredState(speeds.get(), isFieldRelative.get(), true);
 
-		drivetrain.setDesiredState(speeds, isFieldRelative, true);
+		widgetSpeed.setDouble(speedRotation.get());
+		widgetSpeedX.setDouble(speedX.get());
+		widgetSpeedY.setDouble(speedY.get());
 
 		// Display relevant data on shuffleboard.
-		SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);
-		SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
+		widgetSpeedMode.setString(DriverConstants.maxSpeedOptionsNames[speedLevel.get()]);
+		widgetFieldRelative.setBoolean(isFieldRelative.get());
 
 		// Position display
 		final Pose2d robotPosition = drivetrain.getPosition();
 
-		SmartDashboard.putNumber("PoseX", robotPosition.getX());
-		SmartDashboard.putNumber("PoseY", robotPosition.getY());
-		SmartDashboard.putNumber("PoseDegrees", robotPosition.getRotation().getDegrees());
+		widgetPoseX.setDouble(robotPosition.getX());
+		widgetPoseY.setDouble(robotPosition.getY());
+		widgetPoseDegrees.setDouble(robotPosition.getRotation().getDegrees());
 
 		// Speed and Heading
 		final ChassisSpeeds currentSpeeds = drivetrain.getState();
@@ -102,8 +126,8 @@ public void execute() {
 				.sqrt(Math.pow(currentSpeeds.vxMetersPerSecond, 2) + Math.pow(currentSpeeds.vyMetersPerSecond, 2));
 
 		final double metersPerSecondToMilesPerHourConversion = 2.237;
-		SmartDashboard.putNumber("Robot Speed", speedMetersPerSecond * metersPerSecondToMilesPerHourConversion);
-		SmartDashboard.putNumber("Heading Degrees", drivetrain.getHeading().getDegrees());
+		widgetTotalSpeed.setDouble(speedMetersPerSecond * metersPerSecondToMilesPerHourConversion);
+		widgetHeading.setDouble(drivetrain.getHeading().getDegrees());
 	}
 
 	/**
@@ -125,6 +149,6 @@ public boolean isFinished() {
 	public void end(boolean interrupted) {
 		drivetrain.stop();
 
-		SmartDashboard.putBoolean("ControlActive", false);
+		// turn red
 	}
 }
\ No newline at end of file

From 6af518dcdee660cd56f0e06b21c09128d7518965 Mon Sep 17 00:00:00 2001
From: Aceius E <aceiuselms@seattleacademy.org>
Date: Mon, 12 Feb 2024 17:40:00 -0800
Subject: [PATCH 48/99] Update javadocs on some commands

---
 src/main/java/frc/robot/commands/AutoDriveTo.java    | 5 +++++
 src/main/java/frc/robot/commands/AutoRotateTo.java   | 5 +++++
 src/main/java/frc/robot/commands/DriveTransform.java | 2 +-
 3 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/AutoDriveTo.java b/src/main/java/frc/robot/commands/AutoDriveTo.java
index eefad5b..50b9c03 100644
--- a/src/main/java/frc/robot/commands/AutoDriveTo.java
+++ b/src/main/java/frc/robot/commands/AutoDriveTo.java
@@ -21,6 +21,11 @@ public class AutoDriveTo extends Command {
 
 	private boolean xOnlyMode;
 
+	/***
+	 * Command to autonomously drive somewhere
+	 * @param subsystem The drivetrain
+	 * @param translation The translation to execute
+	 */
 	public AutoDriveTo(SwerveDrivetrain subsystem, Translation2d translation) {
 		this.drivetrain = subsystem;
 
diff --git a/src/main/java/frc/robot/commands/AutoRotateTo.java b/src/main/java/frc/robot/commands/AutoRotateTo.java
index 8523011..3c570cc 100644
--- a/src/main/java/frc/robot/commands/AutoRotateTo.java
+++ b/src/main/java/frc/robot/commands/AutoRotateTo.java
@@ -17,6 +17,11 @@ public class AutoRotateTo extends Command {
 
 	private double atSetpointCounter = 0;
 
+	/***
+	 * Command to autonomously rotate some direction
+	 * @param subsystem The robot drivetrain
+	 * @param direction Rotation2d class to execute
+	 */
 	public AutoRotateTo(SwerveDrivetrain subsystem, Rotation2d direction) {
 
 		rotatePID = new PIDController(
diff --git a/src/main/java/frc/robot/commands/DriveTransform.java b/src/main/java/frc/robot/commands/DriveTransform.java
index 938be08..21987b5 100644
--- a/src/main/java/frc/robot/commands/DriveTransform.java
+++ b/src/main/java/frc/robot/commands/DriveTransform.java
@@ -12,7 +12,7 @@ public class DriveTransform extends Command {
 	private final Transform2d transform;
 
 	/**
-	 * Create a new DriveToPose command. Tries to drive a certain transform using
+	 * Create a new DriveTransform command. Tries to drive a certain transform using
 	 * the DriveToPose command.
 	 * 
 	 * <p>This drives relative to the robot starting position,

From 7f1bd24db6008c81c7b35fafc5698ae413b5e90a Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Mon, 12 Feb 2024 17:44:23 -0800
Subject: [PATCH 49/99] new pid values

---
 src/main/java/frc/robot/Constants.java            | 12 ++++++------
 src/main/java/frc/robot/commands/AutoDriveTo.java |  7 ++++++-
 2 files changed, 12 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 6e15b83..204ca87 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -62,23 +62,23 @@ public static class DriverConstants {
 		public static final double[] maxSpeedOptionsTranslation = { 0.1, 0.75, 1 };
 
 		// max angular velocity for drivetrain, in radians per second
-		public static final double[] maxSpeedOptionsRotation = { 0.1, 0.75, 1 };
+		public static final double[] maxSpeedOptionsRotation = { 0.25, 0.75, 1 };
 	}
 
 	public static class RobotMovementConstants {
 		public static final double AT_SETPOINT_TOLERANCE_TIME_SECONDS = 1;
 		public static final double ROTATE_AT_SETPOINT_TIME_SECONDS = 1;
 
-		public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(5);
+		public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(0.0001);
 		public static final double ANGLE_TOLERANCE_RADIANS = Units.degreesToRadians(5);
 
 		public static final double ROTATION_PID_P = 0.5;
 		public static final double ROTATION_PID_I = 0;
 		public static final double ROTATION_PID_D = 0;
 
-		public static final double TRANSLATION_PID_P = 75;
-		public static final double TRANSLATION_PID_I = 1;
-		public static final double TRANSLATION_PID_D = 0.5;
+		public static final double TRANSLATION_PID_P = 30;
+		public static final double TRANSLATION_PID_I = 0.5;
+		public static final double TRANSLATION_PID_D = 15;
 	}
 
 	public static class OperatorConstants {
@@ -106,7 +106,7 @@ public static class SwerveModuleConstants {
 					VELOCITY_MOTOR_ID_BL = 3;
 					ANGULAR_MOTOR_ID_BL = 2;
 					ANGULAR_MOTOR_ENCODER_ID_BL = 4;
-					ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.9260253906;
+					ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.91748046875;
 
 					// Back right
 					VELOCITY_MOTOR_ID_BR = 42;
diff --git a/src/main/java/frc/robot/commands/AutoDriveTo.java b/src/main/java/frc/robot/commands/AutoDriveTo.java
index eefad5b..e0ebb6a 100644
--- a/src/main/java/frc/robot/commands/AutoDriveTo.java
+++ b/src/main/java/frc/robot/commands/AutoDriveTo.java
@@ -78,6 +78,11 @@ public void execute() {
 		else
 			xSpeed = 0;
 
+		// TEMP FIX: LEAVE HERE UNTIL BUMPERS!!!
+		if (Math.abs(xSpeed) > 0.5) {
+			xSpeed = 0.5 * Math.signum(xSpeed);
+		}
+
 		final ChassisSpeeds speeds = new ChassisSpeeds(
 				xSpeed,
 				ySpeed,
@@ -88,7 +93,7 @@ public void execute() {
 
 	@Override
 	public boolean isFinished() {
-		return atSetpointCounter > RobotMovementConstants.AT_SETPOINT_TOLERANCE_TIME_SECONDS;
+		return atSetpointCounter > 0;
 	}
 
 	@Override

From 36669f603f54b27a966df2628726f88e6807dc55 Mon Sep 17 00:00:00 2001
From: Aceius E <aceiuselms@seattleacademy.org>
Date: Wed, 14 Feb 2024 15:54:48 -0800
Subject: [PATCH 50/99] Update SwerveDrivetrain.java

---
 src/main/java/frc/robot/subsystems/SwerveDrivetrain.java | 7 ++-----
 1 file changed, 2 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index 8169f84..fa6df0a 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -64,7 +64,7 @@ public class SwerveDrivetrain extends SubsystemBase {
 	private Pose2d pose;
 
 	/**
-	 * Desired pose of robot. The desired pose is the the X, Y and Rotation the robot wants to be in, relative to the last reset.
+	 * Desired pose of robot. The desired pose is the X, Y and Rotation the robot wants to be in, relative to the last reset.
 	 * It can be set to null to not have any desired pose.
 	 * 
 	 * @see https://docs.wpilib.org/en/stable/docs/software/advanced-controls/geometry/pose.html#pose
@@ -126,10 +126,7 @@ public SwerveDrivetrain(AHRS gyro,
 
 	// --- Pose Related Methods ---
 
-	/**
-	 * This is the periodic function of the swerve drivetrain.
-	 * This method is called periodically by the CommandScheduler, about every 20ms.
-	 */
+	/** This is the periodic function of the swerve drivetrain, called periodically by the CommandScheduler, about every 20ms. */
 	@Override
 	public void periodic() {
 		// use odometry to update the estimated pose

From 5b168bc8cb64554195b947bb4c40f78baa1d1c05 Mon Sep 17 00:00:00 2001
From: Aceius E <aceiuselms@seattleacademy.org>
Date: Wed, 14 Feb 2024 16:00:36 -0800
Subject: [PATCH 51/99] Fix out of date comment

---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 995cef4..212e809 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -57,11 +57,11 @@ public class SwerveModule extends SubsystemBase {
 	 * Sets up both drive and angular motor for swerve module as well as systems to
 	 * monitor and control them
 	 * 
-	 * @param velocityMotorDeviceID  device ID for drive motor
-	 * @param steeringMotorDeviceId  device ID for steering motor
-	 * @param angularEncoderDeviceID device ID for the angular motor's absolute encoder
-	 * @param distanceFromCenter     distance from center of robot to center of swerve module
-	 * @param steeringEncoderZero    the zero (forward) position for the angular motor's absolute encoder, in rotations
+	 * @param driveMotorDeviceId        device ID for drive motor
+	 * @param steeringMotorDeviceId     device ID for steering motor
+	 * @param steeringAbsoluteEncoderId device ID for the angular motor's absolute encoder
+	 * @param distanceFromCenter        distance from center of robot to center of swerve module
+	 * @param steeringEncoderZero       the zero (forward) position for the angular motor's absolute encoder, in rotations
 	 */
 	public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steeringAbsoluteEncoderId, double steeringEncoderZero, Translation2d distanceFromCenter) {
 		// --- Drive Motor ---

From e88a9863580a1de9b88030d1f26176e0db9fe9ba Mon Sep 17 00:00:00 2001
From: Aceius E <aceiuselms@seattleacademy.org>
Date: Wed, 14 Feb 2024 16:18:47 -0800
Subject: [PATCH 52/99] Fix innacurate docs & other comment cleanup

---
 src/main/java/frc/robot/commands/Autos.java          |  3 +++
 src/main/java/frc/robot/subsystems/Vision.java       | 12 +-----------
 .../java/frc/robot/utils/ChassisDriveInputs.java     |  7 +++++--
 src/main/java/frc/robot/utils/OptionButton.java      |  9 ++++-----
 4 files changed, 13 insertions(+), 18 deletions(-)

diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java
index 270c6ca..c336959 100644
--- a/src/main/java/frc/robot/commands/Autos.java
+++ b/src/main/java/frc/robot/commands/Autos.java
@@ -9,6 +9,7 @@
 import frc.robot.Constants.SwerveDrivetrainConstants;
 import frc.robot.subsystems.Arm;
 
+/** This class just contains a bunch of auto-modes. Do not call this class itself. */
 public final class Autos {
 	/** Example static factory for an autonomous command. */
 	public static Command testingAuto(SwerveDrivetrain drivetrain) {
@@ -17,10 +18,12 @@ public static Command testingAuto(SwerveDrivetrain drivetrain) {
 		);
 	}
 
+	/** Auto-mode that attempts to follow an april tag. */
 	public static Command tagFollowAuto(SwerveDrivetrain drivetrain, Vision camera, Integer tagId) {
 		return new FollowTag(drivetrain, camera, new Translation2d(1, 0), tagId, null);
 	}
 
+	/**  Linden did this */
 	public static Command startingAuto(Arm arm, SwerveDrivetrain drivetrain, boolean invertY) {
 
 		// assumes start position in corner
diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java
index cc0ce85..7f22ee2 100644
--- a/src/main/java/frc/robot/subsystems/Vision.java
+++ b/src/main/java/frc/robot/subsystems/Vision.java
@@ -12,10 +12,6 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
-// How to make Subsystem (ignore image instructions, code is out of date, just look at written general instructions): https://compendium.readthedocs.io/en/latest/tasks/subsystems/subsystems.html
-// Command based programming: https://docs.wpilib.org/en/stable/docs/software/commandbased/what-is-command-based.html
-// Subsystem Documentation documentation: https://docs.wpilib.org/en/stable/docs/software/commandbased/subsystems.html
-
 /** Vision subsystem */
 public class Vision extends SubsystemBase {
 
@@ -67,13 +63,7 @@ public Transform3d getDistanceToTarget(PhotonTrackedTarget tag) {
 				.plus(cameraToFrontCenter);
 	}
 
-	/**
-	 * This method is called periodically by the CommandScheduler, about every 20ms.
-	 * It should be used for updating subsystem-specific state that you don't want
-	 * to offload to a Command.
-	 * Try to avoid "doing to much" in this method (for example no driver control
-	 * here).
-	 */
+
 	@Override
 	public void periodic() {
 		if (!DEBUG_INFO)
diff --git a/src/main/java/frc/robot/utils/ChassisDriveInputs.java b/src/main/java/frc/robot/utils/ChassisDriveInputs.java
index 5f249d5..400b4bc 100644
--- a/src/main/java/frc/robot/utils/ChassisDriveInputs.java
+++ b/src/main/java/frc/robot/utils/ChassisDriveInputs.java
@@ -16,10 +16,10 @@ public class ChassisDriveInputs {
 	 * Create a new ChassisDriveInputs
 	 * 
 	 * @param getForward Get the value mapped to X, -1 full backward to +1 full forward
-	 * @param forwardCoefficient Coefficient that forward (X) multiped by
+	 * @param forwardCoefficient Coefficient that forward (X) multiplied by
 	 * 
 	 * @param getLeft Get the value mapped to Y, -1 full right to +1 full left
-	 * @param leftCoefficient Coefficient that forward left (Y) are multiped by
+	 * @param leftCoefficient Coefficient that forward left (Y) are multiplied by
 	 * 
 	 * @param getRotation Get the value mapped to rotation, -1 full clock
 	 * @param rotationCoefficient Coefficient that rotation is multiplied by
@@ -43,14 +43,17 @@ public ChassisDriveInputs(
 		this.deadzone = deadzone;
 	}
 
+	/** @return Joystick X with the deadzone applied */
 	public double getX() {
 		return applyJoystickDeadzone(xSupplier.get(), deadzone) * xCoefficient;
 	}
 
+	/** @return Joystick Y with the deadzone applied */
 	public double getY() {
 		return applyJoystickDeadzone(ySupplier.get(), deadzone) * yCoefficient;
 	}
 
+	/** @return Joystick rotation with deadzone applied */
 	public double getRotation() {
 		return applyJoystickDeadzone(rotationSupplier.get(), deadzone) * rotationCoefficient;
 	}
diff --git a/src/main/java/frc/robot/utils/OptionButton.java b/src/main/java/frc/robot/utils/OptionButton.java
index 9a67d65..6852cc1 100644
--- a/src/main/java/frc/robot/utils/OptionButton.java
+++ b/src/main/java/frc/robot/utils/OptionButton.java
@@ -37,9 +37,9 @@ public static enum ActivationMode {
 	/**
 	 * Create Option button.
 	 * 
-	 * @param commandDevice device that button is on
+	 * @param controller    device that button is on
 	 * @param button        button number
-	 * @param mode          whether or not we want button to act as toggle or hold
+	 * @param mode          whether we want button to act as toggle or hold
 	 *                      button
 	 */
 	public OptionButton(CommandGenericHID controller, int button, ActivationMode mode) {
@@ -48,10 +48,9 @@ public OptionButton(CommandGenericHID controller, int button, ActivationMode mod
 
 	/**
 	 * Create Option button.
-	 * 
-	 * @param commandDevice device that button is on
+	 *
 	 * @param button        button number
-	 * @param mode          whether or not we want button to act as toggle or hold button
+	 * @param mode          whether we want button to act as toggle or hold button
 	 */
 	public OptionButton(Supplier<Trigger> button, ActivationMode mode) {
 		this.button = button;

From 49d2c13c5f0e55ab6e59fc7a1ffef9d5ed0b8ef9 Mon Sep 17 00:00:00 2001
From: judeconnolly <judeconnolly@seattleacademy.org>
Date: Wed, 14 Feb 2024 16:40:36 -0800
Subject: [PATCH 53/99] Arm Driver Control updating to new format

---
 src/main/java/frc/robot/Constants.java        |   6 +
 src/main/java/frc/robot/RobotContainer.java   |  14 +-
 .../robot/commands/ArmJoystickControl.java    |  48 ------
 .../frc/robot/commands/ArmXboxControl.java    |  48 ------
 .../frc/robot/commands/DriverControl.java     |  26 ++-
 .../SwerveDriveJoystickControl.java           | 157 ------------------
 6 files changed, 39 insertions(+), 260 deletions(-)
 delete mode 100644 src/main/java/frc/robot/commands/ArmJoystickControl.java
 delete mode 100644 src/main/java/frc/robot/commands/ArmXboxControl.java
 delete mode 100644 src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 8d9bf51..f892c87 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -76,7 +76,13 @@ public static class ArmConstants {
         public static final int RIGHT_ENCODER_ID = 0;
 
         public static final double DEGREES_PER_SECOND = 2.0;
+
+		public static final double ROTATION_PID_P = 0;
+		public static final double ROTATION_PID_I = 0;
+		public static final double ROTATION_PID_D = 0;
+
     }
+
 	public static class RobotMovementConstants {
 		public static final double AT_SETPOINT_TOLERANCE_TIME_SECONDS = 1;
 		public static final double ROTATE_AT_SETPOINT_TIME_SECONDS = 1;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 0daaafa..274d6cd 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -101,7 +101,7 @@ public void setUpDriveController() {
 
 		if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
 			final CommandJoystick joystick = new CommandJoystick(genericHID.getPort());
-			control = new DriverControl(drivetrain,
+			control = new DriverControl(drivetrain, arm,
 
 				new ChassisDriveInputs(
 					joystick::getX, joystick::getY, joystick::getTwist,
@@ -109,14 +109,17 @@ public void setUpDriveController() {
 
 				new OptionButton(joystick, 2, ActivationMode.TOGGLE),
 				new OptionButton(joystick, 1, ActivationMode.HOLD),
-				new OptionButton(joystick, 3, ActivationMode.TOGGLE)
+				new OptionButton(joystick, 3, ActivationMode.TOGGLE),
+				
+				new OptionButton(joystick,11,ActivationMode.HOLD),
+				new OptionButton(joystick,12,ActivationMode.HOLD)
 			);
 			
 			joystick.button(4).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
 
 		} else {
 			final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());
-			control = new DriverControl(drivetrain,
+			control = new DriverControl(drivetrain, arm,
 
 				new ChassisDriveInputs(
 					xbox::getLeftX, xbox::getLeftY, xbox::getRightX,
@@ -124,7 +127,10 @@ public void setUpDriveController() {
 
 				new OptionButton(xbox::b, ActivationMode.TOGGLE),
 				new OptionButton(xbox::leftStick, ActivationMode.HOLD),
-				new OptionButton(xbox::povUp, ActivationMode.TOGGLE)
+				new OptionButton(xbox::povUp, ActivationMode.TOGGLE),
+
+				new OptionButton(xbox::rightBumper,ActivationMode.HOLD),
+				new OptionButton(xbox::leftBumper,ActivationMode.HOLD)
 			);
 
 
diff --git a/src/main/java/frc/robot/commands/ArmJoystickControl.java b/src/main/java/frc/robot/commands/ArmJoystickControl.java
deleted file mode 100644
index b8e3d33..0000000
--- a/src/main/java/frc/robot/commands/ArmJoystickControl.java
+++ /dev/null
@@ -1,48 +0,0 @@
-package frc.robot.commands;
-
-import frc.robot.Constants.ArmConstants;
-import frc.robot.subsystems.Arm;
-
-import frc.robot.utils.OptionButton;
-import frc.robot.utils.OptionButton.ActivationMode;
-import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
-
-public class ArmJoystickControl extends Command {
-    private final Arm arm;
-    private final CommandJoystick joystick;
-
-    private final OptionButton raiseArmButton;
-    private final OptionButton lowerArmButton;
-
-
-    public ArmJoystickControl(Arm arm, CommandJoystick joystick) {
-        this.arm = arm;
-        this.joystick = joystick;
-
-
-        lowerArmButton = new OptionButton(joystick, 11, ActivationMode.HOLD);
-        raiseArmButton = new OptionButton(joystick, 12, ActivationMode.HOLD);
-
-        joystick.povUp().onTrue(Commands.run(arm::setArmToSpeakerPosition));
-        joystick.povRight().onTrue(Commands.run(arm::setArmToAmpPosition));
-        joystick.povDown().onTrue(Commands.run(arm::setArmToIntakePosition));
-
-        addRequirements(arm);
-    }
-
-
-    /** 
-     * The main body of a command. Called repeatedly while the command is scheduled (Every 20 ms).
-     */
-    @Override
-    public void execute() {
-
-
-        //two buttons determining the raising and lowering of the arm
-        arm.changeArmAngleDegreesBy(Double.valueOf(raiseArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
-        arm.changeArmAngleDegreesBy(Double.valueOf(-lowerArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
-    }
-}
diff --git a/src/main/java/frc/robot/commands/ArmXboxControl.java b/src/main/java/frc/robot/commands/ArmXboxControl.java
deleted file mode 100644
index afa77e5..0000000
--- a/src/main/java/frc/robot/commands/ArmXboxControl.java
+++ /dev/null
@@ -1,48 +0,0 @@
-package frc.robot.commands;
-
-import frc.robot.Constants.ArmConstants;
-import frc.robot.subsystems.Arm;
-
-import frc.robot.utils.OptionButton;
-import frc.robot.utils.OptionButton.ActivationMode;
-import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-
-public class ArmXboxControl extends Command {
-    private final Arm arm;
-    private final CommandXboxController xbox;
-
-    private final OptionButton raiseArmButton;
-    private final OptionButton lowerArmButton;
-
-
-    public ArmXboxControl(Arm arm, CommandXboxController xbox ) {
-        this.arm = arm;
-        this.xbox = xbox;
-
-
-        lowerArmButton = new OptionButton(xbox, 5, ActivationMode.HOLD);
-        raiseArmButton = new OptionButton(xbox, 6, ActivationMode.HOLD);
-
-        xbox.povUp().onTrue(Commands.run(arm::setArmToSpeakerPosition));
-        xbox.povRight().onTrue(Commands.run(arm::setArmToAmpPosition));
-        xbox.povDown().onTrue(Commands.run(arm::setArmToIntakePosition));
-
-        addRequirements(arm);
-    }
-
-
-    /** 
-     * The main body of a command. Called repeatedly while the command is scheduled (Every 20 ms).
-     */
-    @Override
-    public void execute() {
-
-
-        //two buttons determining the raising and lowering of the arm
-        arm.changeArmAngleDegreesBy(Double.valueOf(raiseArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
-        arm.changeArmAngleDegreesBy(Double.valueOf(-lowerArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
-    }
-}
diff --git a/src/main/java/frc/robot/commands/DriverControl.java b/src/main/java/frc/robot/commands/DriverControl.java
index be36840..e3698e7 100644
--- a/src/main/java/frc/robot/commands/DriverControl.java
+++ b/src/main/java/frc/robot/commands/DriverControl.java
@@ -2,9 +2,12 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.ArmConstants;
 import frc.robot.Constants.DriverConstants;
+import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.utils.ChassisDriveInputs;
 import frc.robot.utils.OptionButton;
@@ -13,7 +16,9 @@
  * This can be the default command for the drivetrain, allowing for remote operation with a controller.
  */
 public class DriverControl extends Command {
-	protected final SwerveDrivetrain drivetrain;
+	private final SwerveDrivetrain drivetrain;
+
+	private final Arm arm;
 
 	private final OptionButton preciseModeButton;
 	private final OptionButton boostModeButton;
@@ -21,14 +26,18 @@ public class DriverControl extends Command {
 
 	private final ChassisDriveInputs chassisDriveInputs;
 
+	private final OptionButton raiseArmButton;
+    private final OptionButton lowerArmButton;
+
 	/**
 	 * Creates a new SwerveDriveBaseControl Command.
 	 *
 	 * @param drivetrain       The drivetrain of the robot
 	 * @param driverController The device used to control drivetrain
 	 */
-	public DriverControl(SwerveDrivetrain drivetrain, ChassisDriveInputs chassisDriveInputs,
-			OptionButton preciseModeButton, OptionButton boostModeButton, OptionButton fieldRelativeButton) {
+	public DriverControl(SwerveDrivetrain drivetrain, Arm arm, ChassisDriveInputs chassisDriveInputs,
+			OptionButton preciseModeButton, OptionButton boostModeButton, OptionButton fieldRelativeButton,
+			OptionButton raiseArmButton, OptionButton lowerArmButton) {
 
 		this.chassisDriveInputs = chassisDriveInputs;
 
@@ -38,6 +47,13 @@ public DriverControl(SwerveDrivetrain drivetrain, ChassisDriveInputs chassisDriv
 
 		this.drivetrain = drivetrain;
 
+		this.arm = arm;
+
+		this.raiseArmButton = raiseArmButton;
+		this.lowerArmButton = lowerArmButton;
+
+		
+
 		// Tell the command schedular we are using the drivetrain
 		addRequirements(drivetrain);
 	}
@@ -103,6 +119,10 @@ public void execute() {
 		final double metersPerSecondToMilesPerHourConversion = 2.237;
 		SmartDashboard.putNumber("Robot Speed", speedMetersPerSecond * metersPerSecondToMilesPerHourConversion);
 		SmartDashboard.putNumber("Heading Degrees", drivetrain.getHeading().getDegrees());
+
+		// Arm Motor
+		arm.changeArmAngleDegreesBy(Double.valueOf(raiseArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
+        arm.changeArmAngleDegreesBy(Double.valueOf(-lowerArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
 	}
 
 	/**
diff --git a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java b/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
deleted file mode 100644
index 7f72c88..0000000
--- a/src/main/java/frc/robot/commands/SwerveRemoteOperation/SwerveDriveJoystickControl.java
+++ /dev/null
@@ -1,157 +0,0 @@
-package frc.robot.commands.SwerveRemoteOperation;
-
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.button.CommandJoystick;
-import frc.robot.Constants.DriverConstants;
-import frc.robot.subsystems.SwerveDrivetrain;
-import frc.robot.utils.OptionButton;
-import frc.robot.utils.OptionButton.ActivationMode;
-
-/**
- * This is the default command for the drivetrain, allowing for remote operation with joystick
- */
-public class SwerveDriveJoystickControl extends Command {
-    private final SwerveDrivetrain drivetrain;
-    private final CommandJoystick joystick;
-
-    private final OptionButton preciseModeButton;
-    private final OptionButton boostModeButton;
-    private final OptionButton fieldRelativeButton;
-
-    /**
-	 * Creates a new SwerveDriveJoystickControl Command.
-	 *
-	 * @param drivetrain The drivetrain of the robot
-	 * @param driverJoystick The joystick used to control drivetrain
-	 */
-    public SwerveDriveJoystickControl(SwerveDrivetrain drivetrain, CommandJoystick driverJoystick) {
-        this.drivetrain = drivetrain;
-        this.joystick = driverJoystick;
-
-        // Create and configure buttons
-        preciseModeButton = new OptionButton(driverJoystick, 2, ActivationMode.TOGGLE);
-        boostModeButton = new OptionButton(driverJoystick, 1, ActivationMode.HOLD);
-        fieldRelativeButton = new OptionButton(driverJoystick, 3, ActivationMode.TOGGLE);
-
-        // Tell the command schedular we are using the drivetrain
-        addRequirements(drivetrain);
-    }
-
-    
-    /**
-     * The initial subroutine of a command. Called once when the command is initially scheduled.
-     * Puts all swerve modules to the default state, staying still and facing forwards.
-     */
-    @Override
-    public void initialize() {
-        drivetrain.toDefaultStates();
-    }
-
-    
-/**
-     * The main body of a command. Called repeatedly while the command is scheduled (Every 20 ms).
-     */
-    @Override
-    public void execute() {
-        // Get joystick inputs
-        final double speedX = -applyJoystickDeadzone(joystick.getX(), DriverConstants.JOYSTICK_DEAD_ZONE);
-		final double speedY = -applyJoystickDeadzone(joystick.getY(), DriverConstants.JOYSTICK_DEAD_ZONE);
-		
-        double speedR = -applyJoystickDeadzone(joystick.getTwist(), DriverConstants.JOYSTICK_DEAD_ZONE);
-
-        // Code for rotating with buttons if driver prefers 
-		if (joystick.button(9).getAsBoolean()) {
-			speedR += DriverConstants.maxSpeedOptionsRotation[1];
-        }
-		if (joystick.button(10).getAsBoolean()) {
-			speedR -= DriverConstants.maxSpeedOptionsRotation[1];
-		}
-
-        
-        // Level of speed from Precise, to Normal, to Boost
-        // Find our speed level, default is one (Normal)
-        final int speedLevel = 1
-            - preciseModeButton.getStateAsInt()
-            + boostModeButton.getStateAsInt();
-
-        // Can be changed for testing
-        final int speedCoefficient = 1;
-
-        final boolean isFieldRelative = fieldRelativeButton.getState();
-
-        final ChassisSpeeds speeds = new ChassisSpeeds(
-            speedY * DriverConstants.maxSpeedOptionsTranslation[speedLevel] * speedCoefficient,
-            speedX * DriverConstants.maxSpeedOptionsTranslation[speedLevel] * speedCoefficient,
-            speedR * DriverConstants.maxSpeedOptionsRotation[speedLevel] * speedCoefficient
-        );
-
-        // Display relevant data on shuffleboard.
-        SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);        
-        SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
-
-        SmartDashboard.putNumber("Heading Degrees", drivetrain.getHeading().getDegrees());
-
-        // (60 / 2*Math.PI) = 94.24777960769379
-        final double radiansPerSecondToRPM = 94.24777960769379;
-        final double metersPerSecondToMPH = 2.2369;
-
-        SmartDashboard.putNumber("Target Speed X MPH", speeds.vxMetersPerSecond * metersPerSecondToMPH);
-        SmartDashboard.putNumber("Target Speed Y MPH", speeds.vyMetersPerSecond * metersPerSecondToMPH);
-        SmartDashboard.putNumber("Target RPM", speeds.omegaRadiansPerSecond * radiansPerSecondToRPM);
-
-        final ChassisSpeeds currentRobotSpeeds = drivetrain.getState();
-
-        SmartDashboard.putNumber("Real Speed X MPH", currentRobotSpeeds.vxMetersPerSecond * metersPerSecondToMPH);
-        SmartDashboard.putNumber("Real Speed Y MPH", currentRobotSpeeds.vyMetersPerSecond * metersPerSecondToMPH);
-        SmartDashboard.putNumber("Real RPM", currentRobotSpeeds.omegaRadiansPerSecond * radiansPerSecondToRPM);
-
-        SmartDashboard.putNumber("Joystick X", joystick.getX());
-		SmartDashboard.putNumber("Joystick Y", joystick.getY());
-		SmartDashboard.putNumber("Joystick R", joystick.getTwist());
-
-        SmartDashboard.putBoolean("X Active", speedX != 0);
-		SmartDashboard.putBoolean("Y Active", speedY != 0);
-		SmartDashboard.putBoolean("R Active", speedR != 0); 
-
-        drivetrain.setDesiredState(speeds, isFieldRelative);
-    }
-
-    /**
-     * Whether the command has finished. Once a command finishes, the scheduler will call its end() method and un-schedule it.
-     * Always return false since we never want to end in this case.
-     */
-	@Override
-	public boolean isFinished() {
-		return false;
-	}
-
-    /**
-     * The action to take when the command ends. Called when either the command finishes normally, or when it interrupted/canceled.
-     * Here, this should only happen in this case if we get interrupted.
-     */
-    @Override
-    public void end(boolean interrupted) {
-        drivetrain.stop();
-    }
-
-    /* -- Util --- */
-
-
-    /**
-     * Utility method. Apply a deadzone to the joystick output to account for stick drift and small bumps.
-     * 
-     * @param joystickValue Value in [-1, 1] from joystick axis
-     * @return {@code 0} if {@code |joystickValue| <= deadzone}, else the {@code joystickValue} scaled to the new control area
-     */
-    public static double applyJoystickDeadzone(double joystickValue, double deadzone) {
-        if (Math.abs(joystickValue) <= deadzone) {
-            // If the joystick |value| is in the deadzone than zero it out
-            return 0;
-        }
-
-        // scale value from the range [0, 1] to (deadzone, 1]
-        return joystickValue * (1 + deadzone) - Math.signum(joystickValue) * deadzone;
-    }
-}
\ No newline at end of file

From 27772941860dc137873c755dfe5c7041457d2bdf Mon Sep 17 00:00:00 2001
From: DigitalFork <113265535+DigitalFork@users.noreply.github.com>
Date: Wed, 14 Feb 2024 16:47:33 -0800
Subject: [PATCH 54/99] Rename camera to vision in FollowTag (disambiguity)

---
 .../java/frc/robot/commands/FollowTag.java    | 38 +++++++++++--------
 1 file changed, 23 insertions(+), 15 deletions(-)

diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java
index 298d6f9..60658ab 100644
--- a/src/main/java/frc/robot/commands/FollowTag.java
+++ b/src/main/java/frc/robot/commands/FollowTag.java
@@ -16,7 +16,7 @@
 public class FollowTag extends Command {
 	private final SwerveDrivetrain drivetrain;
 
-	private final Vision camera;
+	private final Vision vision;
 	private final Integer tagID;
 	private final Transform2d targetDistance;
 
@@ -24,18 +24,22 @@ public class FollowTag extends Command {
 	private double secondsSinceTagLastSeen;
 
 	/**
-	 * Create a new FollowTag command. Tries to follow a tag while staying a certain distance away.
+	 * Create a new FollowTag command. Tries to follow a tag while staying a certain
+	 * distance away.
 	 * 
 	 * @param drivetrain          the drivetrain of the robot
-	 * @param camera          	  the vision subsystem of the robot
-	 * @param tagID               the numerical ID of the the tag to follow, null for whatever best is
+	 * @param vision              the vision subsystem of the robot
+	 * @param tagID               the numerical ID of the the tag to follow, null
+	 *                            for whatever best is
 	 * @param targetDistanceToTag the target distance away from the tag to be
-	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover tag, set to null to never finish
+	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover
+	 *                            tag, set to null to never finish
 	 */
-	public FollowTag(SwerveDrivetrain drivetrain, Vision camera, Transform2d targetDistanceToTag, Integer tagID, Double loseTagAfterSeconds) {
+	public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Transform2d targetDistanceToTag, Integer tagID,
+			Double loseTagAfterSeconds) {
 		this.drivetrain = drivetrain;
 
-		this.camera = camera;
+		this.vision = vision;
 		this.tagID = tagID;
 
 		this.targetDistance = targetDistanceToTag;
@@ -45,16 +49,20 @@ public FollowTag(SwerveDrivetrain drivetrain, Vision camera, Transform2d targetD
 	}
 
 	/**
-	 * Create a new FollowTag command. Tries to follow a tag while staying a certain distance away.
+	 * Create a new FollowTag command. Tries to follow a tag while staying a certain
+	 * distance away.
 	 * 
 	 * @param drivetrain          the drivetrain of the robot
-	 * @param camera          	  the vision subsystem of the robot
-	 * @param tagID               the numerical ID of the the tag to follow, null for whatever best is
+	 * @param vision              the vision subsystem of the robot
+	 * @param tagID               the numerical ID of the the tag to follow, null
+	 *                            for whatever best is
 	 * @param targetDistanceToTag the target distance away from the tag to be
-	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover tag, set to null to never finish
+	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover
+	 *                            tag, set to null to never finish
 	 */
-	public FollowTag(SwerveDrivetrain drivetrain, Vision camera, Translation2d targetDistanceToTag, Integer tagID, Double loseTagAfterSeconds) {
-		this(drivetrain, camera, new Transform2d(targetDistanceToTag, new Rotation2d()), tagID, loseTagAfterSeconds);
+	public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Translation2d targetDistanceToTag, Integer tagID,
+			Double loseTagAfterSeconds) {
+		this(drivetrain, vision, new Transform2d(targetDistanceToTag, new Rotation2d()), tagID, loseTagAfterSeconds);
 	}
 
 	@Override
@@ -66,14 +74,14 @@ public void initialize() {
 	@Override
 	public void execute() {
 
-		final PhotonTrackedTarget tag = (tagID == null) ? camera.getDistToTag() : camera.getDistToTag(tagID);
+		final PhotonTrackedTarget tag = (tagID == null) ? vision.getDistToTag() : vision.getDistToTag(tagID);
 
 		if (tag == null) {
 			secondsSinceTagLastSeen += TimedRobot.kDefaultPeriod;
 
 			drivetrain.stop();
 		} else {
-			final Transform3d tagPosition3d = camera.getDistanceToTarget(tag);
+			final Transform3d tagPosition3d = vision.getDistanceToTarget(tag);
 
 			secondsSinceTagLastSeen = 0;
 

From 9a0aae21000fdb85dde8fa36a986714445b6abd9 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Wed, 14 Feb 2024 16:48:03 -0800
Subject: [PATCH 55/99] Fix future in follow tag

---
 .../java/frc/robot/commands/FollowTag.java    | 37 +++++++++++++------
 .../java/frc/robot/subsystems/Vision.java     |  4 +-
 2 files changed, 27 insertions(+), 14 deletions(-)

diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java
index 298d6f9..e1c9cbb 100644
--- a/src/main/java/frc/robot/commands/FollowTag.java
+++ b/src/main/java/frc/robot/commands/FollowTag.java
@@ -24,15 +24,19 @@ public class FollowTag extends Command {
 	private double secondsSinceTagLastSeen;
 
 	/**
-	 * Create a new FollowTag command. Tries to follow a tag while staying a certain distance away.
+	 * Create a new FollowTag command. Tries to follow a tag while staying a certain
+	 * distance away.
 	 * 
 	 * @param drivetrain          the drivetrain of the robot
-	 * @param camera          	  the vision subsystem of the robot
-	 * @param tagID               the numerical ID of the the tag to follow, null for whatever best is
+	 * @param camera              the vision subsystem of the robot
+	 * @param tagID               the numerical ID of the the tag to follow, null
+	 *                            for whatever best is
 	 * @param targetDistanceToTag the target distance away from the tag to be
-	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover tag, set to null to never finish
+	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover
+	 *                            tag, set to null to never finish
 	 */
-	public FollowTag(SwerveDrivetrain drivetrain, Vision camera, Transform2d targetDistanceToTag, Integer tagID, Double loseTagAfterSeconds) {
+	public FollowTag(SwerveDrivetrain drivetrain, Vision camera, Transform2d targetDistanceToTag, Integer tagID,
+			Double loseTagAfterSeconds) {
 		this.drivetrain = drivetrain;
 
 		this.camera = camera;
@@ -45,15 +49,19 @@ public FollowTag(SwerveDrivetrain drivetrain, Vision camera, Transform2d targetD
 	}
 
 	/**
-	 * Create a new FollowTag command. Tries to follow a tag while staying a certain distance away.
+	 * Create a new FollowTag command. Tries to follow a tag while staying a certain
+	 * distance away.
 	 * 
 	 * @param drivetrain          the drivetrain of the robot
-	 * @param camera          	  the vision subsystem of the robot
-	 * @param tagID               the numerical ID of the the tag to follow, null for whatever best is
+	 * @param camera              the vision subsystem of the robot
+	 * @param tagID               the numerical ID of the the tag to follow, null
+	 *                            for whatever best is
 	 * @param targetDistanceToTag the target distance away from the tag to be
-	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover tag, set to null to never finish
+	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover
+	 *                            tag, set to null to never finish
 	 */
-	public FollowTag(SwerveDrivetrain drivetrain, Vision camera, Translation2d targetDistanceToTag, Integer tagID, Double loseTagAfterSeconds) {
+	public FollowTag(SwerveDrivetrain drivetrain, Vision camera, Translation2d targetDistanceToTag, Integer tagID,
+			Double loseTagAfterSeconds) {
 		this(drivetrain, camera, new Transform2d(targetDistanceToTag, new Rotation2d()), tagID, loseTagAfterSeconds);
 	}
 
@@ -66,7 +74,7 @@ public void initialize() {
 	@Override
 	public void execute() {
 
-		final PhotonTrackedTarget tag = (tagID == null) ? camera.getDistToTag() : camera.getDistToTag(tagID);
+		final PhotonTrackedTarget tag = (tagID == null) ? camera.getTag() : camera.getTag(tagID);
 
 		if (tag == null) {
 			secondsSinceTagLastSeen += TimedRobot.kDefaultPeriod;
@@ -92,7 +100,12 @@ public void execute() {
 
 	@Override
 	public boolean isFinished() {
-		drivetrain.clearDesiredPosition();
 		return (loseTagAfterSeconds != null) && (secondsSinceTagLastSeen < loseTagAfterSeconds);
 	}
+
+	@Override
+	public void end(boolean interrupted) {
+		drivetrain.clearDesiredPosition();
+		drivetrain.stop();
+	}
 }
diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java
index cc0ce85..2231173 100644
--- a/src/main/java/frc/robot/subsystems/Vision.java
+++ b/src/main/java/frc/robot/subsystems/Vision.java
@@ -41,7 +41,7 @@ public Vision(String cameraName, Transform3d cameraToFrontCenter) {
 	 * 
 	 * @return Object of best target
 	 */
-	public PhotonTrackedTarget getDistToTag() {
+	public PhotonTrackedTarget getTag() {
 		return camera
 				.getLatestResult()
 				.getBestTarget();
@@ -53,7 +53,7 @@ public PhotonTrackedTarget getDistToTag() {
 	 * @param tagID The fiducial ID of the desired April Tag
 	 * @return returns first tag with matching ID, null if None are found
 	 */
-	public PhotonTrackedTarget getDistToTag(int tagID) {
+	public PhotonTrackedTarget getTag(int tagID) {
 		for (PhotonTrackedTarget target : camera.getLatestResult().getTargets()) {
 			if (target.getFiducialId() == tagID)
 				return target;

From 274ebaf8c939ee22c99a036112c46c8ba36ad72f Mon Sep 17 00:00:00 2001
From: Aceius E <aceiuselms@seattleacademy.org>
Date: Wed, 14 Feb 2024 16:53:56 -0800
Subject: [PATCH 56/99] This was a bad idea

This reverts commit 432203419202c253aa8f9a2f0b389b4a7bb0baa0.
---
 .../frc/robot/commands/DriverControl.java     | 110 +++++++-----------
 1 file changed, 43 insertions(+), 67 deletions(-)

diff --git a/src/main/java/frc/robot/commands/DriverControl.java b/src/main/java/frc/robot/commands/DriverControl.java
index b84710f..c0e2214 100644
--- a/src/main/java/frc/robot/commands/DriverControl.java
+++ b/src/main/java/frc/robot/commands/DriverControl.java
@@ -2,16 +2,12 @@
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.networktables.GenericEntry;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.DriverConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.utils.ChassisDriveInputs;
 import frc.robot.utils.OptionButton;
-import java.util.function.Supplier;
 
 /**
  * This can be the default command for the drivetrain, allowing for remote
@@ -26,31 +22,15 @@ public class DriverControl extends Command {
 
 	private final ChassisDriveInputs chassisDriveInputs;
 
-	private final Supplier<Double> speedX;
-	private final Supplier<Double> speedY;
-	private final Supplier<Double> speedRotation;
-	private final Supplier<Boolean> isFieldRelative;
-	private final Supplier<Integer> speedLevel;
-	private final Supplier<ChassisSpeeds> speeds;
-
-	private ShuffleboardTab driverTab;
-	private GenericEntry widgetSpeedX;
-	private GenericEntry widgetSpeedY;
-	private GenericEntry widgetSpeed;
-	private GenericEntry widgetSpeedMode;
-	private GenericEntry widgetFieldRelative;
-	private GenericEntry widgetPoseX;
-	private GenericEntry widgetPoseY;
-	private GenericEntry widgetPoseDegrees;
-	private ShuffleboardTab developerTab;
-	private GenericEntry widgetTotalSpeed;
-	private GenericEntry widgetHeading;
-
 	/**
-	 * Creates a new DriverControl Command.
+	 * Creates a new SwerveDriveBaseControl Command.
+	 *
+	 * @param drivetrain       The drivetrain of the robot
+	 * @param driverController The device used to control drivetrain
 	 */
 	public DriverControl(SwerveDrivetrain drivetrain, ChassisDriveInputs chassisDriveInputs,
 			OptionButton preciseModeButton, OptionButton boostModeButton, OptionButton fieldRelativeButton) {
+
 		this.chassisDriveInputs = chassisDriveInputs;
 
 		this.preciseModeButton = preciseModeButton;
@@ -59,66 +39,62 @@ public DriverControl(SwerveDrivetrain drivetrain, ChassisDriveInputs chassisDriv
 
 		this.drivetrain = drivetrain;
 
-		speedX = chassisDriveInputs::getX;
-		speedY = chassisDriveInputs::getY;
-
-		speedRotation = chassisDriveInputs::getRotation;
-		isFieldRelative = fieldRelativeButton::getState;
-
-		speedLevel = () -> 1
-				- preciseModeButton.getStateAsInt()
-				+ boostModeButton.getStateAsInt();
-
-		speeds = () -> new ChassisSpeeds(
-				speedX.get() * DriverConstants.maxSpeedOptionsTranslation[speedLevel.get()],
-				speedY.get() * DriverConstants.maxSpeedOptionsTranslation[speedLevel.get()],
-				speedRotation.get() * DriverConstants.maxSpeedOptionsRotation[speedLevel.get()]);
-
+		// Tell the command schedular we are using the drivetrain
 		addRequirements(drivetrain);
 	}
 
 	/**
+	 * The initial subroutine of a command. Called once when the command is
+	 * initially scheduled.
 	 * Puts all swerve modules to the default state, staying still and facing
-	 * forwards. Also turns control activity indicator green.
+	 * forwards.
 	 */
 	@Override
 	public void initialize() {
 		drivetrain.toDefaultStates();
 
-		driverTab = Shuffleboard.getTab("Driver Control");
-		driverTab.add("Control Active", true);
-		widgetTotalSpeed = driverTab.add("Robot Speed", 0).getEntry();
-		widgetHeading = driverTab.add("Heading", 0).getEntry();
-
-		developerTab = Shuffleboard.getTab("Debug");
-		widgetSpeedX = developerTab.add("SpeedX", speedX.get()).getEntry();
-		widgetSpeedY = developerTab.add("SpeedY", speedY.get()).getEntry();
-		widgetSpeed = developerTab.add("SpeedRot", speedRotation.get()).getEntry();
-		widgetSpeedMode = developerTab.add("Speed Mode", "-").getEntry();
-		widgetFieldRelative = developerTab.add("Field Relative", false).getEntry();
-		widgetPoseX = developerTab.add("PoseX", 0).getEntry();
-		widgetPoseY = developerTab.add("PoseY", 0).getEntry();
-		widgetPoseDegrees = developerTab.add("PoseDegrees", 0).getEntry();
+		SmartDashboard.putBoolean("ControlActive", true);
 	}
 
+	/**
+	 * The main body of a command. Called repeatedly while the command is scheduled
+	 * (Every 20 ms).
+	 */
 	@Override
 	public void execute() {
-		drivetrain.setDesiredState(speeds.get(), isFieldRelative.get(), true);
+		final double speedX = chassisDriveInputs.getX();
+		final double speedY = chassisDriveInputs.getY();
+
+		final double speedRotation = chassisDriveInputs.getRotation();
+
+		final boolean isFieldRelative = fieldRelativeButton.getState();
+
+		final int speedLevel = 1
+				- preciseModeButton.getStateAsInt()
+				+ boostModeButton.getStateAsInt();
+
+		final ChassisSpeeds speeds = new ChassisSpeeds(
+				speedX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				speedY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
+				speedRotation * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
+
+
+		SmartDashboard.putNumber("SpeedX", speedX);
+		SmartDashboard.putNumber("SpeedY", speedY);
+		SmartDashboard.putNumber("Speed", speedRotation);
 
-		widgetSpeed.setDouble(speedRotation.get());
-		widgetSpeedX.setDouble(speedX.get());
-		widgetSpeedY.setDouble(speedY.get());
+		drivetrain.setDesiredState(speeds, isFieldRelative, true);
 
 		// Display relevant data on shuffleboard.
-		widgetSpeedMode.setString(DriverConstants.maxSpeedOptionsNames[speedLevel.get()]);
-		widgetFieldRelative.setBoolean(isFieldRelative.get());
+		SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);
+		SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
 
 		// Position display
 		final Pose2d robotPosition = drivetrain.getPosition();
 
-		widgetPoseX.setDouble(robotPosition.getX());
-		widgetPoseY.setDouble(robotPosition.getY());
-		widgetPoseDegrees.setDouble(robotPosition.getRotation().getDegrees());
+		SmartDashboard.putNumber("PoseX", robotPosition.getX());
+		SmartDashboard.putNumber("PoseY", robotPosition.getY());
+		SmartDashboard.putNumber("PoseDegrees", robotPosition.getRotation().getDegrees());
 
 		// Speed and Heading
 		final ChassisSpeeds currentSpeeds = drivetrain.getState();
@@ -126,8 +102,8 @@ public void execute() {
 				.sqrt(Math.pow(currentSpeeds.vxMetersPerSecond, 2) + Math.pow(currentSpeeds.vyMetersPerSecond, 2));
 
 		final double metersPerSecondToMilesPerHourConversion = 2.237;
-		widgetTotalSpeed.setDouble(speedMetersPerSecond * metersPerSecondToMilesPerHourConversion);
-		widgetHeading.setDouble(drivetrain.getHeading().getDegrees());
+		SmartDashboard.putNumber("Robot Speed", speedMetersPerSecond * metersPerSecondToMilesPerHourConversion);
+		SmartDashboard.putNumber("Heading Degrees", drivetrain.getHeading().getDegrees());
 	}
 
 	/**
@@ -149,6 +125,6 @@ public boolean isFinished() {
 	public void end(boolean interrupted) {
 		drivetrain.stop();
 
-		// turn red
+		SmartDashboard.putBoolean("ControlActive", false);
 	}
 }
\ No newline at end of file

From 02c76ab71780334e20c5f9580dd3601680866004 Mon Sep 17 00:00:00 2001
From: Aceius E <aceiuselms@seattleacademy.org>
Date: Wed, 14 Feb 2024 17:02:54 -0800
Subject: [PATCH 57/99] Update icon

---
 shuffleboard.json | 17 +++++++++++++++--
 1 file changed, 15 insertions(+), 2 deletions(-)

diff --git a/shuffleboard.json b/shuffleboard.json
index 573cc3f..63450c8 100644
--- a/shuffleboard.json
+++ b/shuffleboard.json
@@ -97,8 +97,8 @@
               "_type": "Boolean Box",
               "_source0": "network_table:///SmartDashboard/ControlActive",
               "_title": "ControlActive",
-              "_glyph": 148,
-              "_showGlyph": false,
+              "_glyph": 392,
+              "_showGlyph": true,
               "Colors/Color when true": "#7CFC00FF",
               "Colors/Color when false": "#8B0000FF"
             }
@@ -240,6 +240,19 @@
         "titleType": 0,
         "tiles": {}
       }
+    },
+    {
+      "title": "Tab 5",
+      "autoPopulate": false,
+      "autoPopulatePrefix": "",
+      "widgetPane": {
+        "gridSize": 128.0,
+        "showGrid": true,
+        "hgap": 16.0,
+        "vgap": 16.0,
+        "titleType": 0,
+        "tiles": {}
+      }
     }
   ],
   "windowGeometry": {

From b024178ac624f843fa8ca2a86b81a962c3322801 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Wed, 14 Feb 2024 17:10:15 -0800
Subject: [PATCH 58/99] Remove newline

---
 src/main/java/frc/robot/subsystems/Arm.java | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 9ecd08c..4f2af06 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -2,15 +2,11 @@
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-// How to make Subsystem (ignore image instructions, code is out of date, just look at written general instructions): https://compendium.readthedocs.io/en/latest/tasks/subsystems/subsystems.html
-// Command based programming: https://docs.wpilib.org/en/stable/docs/software/commandbased/what-is-command-based.html
-// Subsystem Documentation documentation: https://docs.wpilib.org/en/stable/docs/software/commandbased/subsystems.html
 
 /**
  * How to make a Subsystem:
  * 1. Copy this file, remember that class name has to match
  */
-
 public class Arm extends SubsystemBase {
 	/** Constructor. Creates a new ExampleSubsystem. */
 	public Arm() {

From 3a2fcd83e11c9ff5c9237d43cbd89355282da5ef Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Wed, 14 Feb 2024 21:44:24 -0800
Subject: [PATCH 59/99] change template comments to be more helpful

---
 src/main/java/frc/robot/commands/DriveToPose.java    | 2 +-
 src/main/java/frc/robot/commands/DriveTransform.java | 4 +---
 src/main/java/frc/robot/subsystems/SwerveModule.java | 4 +---
 3 files changed, 3 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java
index e70ec3f..6a5bf18 100644
--- a/src/main/java/frc/robot/commands/DriveToPose.java
+++ b/src/main/java/frc/robot/commands/DriveToPose.java
@@ -10,7 +10,7 @@ public class DriveToPose extends Command {
 	private final Pose2d targetPosition;
 
     /**
-     * Create a new DriveToPose command. Tries to drive to a set Pose based on odometry 
+     * Create a new DriveToPose command. Uses setDesiredPosition on drivetrain.
      * 
      * <p>This drives relative to the robot starting position,
 	 * so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
diff --git a/src/main/java/frc/robot/commands/DriveTransform.java b/src/main/java/frc/robot/commands/DriveTransform.java
index 21987b5..5a4e5ba 100644
--- a/src/main/java/frc/robot/commands/DriveTransform.java
+++ b/src/main/java/frc/robot/commands/DriveTransform.java
@@ -29,14 +29,12 @@ public DriveTransform(SwerveDrivetrain drivetrain, Transform2d transform) {
 	}
 
 	/**
-	 * Create a new DriveToPose command. Tries to drive a certain transform (a
-	 * translation and a rotation) using the DriveToPose command
+	 * Create a new DriveToTransform command. Uses setDesiredPosition on drivetrain.
 	 * 
 	 * <p>
 	 * This drives relative to the robot, so a transform of +2x and +1y will drive 2
 	 * meters forward and 1 meter left,
 	 * where forward is whatever direction the robot is currently facing
-	 * </p>
 	 * 
 	 * @param drivetrain  The drivetrain of the robot
 	 * @param translation Target transform to drive
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 212e809..205dacd 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -23,9 +23,7 @@
  * Subsystem for individual swerve module on robot. Each swerve module has one
  * drive motor and one steering motor.
  * 
- * @see <a href=
- *      "https://www.swervedrivespecialties.com/products/mk4-swerve-module">Swerve
- *      Module Kit</a>
+ * @see <a href="https://www.swervedrivespecialties.com/products/mk4-swerve-module">Swerve Module Kit</a>
  */
 public class SwerveModule extends SubsystemBase {
 

From b58e567f4849c26a9c3839c7d82cc69f220806fe Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Thu, 15 Feb 2024 15:28:15 -0800
Subject: [PATCH 60/99] Get ready for testing today and improve control setting
 readability

---
 shuffleboard.json                             | 81 ++++++++++++++-----
 src/main/java/frc/robot/RobotContainer.java   | 38 +++++----
 ...Control.java => ChassisRemoteControl.java} | 40 +++------
 .../java/frc/robot/commands/DriveToPose.java  |  5 ++
 .../frc/robot/commands/DriveTransform.java    |  5 ++
 .../java/frc/robot/commands/FollowTag.java    | 17 ++--
 .../{utils => inputs}/ChassisDriveInputs.java |  3 +-
 .../OptionButtonInput.java}                   |  8 +-
 .../robot/subsystems/SwerveDrivetrain.java    | 33 ++++++++
 .../java/frc/robot/subsystems/Vision.java     |  2 +-
 10 files changed, 148 insertions(+), 84 deletions(-)
 rename src/main/java/frc/robot/commands/{DriverControl.java => ChassisRemoteControl.java} (65%)
 rename src/main/java/frc/robot/{utils => inputs}/ChassisDriveInputs.java (98%)
 rename src/main/java/frc/robot/{utils/OptionButton.java => inputs/OptionButtonInput.java} (91%)

diff --git a/shuffleboard.json b/shuffleboard.json
index 63450c8..f61c1aa 100644
--- a/shuffleboard.json
+++ b/shuffleboard.json
@@ -161,7 +161,7 @@
               1
             ],
             "content": {
-              "_type": "ComboBox Chooser",
+              "_type": "Split Button Chooser",
               "_source0": "network_table:///SmartDashboard/Auto Chooser",
               "_title": "Auto Chooser",
               "_glyph": 167,
@@ -213,8 +213,8 @@
             ],
             "content": {
               "_type": "Number Bar",
-              "_source0": "network_table:///SmartDashboard/Speed",
-              "_title": "Speed",
+              "_source0": "network_table:///SmartDashboard/Spin",
+              "_title": "Spin",
               "_glyph": 419,
               "_showGlyph": true,
               "Range/Min": -1.0,
@@ -224,6 +224,60 @@
               "Visuals/Show text": true,
               "Visuals/Orientation": "HORIZONTAL"
             }
+          },
+          "7,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/tPoseX",
+              "_title": "tPoseX",
+              "_glyph": 110,
+              "_showGlyph": true
+            }
+          },
+          "6,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/tPoseY",
+              "_title": "tPoseY",
+              "_glyph": 111,
+              "_showGlyph": true
+            }
+          },
+          "8,3": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Text View",
+              "_source0": "network_table:///SmartDashboard/tPoseDegrees",
+              "_title": "tPoseDegrees",
+              "_glyph": 419,
+              "_showGlyph": true
+            }
+          },
+          "8,2": {
+            "size": [
+              1,
+              1
+            ],
+            "content": {
+              "_type": "Boolean Box",
+              "_source0": "network_table:///SmartDashboard/tPoseActive",
+              "_title": "tPoseActive",
+              "_glyph": 288,
+              "_showGlyph": true,
+              "Colors/Color when true": "#7CFC00FF",
+              "Colors/Color when false": "#8B0000FF"
+            }
           }
         }
       }
@@ -240,25 +294,12 @@
         "titleType": 0,
         "tiles": {}
       }
-    },
-    {
-      "title": "Tab 5",
-      "autoPopulate": false,
-      "autoPopulatePrefix": "",
-      "widgetPane": {
-        "gridSize": 128.0,
-        "showGrid": true,
-        "hgap": 16.0,
-        "vgap": 16.0,
-        "titleType": 0,
-        "tiles": {}
-      }
     }
   ],
   "windowGeometry": {
-    "x": -7.0,
-    "y": -7.0,
-    "width": 1551.0,
-    "height": 831.0
+    "x": -7.199999809265137,
+    "y": -7.199999809265137,
+    "width": 1551.199951171875,
+    "height": 831.2000122070312
   }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 02eb759..4eb5c82 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -8,10 +8,10 @@
 import frc.robot.subsystems.SwerveModule;
 import frc.robot.subsystems.Vision;
 import frc.robot.commands.Autos;
-import frc.robot.commands.DriverControl;
-import frc.robot.utils.ChassisDriveInputs;
-import frc.robot.utils.OptionButton;
-import frc.robot.utils.OptionButton.ActivationMode;
+import frc.robot.commands.ChassisRemoteControl;
+import frc.robot.inputs.ChassisDriveInputs;
+import frc.robot.inputs.OptionButtonInput;
+import frc.robot.inputs.OptionButtonInput.ActivationMode;
 
 import com.kauailabs.navx.frc.AHRS;
 
@@ -101,42 +101,40 @@ public void setUpDriveController() {
 
 		drivetrain.removeDefaultCommand();
 
-		DriverControl control;
+		ChassisDriveInputs inputs;
+		OptionButtonInput preciseModeButton, boostModeButton, fieldRelativeButton;
 
 		if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
 			final CommandJoystick joystick = new CommandJoystick(genericHID.getPort());
-			control = new DriverControl(drivetrain,
-
-				new ChassisDriveInputs(
+			
+			inputs = new ChassisDriveInputs(
 					joystick::getY, -1,
 					joystick::getX, -1,
 					joystick::getTwist, -1,
-					Constants.DriverConstants.DEAD_ZONE),
+					Constants.DriverConstants.DEAD_ZONE);
 
-					new OptionButton(joystick, 2, ActivationMode.TOGGLE),
-					new OptionButton(joystick, 1, ActivationMode.HOLD),
-					new OptionButton(joystick, 3, ActivationMode.TOGGLE));
+			preciseModeButton = new OptionButtonInput(joystick, 2, ActivationMode.TOGGLE);
+			boostModeButton = new OptionButtonInput(joystick, 1, ActivationMode.HOLD);
+			fieldRelativeButton = new OptionButtonInput(joystick, 3, ActivationMode.TOGGLE);
 
 			joystick.button(10).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
 			joystick.button(11).onTrue(Commands.run(drivetrain::toDefaultStates, drivetrain));
 
 		} else {
 			final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());
-			control = new DriverControl(drivetrain,
 
-				new ChassisDriveInputs(
+			inputs = new ChassisDriveInputs(
 					xbox::getLeftY, +1,
 					xbox::getLeftX, +1,
 					xbox::getRightX, -1,
-					Constants.DriverConstants.DEAD_ZONE),
+					Constants.DriverConstants.DEAD_ZONE);
 
-				new OptionButton(xbox::b, ActivationMode.TOGGLE),
-				new OptionButton(xbox::leftStick, ActivationMode.HOLD),
-				new OptionButton(xbox::povUp, ActivationMode.TOGGLE)
-			);
+			preciseModeButton = new OptionButtonInput(xbox::b, ActivationMode.TOGGLE);
+			boostModeButton = new OptionButtonInput(xbox::leftStick, ActivationMode.HOLD);
+			fieldRelativeButton = new OptionButtonInput(xbox::povUp, ActivationMode.TOGGLE);
 		}
 
-		drivetrain.setDefaultCommand(control);
+		drivetrain.setDefaultCommand(new ChassisRemoteControl(drivetrain, inputs, preciseModeButton, boostModeButton, fieldRelativeButton));
 	}
 
 	/** Use this method to define your trigger->command mappings. */
diff --git a/src/main/java/frc/robot/commands/DriverControl.java b/src/main/java/frc/robot/commands/ChassisRemoteControl.java
similarity index 65%
rename from src/main/java/frc/robot/commands/DriverControl.java
rename to src/main/java/frc/robot/commands/ChassisRemoteControl.java
index be36840..02a4b53 100644
--- a/src/main/java/frc/robot/commands/DriverControl.java
+++ b/src/main/java/frc/robot/commands/ChassisRemoteControl.java
@@ -1,23 +1,21 @@
 package frc.robot.commands;
 
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.DriverConstants;
+import frc.robot.inputs.OptionButtonInput;
+import frc.robot.inputs.ChassisDriveInputs;
 import frc.robot.subsystems.SwerveDrivetrain;
-import frc.robot.utils.ChassisDriveInputs;
-import frc.robot.utils.OptionButton;
 
 /**
- * This can be the default command for the drivetrain, allowing for remote operation with a controller.
+ * This can be the default command for the drivetrain.
+ * It should allow the driver to control the robot, as well displaying relevant driver data to SmartDashboard
  */
-public class DriverControl extends Command {
+public class ChassisRemoteControl extends Command {
 	protected final SwerveDrivetrain drivetrain;
 
-	private final OptionButton preciseModeButton;
-	private final OptionButton boostModeButton;
-	private final OptionButton fieldRelativeButton;
+	private final OptionButtonInput preciseModeButton, boostModeButton, fieldRelativeButton;
 
 	private final ChassisDriveInputs chassisDriveInputs;
 
@@ -27,8 +25,8 @@ public class DriverControl extends Command {
 	 * @param drivetrain       The drivetrain of the robot
 	 * @param driverController The device used to control drivetrain
 	 */
-	public DriverControl(SwerveDrivetrain drivetrain, ChassisDriveInputs chassisDriveInputs,
-			OptionButton preciseModeButton, OptionButton boostModeButton, OptionButton fieldRelativeButton) {
+	public ChassisRemoteControl(SwerveDrivetrain drivetrain, ChassisDriveInputs chassisDriveInputs,
+			OptionButtonInput preciseModeButton, OptionButtonInput boostModeButton, OptionButtonInput fieldRelativeButton) {
 
 		this.chassisDriveInputs = chassisDriveInputs;
 
@@ -78,9 +76,9 @@ public void execute() {
 				speedRotation * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
 
 
-		SmartDashboard.putNumber("SpeedX", speedX);
-		SmartDashboard.putNumber("SpeedY", speedY);
-		SmartDashboard.putNumber("Speed", speedRotation);
+		SmartDashboard.putNumber("SpeedX", speeds.vxMetersPerSecond);
+		SmartDashboard.putNumber("SpeedY", speeds.vyMetersPerSecond);
+		SmartDashboard.putNumber("Spin", speeds.omegaRadiansPerSecond);
 
 		drivetrain.setDesiredState(speeds, isFieldRelative, true);
 
@@ -88,21 +86,7 @@ public void execute() {
 		SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);
 		SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
 
-		// Position display
-		final Pose2d robotPosition = drivetrain.getPosition();
-
-		SmartDashboard.putNumber("PoseX", robotPosition.getX());
-		SmartDashboard.putNumber("PoseY", robotPosition.getY());
-		SmartDashboard.putNumber("PoseDegrees", robotPosition.getRotation().getDegrees());
-
-		// Speed and Heading
-		final ChassisSpeeds currentSpeeds = drivetrain.getState();
-		final double speedMetersPerSecond = Math
-				.sqrt(Math.pow(currentSpeeds.vxMetersPerSecond, 2) + Math.pow(currentSpeeds.vyMetersPerSecond, 2));
-
-		final double metersPerSecondToMilesPerHourConversion = 2.237;
-		SmartDashboard.putNumber("Robot Speed", speedMetersPerSecond * metersPerSecondToMilesPerHourConversion);
-		SmartDashboard.putNumber("Heading Degrees", drivetrain.getHeading().getDegrees());
+		drivetrain.updateSmartDashboard();
 	}
 
 	/**
diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java
index 6a5bf18..7b62832 100644
--- a/src/main/java/frc/robot/commands/DriveToPose.java
+++ b/src/main/java/frc/robot/commands/DriveToPose.java
@@ -36,6 +36,11 @@ public boolean isFinished() {
 		return drivetrain.isAtDesiredPosition();
 	}
 
+	@Override
+	public void execute() {
+		drivetrain.updateSmartDashboard();
+	}
+
 	@Override
 	public void end(boolean interrupted) {
 		drivetrain.clearDesiredPosition();
diff --git a/src/main/java/frc/robot/commands/DriveTransform.java b/src/main/java/frc/robot/commands/DriveTransform.java
index 5a4e5ba..19a83b9 100644
--- a/src/main/java/frc/robot/commands/DriveTransform.java
+++ b/src/main/java/frc/robot/commands/DriveTransform.java
@@ -49,6 +49,11 @@ public void initialize() {
 		drivetrain.setDesiredPosition(drivetrain.getPosition().plus(transform));
 	}
 
+	@Override
+	public void execute() {
+		drivetrain.updateSmartDashboard();
+	}
+
 	@Override
 	public boolean isFinished() {
 		return drivetrain.isAtDesiredPosition();
diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java
index da00b63..098d481 100644
--- a/src/main/java/frc/robot/commands/FollowTag.java
+++ b/src/main/java/frc/robot/commands/FollowTag.java
@@ -17,10 +17,10 @@ public class FollowTag extends Command {
 	private final SwerveDrivetrain drivetrain;
 
 	private final Vision vision;
-	private final Integer tagID;
+	private final Integer tagID;  // Integer as opposed to int so it can be null for best tag
 	private final Transform2d targetDistance;
 
-	private final Double loseTagAfterSeconds;
+	private final Double loseTagAfterSeconds;  // Double as opposed to double so it can be null for never lose mode.
 	private double secondsSinceTagLastSeen;
 
 	/**
@@ -29,11 +29,9 @@ public class FollowTag extends Command {
 	 * 
 	 * @param drivetrain          the drivetrain of the robot
 	 * @param vision              the vision subsystem of the robot
-	 * @param tagID               the numerical ID of the the tag to follow, null
-	 *                            for whatever best is
+	 * @param tagID               the numerical ID of the the tag to follow, null for best tag
 	 * @param targetDistanceToTag the target distance away from the tag to be
-	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover
-	 *                            tag, set to null to never finish
+	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover tag, set to null to never finish
 	 */
 	public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Transform2d targetDistanceToTag, Integer tagID,
 			Double loseTagAfterSeconds) {
@@ -73,12 +71,11 @@ public void initialize() {
 
 	@Override
 	public void execute() {
-
+		
 		final PhotonTrackedTarget tag = (tagID == null) ? vision.getTag() : vision.getTag(tagID);
 
 		if (tag == null) {
 			secondsSinceTagLastSeen += TimedRobot.kDefaultPeriod;
-
 			drivetrain.stop();
 		} else {
 			final Transform3d tagPosition3d = vision.getDistanceToTarget(tag);
@@ -90,12 +87,14 @@ public void execute() {
 			final Transform2d tagPosition = new Transform2d(
 					tagPosition3d.getZ(),
 					tagPosition3d.getX(),
-					Rotation2d.fromRadians(-tag.getYaw()));
+					Rotation2d.fromRadians(tag.getYaw()));
 
 			final Transform2d driveTransform = tagPosition.plus(targetDistance.inverse());
 
 			drivetrain.setDesiredPosition(drivetrain.getPosition().plus(driveTransform));
 		}
+
+		drivetrain.updateSmartDashboard();
 	}
 
 	@Override
diff --git a/src/main/java/frc/robot/utils/ChassisDriveInputs.java b/src/main/java/frc/robot/inputs/ChassisDriveInputs.java
similarity index 98%
rename from src/main/java/frc/robot/utils/ChassisDriveInputs.java
rename to src/main/java/frc/robot/inputs/ChassisDriveInputs.java
index 400b4bc..59b76b8 100644
--- a/src/main/java/frc/robot/utils/ChassisDriveInputs.java
+++ b/src/main/java/frc/robot/inputs/ChassisDriveInputs.java
@@ -1,4 +1,4 @@
-package frc.robot.utils;
+package frc.robot.inputs;
 
 import java.util.function.Supplier;
 
@@ -11,7 +11,6 @@ public class ChassisDriveInputs {
 
 	private final double deadzone;
 
-
 	/**
 	 * Create a new ChassisDriveInputs
 	 * 
diff --git a/src/main/java/frc/robot/utils/OptionButton.java b/src/main/java/frc/robot/inputs/OptionButtonInput.java
similarity index 91%
rename from src/main/java/frc/robot/utils/OptionButton.java
rename to src/main/java/frc/robot/inputs/OptionButtonInput.java
index 6852cc1..73d2e5e 100644
--- a/src/main/java/frc/robot/utils/OptionButton.java
+++ b/src/main/java/frc/robot/inputs/OptionButtonInput.java
@@ -1,4 +1,4 @@
-package frc.robot.utils;
+package frc.robot.inputs;
 
 import java.util.function.Supplier;
 
@@ -9,7 +9,7 @@
 /**
  * Button with different types of control
  */
-public class OptionButton {
+public class OptionButtonInput {
 
 	private final Supplier<Trigger> button;
 
@@ -42,7 +42,7 @@ public static enum ActivationMode {
 	 * @param mode          whether we want button to act as toggle or hold
 	 *                      button
 	 */
-	public OptionButton(CommandGenericHID controller, int button, ActivationMode mode) {
+	public OptionButtonInput(CommandGenericHID controller, int button, ActivationMode mode) {
 		this(() -> controller.button(button), mode);
 	}
 
@@ -52,7 +52,7 @@ public OptionButton(CommandGenericHID controller, int button, ActivationMode mod
 	 * @param button        button number
 	 * @param mode          whether we want button to act as toggle or hold button
 	 */
-	public OptionButton(Supplier<Trigger> button, ActivationMode mode) {
+	public OptionButtonInput(Supplier<Trigger> button, ActivationMode mode) {
 		this.button = button;
 		this.mode = mode;
 
diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index fa6df0a..b2668c7 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -18,6 +18,7 @@
 import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.RobotMovementConstants;
 
@@ -179,6 +180,11 @@ public void setDesiredPosition(Pose2d desiredPose) {
 		this.desiredPose = desiredPose;
 	}
 
+	/** Gets the desired position, returns null if the pose has been cleared */
+	public Pose2d getDesiredPose() {
+		return desiredPose;
+	}
+
 	/** Sets desired position to null, stops robot from continue to try and get to the last set pose  */
 	public void clearDesiredPosition() {
 		setDesiredPosition(null);
@@ -342,6 +348,33 @@ public Rotation3d getHeading3d() {
 
 	// --- Util ---
 
+	/** Update SmartDashboard with */
+	public void updateSmartDashboard() {
+		// Position display
+		final Pose2d robotPosition = getPosition();
+
+		SmartDashboard.putNumber("PoseX", robotPosition.getX());
+		SmartDashboard.putNumber("PoseY", robotPosition.getY());
+		SmartDashboard.putNumber("PoseDegrees", robotPosition.getRotation().getDegrees());
+
+		// Speed and Heading
+		final ChassisSpeeds currentSpeeds = getState();
+		final double speedMetersPerSecond = Math.sqrt(Math.pow(currentSpeeds.vxMetersPerSecond, 2) + Math.pow(currentSpeeds.vyMetersPerSecond, 2));
+
+		final double metersPerSecondToMilesPerHourConversion = 2.237;
+		SmartDashboard.putNumber("Robot Speed", speedMetersPerSecond * metersPerSecondToMilesPerHourConversion);
+		SmartDashboard.putNumber("Heading Degrees", getHeading().getDegrees());
+
+
+		final boolean hasTargetPose = desiredPose != null;
+		final Pose2d targetPose = hasTargetPose ? desiredPose : new Pose2d();
+
+		SmartDashboard.putBoolean("tPoseActive", hasTargetPose);
+		SmartDashboard.putNumber("tPoseX", targetPose.getX());
+		SmartDashboard.putNumber("tPoseY", targetPose.getY());
+		SmartDashboard.putNumber("tPoseDegrees", targetPose.getRotation().getDegrees());
+	}
+
 	/**
 	 * Utility method. Function to easily run a function on each swerve module
 	 * 
diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java
index 41a8115..c685379 100644
--- a/src/main/java/frc/robot/subsystems/Vision.java
+++ b/src/main/java/frc/robot/subsystems/Vision.java
@@ -15,7 +15,7 @@
 /** Vision subsystem */
 public class Vision extends SubsystemBase {
 
-	private final static boolean DEBUG_INFO = true;
+	private final static boolean DEBUG_INFO = false;
 
 	final PhotonCamera camera;
 	final Transform3d cameraToFrontCenter;

From 95e1e186fa44ee2e74b35614b61ee9851e15f7bc Mon Sep 17 00:00:00 2001
From: judeconnolly <judeconnolly@seattleacademy.org>
Date: Thu, 15 Feb 2024 15:43:02 -0800
Subject: [PATCH 61/99] separate arm container + PID constants for arm

---
 src/main/java/frc/robot/Constants.java        |  6 +-
 src/main/java/frc/robot/RobotContainer.java   | 33 ++++++--
 .../java/frc/robot/commands/ArmControl.java   | 77 +++++++++++++++++++
 .../frc/robot/commands/DriverControl.java     | 19 +----
 src/main/java/frc/robot/subsystems/Arm.java   |  6 +-
 5 files changed, 113 insertions(+), 28 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/ArmControl.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index f892c87..6f32bf9 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -77,9 +77,9 @@ public static class ArmConstants {
 
         public static final double DEGREES_PER_SECOND = 2.0;
 
-		public static final double ROTATION_PID_P = 0;
-		public static final double ROTATION_PID_I = 0;
-		public static final double ROTATION_PID_D = 0;
+		public static final double ELEVATION_PID_P = 0;
+		public static final double ELEVATION_PID_I = 0;
+		public static final double ELEVATION_PID_D = 0;
 
     }
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 274d6cd..fc9b6ed 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -6,6 +6,7 @@
 import frc.robot.Constants.ArmConstants;
 import frc.robot.subsystems.Arm;
 import frc.robot.commands.Autos;
+import frc.robot.commands.ArmControl;
 import frc.robot.commands.DriverControl;
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.SwerveModule;
@@ -17,6 +18,7 @@
 
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.GenericHID.HIDType;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -98,10 +100,11 @@ public void setUpDriveController() {
 		drivetrain.removeDefaultCommand();
 
 		DriverControl control;
+		ArmControl armcontrol;
 
 		if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
 			final CommandJoystick joystick = new CommandJoystick(genericHID.getPort());
-			control = new DriverControl(drivetrain, arm,
+			control = new DriverControl(drivetrain,
 
 				new ChassisDriveInputs(
 					joystick::getX, joystick::getY, joystick::getTwist,
@@ -109,17 +112,25 @@ public void setUpDriveController() {
 
 				new OptionButton(joystick, 2, ActivationMode.TOGGLE),
 				new OptionButton(joystick, 1, ActivationMode.HOLD),
-				new OptionButton(joystick, 3, ActivationMode.TOGGLE),
-				
+				new OptionButton(joystick, 3, ActivationMode.TOGGLE)
+
+			);
+
+			armcontrol = new ArmControl(arm,
+
 				new OptionButton(joystick,11,ActivationMode.HOLD),
-				new OptionButton(joystick,12,ActivationMode.HOLD)
+				new OptionButton(joystick, 12, ActivationMode.HOLD),
+
+				new OptionButton(joystick::povLeft, ActivationMode.HOLD),
+				new OptionButton(joystick::povRight, ActivationMode.HOLD),
+				new OptionButton(joystick::povDown, ActivationMode.HOLD)
 			);
 			
 			joystick.button(4).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
 
 		} else {
 			final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());
-			control = new DriverControl(drivetrain, arm,
+			control = new DriverControl(drivetrain, 
 
 				new ChassisDriveInputs(
 					xbox::getLeftX, xbox::getLeftY, xbox::getRightX,
@@ -127,10 +138,18 @@ public void setUpDriveController() {
 
 				new OptionButton(xbox::b, ActivationMode.TOGGLE),
 				new OptionButton(xbox::leftStick, ActivationMode.HOLD),
-				new OptionButton(xbox::povUp, ActivationMode.TOGGLE),
+				new OptionButton(xbox::povUp, ActivationMode.TOGGLE)
+
+			);
+
+			armcontrol = new ArmControl(arm,
 
 				new OptionButton(xbox::rightBumper,ActivationMode.HOLD),
-				new OptionButton(xbox::leftBumper,ActivationMode.HOLD)
+				new OptionButton(xbox::leftBumper, ActivationMode.HOLD),
+
+				new OptionButton(xbox::povLeft, ActivationMode.HOLD),
+				new OptionButton(xbox::povRight, ActivationMode.HOLD),
+				new OptionButton(xbox::povDown, ActivationMode.HOLD)
 			);
 
 
diff --git a/src/main/java/frc/robot/commands/ArmControl.java b/src/main/java/frc/robot/commands/ArmControl.java
new file mode 100644
index 0000000..a2b4a4e
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ArmControl.java
@@ -0,0 +1,77 @@
+package frc.robot.commands;
+
+import frc.robot.Constants.ArmConstants;
+import frc.robot.subsystems.Arm;
+import frc.robot.subsystems.ExampleSubsystem;
+import frc.robot.utils.OptionButton;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+
+
+public class ArmControl extends Command {
+	private final Arm arm;
+
+	private final OptionButton raiseArmButton;
+	private final OptionButton lowerArmButton;
+
+    private final OptionButton speakerPositionButton;
+    private final OptionButton ampPositionButton;
+    private final OptionButton intakePositionButton;
+
+
+	public ArmControl(Arm arm, OptionButton raiseArmButton, OptionButton lowerArmButton,
+            OptionButton speakerPositionButton, OptionButton ampPositionButton, OptionButton intakePositionButton) {
+        this.arm = arm;
+
+        this.raiseArmButton = raiseArmButton;
+        this.lowerArmButton = lowerArmButton;
+
+        this.speakerPositionButton = speakerPositionButton;
+        this.ampPositionButton = ampPositionButton;
+        this.intakePositionButton = intakePositionButton;
+
+
+		addRequirements(arm);
+	}
+
+
+	
+	@Override
+	public void initialize() {
+	}
+
+	/**
+	 * execute() is called repeatedly while a command is scheduled, about every
+	 * 20ms.
+	 */
+	@Override
+	public void execute() {
+
+        // Arm Motor
+		arm.changeArmAngleDegreesBy(Double.valueOf(raiseArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
+        arm.changeArmAngleDegreesBy(Double.valueOf(-lowerArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
+
+        // Arm Povs
+        if (speakerPositionButton.getState()) {
+            arm.setArmToSpeakerPosition();
+        }
+        if (ampPositionButton.getState()) {
+            arm.setArmToAmpPosition();
+        }
+        if (intakePositionButton.getState()) {
+            arm.setArmToIntakePosition();
+        }
+
+	}
+
+    @Override
+	public boolean isFinished() {
+		return true;
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+	}
+}
diff --git a/src/main/java/frc/robot/commands/DriverControl.java b/src/main/java/frc/robot/commands/DriverControl.java
index e3698e7..0024e41 100644
--- a/src/main/java/frc/robot/commands/DriverControl.java
+++ b/src/main/java/frc/robot/commands/DriverControl.java
@@ -18,26 +18,20 @@
 public class DriverControl extends Command {
 	private final SwerveDrivetrain drivetrain;
 
-	private final Arm arm;
-
 	private final OptionButton preciseModeButton;
 	private final OptionButton boostModeButton;
 	private final OptionButton fieldRelativeButton;
 
 	private final ChassisDriveInputs chassisDriveInputs;
 
-	private final OptionButton raiseArmButton;
-    private final OptionButton lowerArmButton;
-
 	/**
 	 * Creates a new SwerveDriveBaseControl Command.
 	 *
 	 * @param drivetrain       The drivetrain of the robot
 	 * @param driverController The device used to control drivetrain
 	 */
-	public DriverControl(SwerveDrivetrain drivetrain, Arm arm, ChassisDriveInputs chassisDriveInputs,
-			OptionButton preciseModeButton, OptionButton boostModeButton, OptionButton fieldRelativeButton,
-			OptionButton raiseArmButton, OptionButton lowerArmButton) {
+	public DriverControl(SwerveDrivetrain drivetrain,  ChassisDriveInputs chassisDriveInputs,
+			OptionButton preciseModeButton, OptionButton boostModeButton, OptionButton fieldRelativeButton) {
 
 		this.chassisDriveInputs = chassisDriveInputs;
 
@@ -47,10 +41,7 @@ public DriverControl(SwerveDrivetrain drivetrain, Arm arm, ChassisDriveInputs ch
 
 		this.drivetrain = drivetrain;
 
-		this.arm = arm;
-
-		this.raiseArmButton = raiseArmButton;
-		this.lowerArmButton = lowerArmButton;
+		
 
 		
 
@@ -120,9 +111,7 @@ public void execute() {
 		SmartDashboard.putNumber("Robot Speed", speedMetersPerSecond * metersPerSecondToMilesPerHourConversion);
 		SmartDashboard.putNumber("Heading Degrees", drivetrain.getHeading().getDegrees());
 
-		// Arm Motor
-		arm.changeArmAngleDegreesBy(Double.valueOf(raiseArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
-        arm.changeArmAngleDegreesBy(Double.valueOf(-lowerArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
+		
 	}
 
 	/**
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 0245485..972912f 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -43,9 +43,9 @@ public Arm(int leftMotorId, int rightMotorId, int rightEncoderId) {
         rightArmEncoder = new CANcoder(rightEncoderId);
 
         armRaisePIDController = new PIDController(
-            0,
-            0,
-            0
+            ArmConstants.ELEVATION_PID_P,
+            ArmConstants.ELEVATION_PID_I,
+            ArmConstants.ELEVATION_PID_D
         );
 
         armPosition = rightArmEncoder.getAbsolutePosition();

From 1ee510e9e48e8715fbac23d43b43bdb52786c97e Mon Sep 17 00:00:00 2001
From: judeconnolly <judeconnolly@seattleacademy.org>
Date: Thu, 15 Feb 2024 16:44:15 -0800
Subject: [PATCH 62/99] main merge cleanup

---
 src/main/java/frc/robot/RobotContainer.java      |  2 +-
 src/main/java/frc/robot/commands/ArmControl.java | 16 ++++++++--------
 2 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 5c4ac14..2bd9aa5 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -7,7 +7,7 @@
 import frc.robot.subsystems.Arm;
 import frc.robot.commands.Autos;
 import frc.robot.commands.ArmControl;
-import frc.robot.commands.DriverControl;
+import frc.robot.commands.ChassisRemoteControl;
 import frc.robot.Constants.VisionConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.SwerveModule;
diff --git a/src/main/java/frc/robot/commands/ArmControl.java b/src/main/java/frc/robot/commands/ArmControl.java
index a2b4a4e..0c50d9c 100644
--- a/src/main/java/frc/robot/commands/ArmControl.java
+++ b/src/main/java/frc/robot/commands/ArmControl.java
@@ -3,7 +3,7 @@
 import frc.robot.Constants.ArmConstants;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.ExampleSubsystem;
-import frc.robot.utils.OptionButton;
+import frc.robot.inputs.OptionButtonInput;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
@@ -13,16 +13,16 @@
 public class ArmControl extends Command {
 	private final Arm arm;
 
-	private final OptionButton raiseArmButton;
-	private final OptionButton lowerArmButton;
+	private final OptionButtonInput raiseArmButton;
+	private final OptionButtonInput lowerArmButton;
 
-    private final OptionButton speakerPositionButton;
-    private final OptionButton ampPositionButton;
-    private final OptionButton intakePositionButton;
+    private final OptionButtonInput speakerPositionButton;
+    private final OptionButtonInput ampPositionButton;
+    private final OptionButtonInput intakePositionButton;
 
 
-	public ArmControl(Arm arm, OptionButton raiseArmButton, OptionButton lowerArmButton,
-            OptionButton speakerPositionButton, OptionButton ampPositionButton, OptionButton intakePositionButton) {
+	public ArmControl(Arm arm, OptionButtonInput raiseArmButton, OptionButtonInput lowerArmButton,
+            OptionButtonInput speakerPositionButton, OptionButtonInput ampPositionButton, OptionButtonInput intakePositionButton) {
         this.arm = arm;
 
         this.raiseArmButton = raiseArmButton;

From 48a1c460ee1840cfd9d0b8a66f50a30e641b163a Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Thu, 15 Feb 2024 17:25:59 -0800
Subject: [PATCH 63/99] Get auto rotate to working within 1 degree

---
 src/main/java/frc/robot/Constants.java        | 12 ++---
 src/main/java/frc/robot/RobotContainer.java   | 14 ++---
 .../java/frc/robot/commands/AutoDriveTo.java  |  3 +-
 .../java/frc/robot/commands/AutoRotateTo.java |  8 +--
 src/main/java/frc/robot/commands/Autos.java   |  9 ++--
 .../java/frc/robot/commands/DriveToPose.java  | 53 ++++++++++++-------
 .../robot/subsystems/SwerveDrivetrain.java    | 13 +++--
 .../frc/robot/subsystems/SwerveModule.java    |  6 ++-
 8 files changed, 73 insertions(+), 45 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 204ca87..329f92d 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -66,15 +66,15 @@ public static class DriverConstants {
 	}
 
 	public static class RobotMovementConstants {
-		public static final double AT_SETPOINT_TOLERANCE_TIME_SECONDS = 1;
+		public static final double AT_SETPOINT_TOLERANCE_TIME_SECONDS = 0;
 		public static final double ROTATE_AT_SETPOINT_TIME_SECONDS = 1;
 
 		public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(0.0001);
-		public static final double ANGLE_TOLERANCE_RADIANS = Units.degreesToRadians(5);
+		public static final double ANGLE_TOLERANCE_RADIANS = Units.degreesToRadians(1);
 
-		public static final double ROTATION_PID_P = 0.5;
-		public static final double ROTATION_PID_I = 0;
-		public static final double ROTATION_PID_D = 0;
+		public static final double ROTATION_PID_P = 4;
+		public static final double ROTATION_PID_I = 0.1;
+		public static final double ROTATION_PID_D = 4;
 
 		public static final double TRANSLATION_PID_P = 30;
 		public static final double TRANSLATION_PID_I = 0.5;
@@ -170,7 +170,7 @@ public static class SwerveModuleConstants {
 		public static final double DRIVE_PID_MAX_I = 0.001;
 
 		// Steering PID values
-		public static final double STEERING_PID_P = 0.5;
+		public static final double STEERING_PID_P = 1;
 		public static final double STEERING_PID_I = 0;
 		public static final double STEERING_PID_D = 0;
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 4eb5c82..510525a 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -3,10 +3,8 @@
 import frc.robot.Constants.DriverConstants;
 import frc.robot.Constants.SwerveDrivetrainConstants;
 import frc.robot.Constants.SwerveModuleConstants;
-import frc.robot.Constants.VisionConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.SwerveModule;
-import frc.robot.subsystems.Vision;
 import frc.robot.commands.Autos;
 import frc.robot.commands.ChassisRemoteControl;
 import frc.robot.inputs.ChassisDriveInputs;
@@ -74,14 +72,14 @@ public class RobotContainer {
 
 	private final SendableChooser<Command> autoChooser = new SendableChooser<Command>();
 
-	private final Vision vision = new Vision(VisionConstants.CAMERA_NAME, Constants.VisionConstants.CAMERA_POSE);
+	// private final Vision vision = new Vision(VisionConstants.CAMERA_NAME, Constants.VisionConstants.CAMERA_POSE);
 
 	/**
 	 * The container for the robot. Contains subsystems, OI devices, and commands.
 	 */
 	public RobotContainer() {
 		autoChooser.setDefaultOption("Testing Auto", Autos.testingAuto(drivetrain));
-		autoChooser.addOption("Follow Tag", Autos.tagFollowAuto(drivetrain, vision, 1));
+		// autoChooser.addOption("Follow Tag", Autos.tagFollowAuto(drivetrain, vision, 1));
 		SmartDashboard.putData("Auto Chooser", autoChooser);
 
 		configureBindings();
@@ -108,8 +106,8 @@ public void setUpDriveController() {
 			final CommandJoystick joystick = new CommandJoystick(genericHID.getPort());
 			
 			inputs = new ChassisDriveInputs(
-					joystick::getY, -1,
 					joystick::getX, -1,
+					joystick::getY, -1,
 					joystick::getTwist, -1,
 					Constants.DriverConstants.DEAD_ZONE);
 
@@ -117,8 +115,10 @@ public void setUpDriveController() {
 			boostModeButton = new OptionButtonInput(joystick, 1, ActivationMode.HOLD);
 			fieldRelativeButton = new OptionButtonInput(joystick, 3, ActivationMode.TOGGLE);
 
-			joystick.button(10).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
-			joystick.button(11).onTrue(Commands.run(drivetrain::toDefaultStates, drivetrain));
+			joystick.button(10).onTrue(Commands.sequence(
+				Commands.runOnce(drivetrain::toDefaultStates, drivetrain),
+				Commands.waitSeconds(0.5)
+			));
 
 		} else {
 			final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());
diff --git a/src/main/java/frc/robot/commands/AutoDriveTo.java b/src/main/java/frc/robot/commands/AutoDriveTo.java
index d255f2b..90f92dc 100644
--- a/src/main/java/frc/robot/commands/AutoDriveTo.java
+++ b/src/main/java/frc/robot/commands/AutoDriveTo.java
@@ -94,11 +94,12 @@ public void execute() {
 				0);
 
 		drivetrain.setDesiredState(speeds);
+		drivetrain.updateSmartDashboard();
 	}
 
 	@Override
 	public boolean isFinished() {
-		return atSetpointCounter > 0;
+		return atSetpointCounter > RobotMovementConstants.AT_SETPOINT_TOLERANCE_TIME_SECONDS;
 	}
 
 	@Override
diff --git a/src/main/java/frc/robot/commands/AutoRotateTo.java b/src/main/java/frc/robot/commands/AutoRotateTo.java
index 3c570cc..beadce4 100644
--- a/src/main/java/frc/robot/commands/AutoRotateTo.java
+++ b/src/main/java/frc/robot/commands/AutoRotateTo.java
@@ -47,10 +47,10 @@ public void execute() {
 
 		drivetrain.setDesiredState(new ChassisSpeeds(0, 0, turnsSeed));
 
-		if (Math.abs(currentAngle - this.angleGoal) < RobotMovementConstants.ANGLE_TOLERANCE_RADIANS)
-			atSetpointCounter += TimedRobot.kDefaultPeriod;
-		else
-			atSetpointCounter = 0;
+		if (Math.abs(currentAngle - this.angleGoal) < RobotMovementConstants.ANGLE_TOLERANCE_RADIANS) atSetpointCounter += TimedRobot.kDefaultPeriod;
+		else atSetpointCounter = 0;
+		
+		drivetrain.updateSmartDashboard();
 	}
 
 	@Override
diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java
index c336959..43f0438 100644
--- a/src/main/java/frc/robot/commands/Autos.java
+++ b/src/main/java/frc/robot/commands/Autos.java
@@ -9,12 +9,15 @@
 import frc.robot.Constants.SwerveDrivetrainConstants;
 import frc.robot.subsystems.Arm;
 
-/** This class just contains a bunch of auto-modes. Do not call this class itself. */
+/**
+ * This class just contains a bunch of auto-modes. Do not call this class
+ * itself.
+ */
 public final class Autos {
 	/** Example static factory for an autonomous command. */
 	public static Command testingAuto(SwerveDrivetrain drivetrain) {
 		return Commands.sequence(
-				new DriveTransform(drivetrain, new Translation2d(1, 0), new Rotation2d())
+				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(0))
 		);
 	}
 
@@ -23,7 +26,7 @@ public static Command tagFollowAuto(SwerveDrivetrain drivetrain, Vision camera,
 		return new FollowTag(drivetrain, camera, new Translation2d(1, 0), tagId, null);
 	}
 
-	/**  Linden did this */
+	/** Linden did this */
 	public static Command startingAuto(Arm arm, SwerveDrivetrain drivetrain, boolean invertY) {
 
 		// assumes start position in corner
diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java
index 7b62832..ecfa4bb 100644
--- a/src/main/java/frc/robot/commands/DriveToPose.java
+++ b/src/main/java/frc/robot/commands/DriveToPose.java
@@ -1,6 +1,9 @@
 package frc.robot.commands;
 
 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.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.SwerveDrivetrain;
 
@@ -9,40 +12,54 @@ public class DriveToPose extends Command {
 	private final SwerveDrivetrain drivetrain;
 	private final Pose2d targetPosition;
 
-    /**
-     * Create a new DriveToPose command. Uses setDesiredPosition on drivetrain.
-     * 
-     * <p>This drives relative to the robot starting position,
-	 * so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
-     * where forward is whatever direction the robot started in</p>
-     * 
-     * <p>The last place the drivetrain position was reset counts as the starting position</p>
-     * 
-     * @param drivetrain the drivetrain of the robot
-     * @param targetPosition the pose the robot is trying to achieve
-     */
-    public DriveToPose(SwerveDrivetrain drivetrain, Pose2d targetPosition) {
+	/**
+	 * Create a new DriveToPose command. Uses setDesiredPosition on drivetrain.
+	 * 
+	 * <p>
+	 * This drives relative to the robot starting position,
+	 * so a pose of +2x and +1y will drive to the position 2 meters forward and 1
+	 * meter left of whether the robot started,
+	 * where forward is whatever direction the robot started in
+	 * </p>
+	 * 
+	 * <p>
+	 * The last place the drivetrain position was reset counts as the starting
+	 * position
+	 * </p>
+	 * 
+	 * @param drivetrain     the drivetrain of the robot
+	 * @param targetPosition the pose the robot is trying to achieve
+	 */
+	public DriveToPose(SwerveDrivetrain drivetrain, Pose2d targetPosition) {
 		this.drivetrain = drivetrain;
 		this.targetPosition = targetPosition;
-    }
+		SmartDashboard.putBoolean("Made shit", true);
+	}
+
+	public DriveToPose(SwerveDrivetrain drivetrain, Translation2d translation, Rotation2d rotation) {
+		this(drivetrain, new Pose2d(translation, rotation));
+	}
 
 	@Override
 	public void initialize() {
+		SmartDashboard.putBoolean("Doing shit", true);
 		drivetrain.setDesiredPosition(targetPosition);
 	}
 
 	@Override
-	public boolean isFinished() {
-		return drivetrain.isAtDesiredPosition();
+	public void execute() {
+		drivetrain.updateSmartDashboard();
 	}
 
 	@Override
-	public void execute() {
-		drivetrain.updateSmartDashboard();
+	public boolean isFinished() {
+		// return drivetrain.isAtDesiredPosition();
+		return false;
 	}
 
 	@Override
 	public void end(boolean interrupted) {
+		SmartDashboard.putBoolean("Doing shit", false);
 		drivetrain.clearDesiredPosition();
 		drivetrain.stop();
 	}
diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index b2668c7..06f8916 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -141,6 +141,10 @@ public void periodic() {
 					xController.calculate(pose.getX(), desiredPose.getX()),
 					yController.calculate(pose.getY(), desiredPose.getY()),
 					rotationController.calculate(pose.getRotation().getRadians(), desiredPose.getRotation().getRadians()));
+			
+			SmartDashboard.putNumber("SpeedX", speeds.vxMetersPerSecond);
+			SmartDashboard.putNumber("SpeedY", speeds.vyMetersPerSecond);
+			SmartDashboard.putNumber("Spin", speeds.omegaRadiansPerSecond);
 
 			// Set those speeds
 			setDesiredState(speeds);
@@ -368,11 +372,12 @@ public void updateSmartDashboard() {
 
 		final boolean hasTargetPose = desiredPose != null;
 		final Pose2d targetPose = hasTargetPose ? desiredPose : new Pose2d();
-
 		SmartDashboard.putBoolean("tPoseActive", hasTargetPose);
-		SmartDashboard.putNumber("tPoseX", targetPose.getX());
-		SmartDashboard.putNumber("tPoseY", targetPose.getY());
-		SmartDashboard.putNumber("tPoseDegrees", targetPose.getRotation().getDegrees());
+		if (hasTargetPose) {
+			SmartDashboard.putNumber("tPoseX", targetPose.getX());
+			SmartDashboard.putNumber("tPoseY", targetPose.getY());
+			SmartDashboard.putNumber("tPoseDegrees", targetPose.getRotation().getDegrees());
+		}
 	}
 
 	/**
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 205dacd..14edddb 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -183,13 +183,15 @@ public SwerveModuleState getState() {
 	 * @param powerDriveMode Whether the SwerveModuleState is in meters per second (false) or motor power (true)
 	 */
 	public void setDesiredState(SwerveModuleState state, boolean powerDriveMode) {
-
+		
 		// If state is null, then stop robot and don't set any states
 		if (state == null) {
 			stop();
 			return;
 		}
-
+		
+		stopped = false;
+		
 		// Optimize the reference state to avoid spinning further than 90 degrees
 		state = optimize(state, Rotation2d.fromRotations(getSteeringAngleRotations()));
 

From 93e6fe7cd078c9614b8b50e87446510f39e0d2fe Mon Sep 17 00:00:00 2001
From: judeconnolly <judeconnolly@seattleacademy.org>
Date: Thu, 15 Feb 2024 17:31:15 -0800
Subject: [PATCH 64/99] arm button remapping

---
 src/main/java/frc/robot/RobotContainer.java   | 21 ++++++++++++++++++-
 ...{ArmControl.java => ArmRemoteControl.java} |  4 ++--
 2 files changed, 22 insertions(+), 3 deletions(-)
 rename src/main/java/frc/robot/commands/{ArmControl.java => ArmRemoteControl.java} (93%)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 2bd9aa5..b214531 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -6,7 +6,7 @@
 import frc.robot.Constants.ArmConstants;
 import frc.robot.subsystems.Arm;
 import frc.robot.commands.Autos;
-import frc.robot.commands.ArmControl;
+import frc.robot.commands.ArmRemoteControl;
 import frc.robot.commands.ChassisRemoteControl;
 import frc.robot.Constants.VisionConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
@@ -112,6 +112,7 @@ public void setUpDriveController() {
 
 		drivetrain.removeDefaultCommand();
 
+		ArmRemoteControl armInputs;
 		ChassisDriveInputs inputs;
 		OptionButtonInput preciseModeButton, boostModeButton, fieldRelativeButton;
 
@@ -124,6 +125,15 @@ public void setUpDriveController() {
 					joystick::getTwist, -1,
 					Constants.DriverConstants.DEAD_ZONE);
 
+			armInputs = new ArmRemoteControl(arm,
+					new OptionButtonInput(joystick,11, ActivationMode.HOLD), 
+					new OptionButtonInput(joystick,12, ActivationMode.HOLD), 
+
+					new OptionButtonInput(joystick::povLeft, ActivationMode.HOLD), 
+					new OptionButtonInput(joystick::povRight, ActivationMode.HOLD), 
+					new OptionButtonInput(joystick::povDown, ActivationMode.HOLD)
+					);
+
 			preciseModeButton = new OptionButtonInput(joystick, 2, ActivationMode.TOGGLE);
 			boostModeButton = new OptionButtonInput(joystick, 1, ActivationMode.HOLD);
 			fieldRelativeButton = new OptionButtonInput(joystick, 3, ActivationMode.TOGGLE);
@@ -140,6 +150,15 @@ public void setUpDriveController() {
 					xbox::getRightX, -1,
 					Constants.DriverConstants.DEAD_ZONE);
 
+			armInputs = new ArmRemoteControl(arm,
+					new OptionButtonInput(xbox::rightBumper, ActivationMode.HOLD), 
+					new OptionButtonInput(xbox::leftBumper, ActivationMode.HOLD), 
+
+					new OptionButtonInput(xbox::povLeft, ActivationMode.HOLD), 
+					new OptionButtonInput(xbox::povRight, ActivationMode.HOLD), 
+					new OptionButtonInput(xbox::povDown, ActivationMode.HOLD)
+					);
+
 			preciseModeButton = new OptionButtonInput(xbox::b, ActivationMode.TOGGLE);
 			boostModeButton = new OptionButtonInput(xbox::leftStick, ActivationMode.HOLD);
 			fieldRelativeButton = new OptionButtonInput(xbox::povUp, ActivationMode.TOGGLE);
diff --git a/src/main/java/frc/robot/commands/ArmControl.java b/src/main/java/frc/robot/commands/ArmRemoteControl.java
similarity index 93%
rename from src/main/java/frc/robot/commands/ArmControl.java
rename to src/main/java/frc/robot/commands/ArmRemoteControl.java
index 0c50d9c..b37bc38 100644
--- a/src/main/java/frc/robot/commands/ArmControl.java
+++ b/src/main/java/frc/robot/commands/ArmRemoteControl.java
@@ -10,7 +10,7 @@
 import edu.wpi.first.wpilibj2.command.Subsystem;
 
 
-public class ArmControl extends Command {
+public class ArmRemoteControl extends Command {
 	private final Arm arm;
 
 	private final OptionButtonInput raiseArmButton;
@@ -21,7 +21,7 @@ public class ArmControl extends Command {
     private final OptionButtonInput intakePositionButton;
 
 
-	public ArmControl(Arm arm, OptionButtonInput raiseArmButton, OptionButtonInput lowerArmButton,
+	public ArmRemoteControl(Arm arm, OptionButtonInput raiseArmButton, OptionButtonInput lowerArmButton,
             OptionButtonInput speakerPositionButton, OptionButtonInput ampPositionButton, OptionButtonInput intakePositionButton) {
         this.arm = arm;
 

From 131a05939e7ae520b4c8bea940f34d71b72613a7 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Thu, 15 Feb 2024 17:42:22 -0800
Subject: [PATCH 65/99] Add auto rotate relative to start

---
 src/main/java/frc/robot/commands/AutoRotateTo.java | 10 +++++++---
 src/main/java/frc/robot/commands/Autos.java        |  2 +-
 src/main/java/frc/robot/commands/DriveToPose.java  |  5 ++---
 3 files changed, 10 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/commands/AutoRotateTo.java b/src/main/java/frc/robot/commands/AutoRotateTo.java
index beadce4..c08575a 100644
--- a/src/main/java/frc/robot/commands/AutoRotateTo.java
+++ b/src/main/java/frc/robot/commands/AutoRotateTo.java
@@ -19,22 +19,26 @@ public class AutoRotateTo extends Command {
 
 	/***
 	 * Command to autonomously rotate some direction
-	 * @param subsystem The robot drivetrain
+	 * @param drivetrain The robot drivetrain
 	 * @param direction Rotation2d class to execute
 	 */
-	public AutoRotateTo(SwerveDrivetrain subsystem, Rotation2d direction) {
+	public AutoRotateTo(SwerveDrivetrain drivetrain, Rotation2d direction) {
 
 		rotatePID = new PIDController(
 				RobotMovementConstants.ROTATION_PID_P,
 				RobotMovementConstants.ROTATION_PID_I,
 				RobotMovementConstants.ROTATION_PID_D);
 
-		this.drivetrain = subsystem;
+		this.drivetrain = drivetrain;
 		this.angleGoal = direction.getRadians();
 
 		addRequirements(this.drivetrain);
 	}
 
+	public static AutoRotateTo autoRotateToRelative(SwerveDrivetrain drivetrain, Rotation2d direction) {
+		return new AutoRotateTo(drivetrain, Rotation2d.fromRadians(drivetrain.getHeading().getRadians() + direction.getRadians()));
+	}
+
 	@Override
 	public void initialize() {
 	}
diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java
index 43f0438..a01407b 100644
--- a/src/main/java/frc/robot/commands/Autos.java
+++ b/src/main/java/frc/robot/commands/Autos.java
@@ -17,7 +17,7 @@ public final class Autos {
 	/** Example static factory for an autonomous command. */
 	public static Command testingAuto(SwerveDrivetrain drivetrain) {
 		return Commands.sequence(
-				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(0))
+				AutoRotateTo.autoRotateToRelative(drivetrain, Rotation2d.fromDegrees(90))
 		);
 	}
 
diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java
index ecfa4bb..be288c0 100644
--- a/src/main/java/frc/robot/commands/DriveToPose.java
+++ b/src/main/java/frc/robot/commands/DriveToPose.java
@@ -33,7 +33,8 @@ public class DriveToPose extends Command {
 	public DriveToPose(SwerveDrivetrain drivetrain, Pose2d targetPosition) {
 		this.drivetrain = drivetrain;
 		this.targetPosition = targetPosition;
-		SmartDashboard.putBoolean("Made shit", true);
+		
+		addRequirements(drivetrain);
 	}
 
 	public DriveToPose(SwerveDrivetrain drivetrain, Translation2d translation, Rotation2d rotation) {
@@ -42,7 +43,6 @@ public DriveToPose(SwerveDrivetrain drivetrain, Translation2d translation, Rotat
 
 	@Override
 	public void initialize() {
-		SmartDashboard.putBoolean("Doing shit", true);
 		drivetrain.setDesiredPosition(targetPosition);
 	}
 
@@ -59,7 +59,6 @@ public boolean isFinished() {
 
 	@Override
 	public void end(boolean interrupted) {
-		SmartDashboard.putBoolean("Doing shit", false);
 		drivetrain.clearDesiredPosition();
 		drivetrain.stop();
 	}

From b3343f4e504610f7f3f3ae7b3a87b28a96a10f77 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Thu, 15 Feb 2024 20:06:35 -0800
Subject: [PATCH 66/99] reenable vision subsystem

---
 src/main/java/frc/robot/RobotContainer.java   |  6 ++--
 .../java/frc/robot/commands/DriveToPose.java  | 33 +++++++++++--------
 .../frc/robot/commands/DriveTransform.java    |  7 ++--
 3 files changed, 28 insertions(+), 18 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 510525a..454c0df 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -3,8 +3,10 @@
 import frc.robot.Constants.DriverConstants;
 import frc.robot.Constants.SwerveDrivetrainConstants;
 import frc.robot.Constants.SwerveModuleConstants;
+import frc.robot.Constants.VisionConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.SwerveModule;
+import frc.robot.subsystems.Vision;
 import frc.robot.commands.Autos;
 import frc.robot.commands.ChassisRemoteControl;
 import frc.robot.inputs.ChassisDriveInputs;
@@ -72,14 +74,14 @@ public class RobotContainer {
 
 	private final SendableChooser<Command> autoChooser = new SendableChooser<Command>();
 
-	// private final Vision vision = new Vision(VisionConstants.CAMERA_NAME, Constants.VisionConstants.CAMERA_POSE);
+	private final Vision vision = new Vision(VisionConstants.CAMERA_NAME, VisionConstants.CAMERA_POSE);
 
 	/**
 	 * The container for the robot. Contains subsystems, OI devices, and commands.
 	 */
 	public RobotContainer() {
 		autoChooser.setDefaultOption("Testing Auto", Autos.testingAuto(drivetrain));
-		// autoChooser.addOption("Follow Tag", Autos.tagFollowAuto(drivetrain, vision, 1));
+		autoChooser.addOption("Follow Tag", Autos.tagFollowAuto(drivetrain, vision, 1));
 		SmartDashboard.putData("Auto Chooser", autoChooser);
 
 		configureBindings();
diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java
index be288c0..0782cbe 100644
--- a/src/main/java/frc/robot/commands/DriveToPose.java
+++ b/src/main/java/frc/robot/commands/DriveToPose.java
@@ -3,7 +3,6 @@
 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.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.SwerveDrivetrain;
 
@@ -17,26 +16,35 @@ public class DriveToPose extends Command {
 	 * 
 	 * <p>
 	 * This drives relative to the robot starting position,
-	 * so a pose of +2x and +1y will drive to the position 2 meters forward and 1
-	 * meter left of whether the robot started,
+	 * so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
 	 * where forward is whatever direction the robot started in
-	 * </p>
 	 * 
 	 * <p>
-	 * The last place the drivetrain position was reset counts as the starting
-	 * position
-	 * </p>
+	 * The last place the drivetrain position was reset counts as the starting position
 	 * 
-	 * @param drivetrain     the drivetrain of the robot
-	 * @param targetPosition the pose the robot is trying to achieve
+	 * @param drivetrain	the drivetrain of the robot
+	 * @param position		Target pose the robot is trying to drive to
 	 */
-	public DriveToPose(SwerveDrivetrain drivetrain, Pose2d targetPosition) {
+	public DriveToPose(SwerveDrivetrain drivetrain, Pose2d position) {
 		this.drivetrain = drivetrain;
-		this.targetPosition = targetPosition;
+		this.targetPosition = position;
 		
 		addRequirements(drivetrain);
 	}
 
+	/**
+	 * <p>
+	 * This drives relative to the robot starting position,
+	 * so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
+	 * where forward is whatever direction the robot started in
+	 * 
+	 * <p>
+	 * The last place the drivetrain position was reset counts as the starting position
+	 * 
+	 * @param drivetrain	The drivetrain of the robot
+	 * @param translation	Target transform to drive
+	 * @param rotation	 	Target rotation to drive
+	 */
 	public DriveToPose(SwerveDrivetrain drivetrain, Translation2d translation, Rotation2d rotation) {
 		this(drivetrain, new Pose2d(translation, rotation));
 	}
@@ -53,8 +61,7 @@ public void execute() {
 
 	@Override
 	public boolean isFinished() {
-		// return drivetrain.isAtDesiredPosition();
-		return false;
+		return drivetrain.isAtDesiredPosition();
 	}
 
 	@Override
diff --git a/src/main/java/frc/robot/commands/DriveTransform.java b/src/main/java/frc/robot/commands/DriveTransform.java
index 19a83b9..866aa51 100644
--- a/src/main/java/frc/robot/commands/DriveTransform.java
+++ b/src/main/java/frc/robot/commands/DriveTransform.java
@@ -12,8 +12,7 @@ public class DriveTransform extends Command {
 	private final Transform2d transform;
 
 	/**
-	 * Create a new DriveTransform command. Tries to drive a certain transform using
-	 * the DriveToPose command.
+	 * Create a new DriveTransform command. Tries to drive a certain transform using the DriveToPose command.
 	 * 
 	 * <p>This drives relative to the robot starting position,
 	 * so a transform of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
@@ -26,6 +25,8 @@ public class DriveTransform extends Command {
 	public DriveTransform(SwerveDrivetrain drivetrain, Transform2d transform) {
 		this.drivetrain = drivetrain;
 		this.transform = transform;
+
+		addRequirements(drivetrain);
 	}
 
 	/**
@@ -38,7 +39,7 @@ public DriveTransform(SwerveDrivetrain drivetrain, Transform2d transform) {
 	 * 
 	 * @param drivetrain  The drivetrain of the robot
 	 * @param translation Target transform to drive
-	 * @param translation Target rotation to drive
+	 * @param rotation Target rotation to drive
 	 */
 	public DriveTransform(SwerveDrivetrain drivetrain, Translation2d translation, Rotation2d rotation) {
 		this(drivetrain, new Transform2d(translation, rotation));

From f594505b80acf8e5c906dac660c8f8e56c29fb0a Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Fri, 16 Feb 2024 08:29:42 -0800
Subject: [PATCH 67/99] cleanup minor code logic

---
 src/main/java/frc/robot/commands/AutoRotateTo.java   |  2 +-
 src/main/java/frc/robot/subsystems/SwerveModule.java | 10 +++++-----
 2 files changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/commands/AutoRotateTo.java b/src/main/java/frc/robot/commands/AutoRotateTo.java
index c08575a..8c608a0 100644
--- a/src/main/java/frc/robot/commands/AutoRotateTo.java
+++ b/src/main/java/frc/robot/commands/AutoRotateTo.java
@@ -36,7 +36,7 @@ public AutoRotateTo(SwerveDrivetrain drivetrain, Rotation2d direction) {
 	}
 
 	public static AutoRotateTo autoRotateToRelative(SwerveDrivetrain drivetrain, Rotation2d direction) {
-		return new AutoRotateTo(drivetrain, Rotation2d.fromRadians(drivetrain.getHeading().getRadians() + direction.getRadians()));
+		return new AutoRotateTo(drivetrain, drivetrain.getHeading().plus(direction));
 	}
 
 	@Override
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 14edddb..5645046 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -48,7 +48,7 @@ public class SwerveModule extends SubsystemBase {
 	private final Translation2d distanceFromCenter;
 
 	/** Whether swerve module is stopped */
-	private boolean stopped = false;
+	private boolean steeringStopped = false;
 
 	/**
 	 * Constructor for an individual Swerve Module.
@@ -123,7 +123,7 @@ public SwerveModule(int driveMotorDeviceId, int steeringMotorDeviceId, int steer
 	 */
 	@Override
 	public void periodic() {
-		if (!stopped) {
+		if (!steeringStopped) {
 			// Calculate how fast to spin the motor to get to the desired angle using our PID controller,
 			// then set the motor to spin at that speed
 			steeringMotor.set(steeringPIDController.calculate(getSteeringAngleRotations()));
@@ -136,7 +136,7 @@ public void periodic() {
 	 * Stop drive and steering motor of swerve module, module can be moved again by calling setDesiredState.
 	 */
 	public void stop() {
-		stopped = true;
+		steeringStopped = true;
 
 		// Manually stop both motors in swerve module
 		driveMotor.stopMotor();
@@ -190,13 +190,13 @@ public void setDesiredState(SwerveModuleState state, boolean powerDriveMode) {
 			return;
 		}
 		
-		stopped = false;
 		
 		// Optimize the reference state to avoid spinning further than 90 degrees
 		state = optimize(state, Rotation2d.fromRotations(getSteeringAngleRotations()));
-
+		
 		// --- Set steering motor ---
 		steeringPIDController.setSetpoint(state.angle.getRotations());
+		steeringStopped = false;
 
 		// --- Set drive motor ---
 

From 3b7bbc25fbf88daf618b14cfc8fadf7203590e64 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Sat, 17 Feb 2024 10:09:51 -0800
Subject: [PATCH 68/99] make auto rotate work robot relative

---
 .../java/frc/robot/commands/AutoRotateTo.java    | 16 +++++++++++-----
 src/main/java/frc/robot/commands/Autos.java      |  2 +-
 2 files changed, 12 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/commands/AutoRotateTo.java b/src/main/java/frc/robot/commands/AutoRotateTo.java
index 8c608a0..5c9fd8e 100644
--- a/src/main/java/frc/robot/commands/AutoRotateTo.java
+++ b/src/main/java/frc/robot/commands/AutoRotateTo.java
@@ -14,7 +14,9 @@ public class AutoRotateTo extends Command {
 
 	private final PIDController rotatePID;
 	private final double angleGoal;
+	private final boolean relative;
 
+	private double currentAngleGoal;
 	private double atSetpointCounter = 0;
 
 	/***
@@ -22,7 +24,7 @@ public class AutoRotateTo extends Command {
 	 * @param drivetrain The robot drivetrain
 	 * @param direction Rotation2d class to execute
 	 */
-	public AutoRotateTo(SwerveDrivetrain drivetrain, Rotation2d direction) {
+	public AutoRotateTo(SwerveDrivetrain drivetrain, Rotation2d direction, boolean relative) {
 
 		rotatePID = new PIDController(
 				RobotMovementConstants.ROTATION_PID_P,
@@ -30,28 +32,32 @@ public AutoRotateTo(SwerveDrivetrain drivetrain, Rotation2d direction) {
 				RobotMovementConstants.ROTATION_PID_D);
 
 		this.drivetrain = drivetrain;
+		this.relative = relative;
 		this.angleGoal = direction.getRadians();
 
 		addRequirements(this.drivetrain);
 	}
 
-	public static AutoRotateTo autoRotateToRelative(SwerveDrivetrain drivetrain, Rotation2d direction) {
-		return new AutoRotateTo(drivetrain, drivetrain.getHeading().plus(direction));
+	public AutoRotateTo(SwerveDrivetrain drivetrain, Rotation2d direction) {
+		this(drivetrain, direction, true);
 	}
 
 	@Override
 	public void initialize() {
+		currentAngleGoal = 
+			relative ? drivetrain.getHeading().getRadians() : 0
+			+ angleGoal;
 	}
 
 	@Override
 	public void execute() {
 		final double currentAngle = drivetrain.getHeading().getRadians();
 
-		double turnsSeed = rotatePID.calculate(currentAngle, this.angleGoal);
+		double turnsSeed = rotatePID.calculate(currentAngle, this.currentAngleGoal);
 
 		drivetrain.setDesiredState(new ChassisSpeeds(0, 0, turnsSeed));
 
-		if (Math.abs(currentAngle - this.angleGoal) < RobotMovementConstants.ANGLE_TOLERANCE_RADIANS) atSetpointCounter += TimedRobot.kDefaultPeriod;
+		if (Math.abs(currentAngle - this.currentAngleGoal) < RobotMovementConstants.ANGLE_TOLERANCE_RADIANS) atSetpointCounter += TimedRobot.kDefaultPeriod;
 		else atSetpointCounter = 0;
 		
 		drivetrain.updateSmartDashboard();
diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java
index a01407b..a35965a 100644
--- a/src/main/java/frc/robot/commands/Autos.java
+++ b/src/main/java/frc/robot/commands/Autos.java
@@ -17,7 +17,7 @@ public final class Autos {
 	/** Example static factory for an autonomous command. */
 	public static Command testingAuto(SwerveDrivetrain drivetrain) {
 		return Commands.sequence(
-				AutoRotateTo.autoRotateToRelative(drivetrain, Rotation2d.fromDegrees(90))
+				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(90))
 		);
 	}
 

From f9f16d2cf7ec48c3e27b21b55d732bc19a8ebac4 Mon Sep 17 00:00:00 2001
From: Aleah H <aleahjham@gmail.com>
Date: Sat, 17 Feb 2024 15:01:36 -0800
Subject: [PATCH 69/99] Test rotateTo Command

---
 shuffleboard.json                             |  2 +-
 src/main/java/frc/robot/Constants.java        |  8 +++---
 src/main/java/frc/robot/RobotContainer.java   | 17 +++++++----
 .../java/frc/robot/commands/AutoRotateTo.java | 24 ++++++++++------
 src/main/java/frc/robot/commands/Autos.java   | 28 +++++++++++++++++--
 5 files changed, 58 insertions(+), 21 deletions(-)

diff --git a/shuffleboard.json b/shuffleboard.json
index f61c1aa..1c010c3 100644
--- a/shuffleboard.json
+++ b/shuffleboard.json
@@ -46,7 +46,7 @@
             ],
             "content": {
               "_type": "Gyro",
-              "_source0": "network_table:///SmartDashboard/Heading Degrees",
+              "_source0": "network_table:///SmartDashboard/ Degrees",
               "_title": "Heading Degrees",
               "_glyph": 139,
               "_showGlyph": true,
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 329f92d..de37f72 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -67,14 +67,14 @@ public static class DriverConstants {
 
 	public static class RobotMovementConstants {
 		public static final double AT_SETPOINT_TOLERANCE_TIME_SECONDS = 0;
-		public static final double ROTATE_AT_SETPOINT_TIME_SECONDS = 1;
+		public static final double ROTATE_AT_SETPOINT_TIME_SECONDS = 0;
 
 		public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(0.0001);
 		public static final double ANGLE_TOLERANCE_RADIANS = Units.degreesToRadians(1);
 
-		public static final double ROTATION_PID_P = 4;
-		public static final double ROTATION_PID_I = 0.1;
-		public static final double ROTATION_PID_D = 4;
+		public static final double ROTATION_PID_P = 5;
+		public static final double ROTATION_PID_I = 0;
+		public static final double ROTATION_PID_D = 0;
 
 		public static final double TRANSLATION_PID_P = 30;
 		public static final double TRANSLATION_PID_I = 0.5;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 454c0df..f6ed3f0 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -67,7 +67,7 @@ public class RobotContainer {
 			new Translation2d(-SwerveDrivetrainConstants.MODULE_LOCATION_X,
 					-SwerveDrivetrainConstants.MODULE_LOCATION_Y));
 
-	private final AHRS gyro = new AHRS(I2C.Port.kOnboard);
+	private final AHRS gyro = new AHRS();
 
 	private final SwerveDrivetrain drivetrain = new SwerveDrivetrain(gyro, swerveModuleFL, swerveModuleFR,
 			swerveModuleBL, swerveModuleBR);
@@ -82,6 +82,11 @@ public class RobotContainer {
 	public RobotContainer() {
 		autoChooser.setDefaultOption("Testing Auto", Autos.testingAuto(drivetrain));
 		autoChooser.addOption("Follow Tag", Autos.tagFollowAuto(drivetrain, vision, 1));
+		autoChooser.addOption("Rotate by 90", Autos.rotateBy90Auto(drivetrain));
+		autoChooser.addOption("Rotate to 90", Autos.rotateTo90Auto(drivetrain));
+		autoChooser.addOption("Rotate by -90", Autos.rotateByNegative90Auto(drivetrain));
+		autoChooser.addOption("Rotate to -90", Autos.rotateToNegative90Auto(drivetrain));
+		autoChooser.addOption("Rotate by 10", Autos.rotateBy10Auto(drivetrain));
 		SmartDashboard.putData("Auto Chooser", autoChooser);
 
 		configureBindings();
@@ -106,7 +111,7 @@ public void setUpDriveController() {
 
 		if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
 			final CommandJoystick joystick = new CommandJoystick(genericHID.getPort());
-			
+
 			inputs = new ChassisDriveInputs(
 					joystick::getX, -1,
 					joystick::getY, -1,
@@ -118,9 +123,8 @@ public void setUpDriveController() {
 			fieldRelativeButton = new OptionButtonInput(joystick, 3, ActivationMode.TOGGLE);
 
 			joystick.button(10).onTrue(Commands.sequence(
-				Commands.runOnce(drivetrain::toDefaultStates, drivetrain),
-				Commands.waitSeconds(0.5)
-			));
+					Commands.runOnce(drivetrain::toDefaultStates, drivetrain),
+					Commands.waitSeconds(0.5)));
 
 		} else {
 			final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());
@@ -136,7 +140,8 @@ public void setUpDriveController() {
 			fieldRelativeButton = new OptionButtonInput(xbox::povUp, ActivationMode.TOGGLE);
 		}
 
-		drivetrain.setDefaultCommand(new ChassisRemoteControl(drivetrain, inputs, preciseModeButton, boostModeButton, fieldRelativeButton));
+		drivetrain.setDefaultCommand(
+				new ChassisRemoteControl(drivetrain, inputs, preciseModeButton, boostModeButton, fieldRelativeButton));
 	}
 
 	/** Use this method to define your trigger->command mappings. */
diff --git a/src/main/java/frc/robot/commands/AutoRotateTo.java b/src/main/java/frc/robot/commands/AutoRotateTo.java
index 5c9fd8e..ba7bd2d 100644
--- a/src/main/java/frc/robot/commands/AutoRotateTo.java
+++ b/src/main/java/frc/robot/commands/AutoRotateTo.java
@@ -7,6 +7,7 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.RobotMovementConstants;
 
 public class AutoRotateTo extends Command {
@@ -21,8 +22,9 @@ public class AutoRotateTo extends Command {
 
 	/***
 	 * Command to autonomously rotate some direction
+	 * 
 	 * @param drivetrain The robot drivetrain
-	 * @param direction Rotation2d class to execute
+	 * @param direction  Rotation2d class to execute
 	 */
 	public AutoRotateTo(SwerveDrivetrain drivetrain, Rotation2d direction, boolean relative) {
 
@@ -30,6 +32,8 @@ public AutoRotateTo(SwerveDrivetrain drivetrain, Rotation2d direction, boolean r
 				RobotMovementConstants.ROTATION_PID_P,
 				RobotMovementConstants.ROTATION_PID_I,
 				RobotMovementConstants.ROTATION_PID_D);
+		rotatePID.enableContinuousInput(-Math.PI, Math.PI);
+		rotatePID.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
 
 		this.drivetrain = drivetrain;
 		this.relative = relative;
@@ -44,9 +48,10 @@ public AutoRotateTo(SwerveDrivetrain drivetrain, Rotation2d direction) {
 
 	@Override
 	public void initialize() {
-		currentAngleGoal = 
-			relative ? drivetrain.getHeading().getRadians() : 0
-			+ angleGoal;
+		currentAngleGoal = relative ? drivetrain.getHeading().getRadians()
+				: 0;
+		currentAngleGoal += angleGoal;
+		SmartDashboard.putNumber("Target Angle Auto", currentAngleGoal);
 	}
 
 	@Override
@@ -54,18 +59,21 @@ public void execute() {
 		final double currentAngle = drivetrain.getHeading().getRadians();
 
 		double turnsSeed = rotatePID.calculate(currentAngle, this.currentAngleGoal);
+		SmartDashboard.putNumber("Turn Speed Auto", turnsSeed);
 
 		drivetrain.setDesiredState(new ChassisSpeeds(0, 0, turnsSeed));
 
-		if (Math.abs(currentAngle - this.currentAngleGoal) < RobotMovementConstants.ANGLE_TOLERANCE_RADIANS) atSetpointCounter += TimedRobot.kDefaultPeriod;
-		else atSetpointCounter = 0;
-		
+		if (Math.abs(currentAngle - this.currentAngleGoal) < RobotMovementConstants.ANGLE_TOLERANCE_RADIANS)
+			atSetpointCounter += TimedRobot.kDefaultPeriod;
+		else
+			atSetpointCounter = 0;
+
 		drivetrain.updateSmartDashboard();
 	}
 
 	@Override
 	public boolean isFinished() {
-		return atSetpointCounter > RobotMovementConstants.ROTATE_AT_SETPOINT_TIME_SECONDS;
+		return rotatePID.atSetpoint();
 	}
 
 	@Override
diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java
index a35965a..2dd872b 100644
--- a/src/main/java/frc/robot/commands/Autos.java
+++ b/src/main/java/frc/robot/commands/Autos.java
@@ -6,6 +6,7 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import frc.robot.Constants.SwerveDrivetrainConstants;
 import frc.robot.subsystems.Arm;
 
@@ -17,8 +18,31 @@ public final class Autos {
 	/** Example static factory for an autonomous command. */
 	public static Command testingAuto(SwerveDrivetrain drivetrain) {
 		return Commands.sequence(
-				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(90))
-		);
+				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(90), false));
+	}
+
+	public static Command rotateTo90Auto(SwerveDrivetrain drivetrain) {
+		return Commands.sequence(
+				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(90), true));
+	}
+
+	public static Command rotateBy90Auto(SwerveDrivetrain drivetrain) {
+		return Commands.sequence(
+				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(90), false));
+	}
+
+	public static Command rotateToNegative90Auto(SwerveDrivetrain drivetrain) {
+		return Commands.sequence(
+				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(-90), true));
+	}
+
+	public static Command rotateByNegative90Auto(SwerveDrivetrain drivetrain) {
+		return Commands.sequence(
+				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(-90), false));
+	}
+
+	public static Command rotateBy10Auto(SwerveDrivetrain drivetrain) {
+		return Commands.sequence(new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(10), true));
 	}
 
 	/** Auto-mode that attempts to follow an april tag. */

From 14119912f32c816e39ad131c3b5fb1656a2c72c9 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Sat, 17 Feb 2024 17:28:22 -0800
Subject: [PATCH 70/99] small cleanup after testing (unused constants/imports,
 etc)

---
 src/main/java/frc/robot/Constants.java             |  3 ---
 src/main/java/frc/robot/RobotContainer.java        |  1 -
 src/main/java/frc/robot/commands/AutoDriveTo.java  | 13 +++----------
 src/main/java/frc/robot/commands/AutoRotateTo.java |  7 -------
 src/main/java/frc/robot/commands/Autos.java        |  1 -
 5 files changed, 3 insertions(+), 22 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index de37f72..f43ed3b 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -66,9 +66,6 @@ public static class DriverConstants {
 	}
 
 	public static class RobotMovementConstants {
-		public static final double AT_SETPOINT_TOLERANCE_TIME_SECONDS = 0;
-		public static final double ROTATE_AT_SETPOINT_TIME_SECONDS = 0;
-
 		public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(0.0001);
 		public static final double ANGLE_TOLERANCE_RADIANS = Units.degreesToRadians(1);
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index f6ed3f0..cdf8fbe 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -16,7 +16,6 @@
 import com.kauailabs.navx.frc.AHRS;
 
 import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.I2C;
 import edu.wpi.first.wpilibj.GenericHID.HIDType;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
diff --git a/src/main/java/frc/robot/commands/AutoDriveTo.java b/src/main/java/frc/robot/commands/AutoDriveTo.java
index 90f92dc..fd07c4c 100644
--- a/src/main/java/frc/robot/commands/AutoDriveTo.java
+++ b/src/main/java/frc/robot/commands/AutoDriveTo.java
@@ -7,7 +7,6 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.RobotMovementConstants;
 
@@ -17,8 +16,6 @@ public class AutoDriveTo extends Command {
 	private final PIDController xMovePID, yMovePID;
 	private double initX, initY, goalX, goalY;
 
-	private double atSetpointCounter = 0;
-
 	private boolean xOnlyMode;
 
 	/***
@@ -38,11 +35,13 @@ public AutoDriveTo(SwerveDrivetrain subsystem, Translation2d translation) {
 				RobotMovementConstants.TRANSLATION_PID_P,
 				RobotMovementConstants.TRANSLATION_PID_I,
 				RobotMovementConstants.TRANSLATION_PID_D);
+		xMovePID.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
 
 		yMovePID = new PIDController(
 				RobotMovementConstants.TRANSLATION_PID_P,
 				RobotMovementConstants.TRANSLATION_PID_I,
 				RobotMovementConstants.TRANSLATION_PID_D);
+		yMovePID.setTolerance(RobotMovementConstants.POSITION_TOLERANCE_METERS);
 
 		addRequirements(this.drivetrain);
 	}
@@ -69,12 +68,6 @@ public void execute() {
 		SmartDashboard.putNumber("PoseX", position.getX());
 		SmartDashboard.putNumber("PoseDegrees", position.getRotation().getDegrees());
 
-		if (Math.abs(x - goalX) < RobotMovementConstants.POSITION_TOLERANCE_METERS
-				&& Math.abs(y - goalY) < RobotMovementConstants.POSITION_TOLERANCE_METERS)
-			atSetpointCounter += TimedRobot.kDefaultPeriod;
-		else
-			atSetpointCounter = 0;
-
 		double xSpeed = xMovePID.calculate(x, goalX);
 		double ySpeed = yMovePID.calculate(y, goalY);
 
@@ -99,7 +92,7 @@ public void execute() {
 
 	@Override
 	public boolean isFinished() {
-		return atSetpointCounter > RobotMovementConstants.AT_SETPOINT_TOLERANCE_TIME_SECONDS;
+		return xMovePID.atSetpoint() && yMovePID.atSetpoint();
 	}
 
 	@Override
diff --git a/src/main/java/frc/robot/commands/AutoRotateTo.java b/src/main/java/frc/robot/commands/AutoRotateTo.java
index ba7bd2d..eeef2ba 100644
--- a/src/main/java/frc/robot/commands/AutoRotateTo.java
+++ b/src/main/java/frc/robot/commands/AutoRotateTo.java
@@ -6,7 +6,6 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.RobotMovementConstants;
 
@@ -18,7 +17,6 @@ public class AutoRotateTo extends Command {
 	private final boolean relative;
 
 	private double currentAngleGoal;
-	private double atSetpointCounter = 0;
 
 	/***
 	 * Command to autonomously rotate some direction
@@ -63,11 +61,6 @@ public void execute() {
 
 		drivetrain.setDesiredState(new ChassisSpeeds(0, 0, turnsSeed));
 
-		if (Math.abs(currentAngle - this.currentAngleGoal) < RobotMovementConstants.ANGLE_TOLERANCE_RADIANS)
-			atSetpointCounter += TimedRobot.kDefaultPeriod;
-		else
-			atSetpointCounter = 0;
-
 		drivetrain.updateSmartDashboard();
 	}
 
diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java
index 2dd872b..2aee1e2 100644
--- a/src/main/java/frc/robot/commands/Autos.java
+++ b/src/main/java/frc/robot/commands/Autos.java
@@ -6,7 +6,6 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import frc.robot.Constants.SwerveDrivetrainConstants;
 import frc.robot.subsystems.Arm;
 

From 2c97fadd0ccf23bbac23ab4a40db917e0c08ad47 Mon Sep 17 00:00:00 2001
From: Aleah H <aleahjham@gmail.com>
Date: Mon, 19 Feb 2024 13:10:19 -0800
Subject: [PATCH 71/99] Add New Swerve Offsets (Comp Bot)

---
 src/main/java/frc/robot/Constants.java | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index f43ed3b..63443de 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -118,25 +118,25 @@ public static class SwerveModuleConstants {
 					VELOCITY_MOTOR_ID_FL = 2;
 					ANGULAR_MOTOR_ID_FL = 3;
 					ANGULAR_MOTOR_ENCODER_ID_FL = 3;
-					ANGULAR_MOTOR_ENCODER_OFFSET_FL = -0.256015325670498;
+					ANGULAR_MOTOR_ENCODER_OFFSET_FL = -0.364013671875;
 
 					// Front right
 					VELOCITY_MOTOR_ID_FR = 16;
 					ANGULAR_MOTOR_ID_FR = 17;
-					ANGULAR_MOTOR_ENCODER_ID_FR = 2;
-					ANGULAR_MOTOR_ENCODER_OFFSET_FR = -0.248045977011494;
+					ANGULAR_MOTOR_ENCODER_ID_FR = 4;
+					ANGULAR_MOTOR_ENCODER_OFFSET_FR = -0.42114257812;
 
 					// Back left
 					VELOCITY_MOTOR_ID_BL = 8;
 					ANGULAR_MOTOR_ID_BL = 9;
-					ANGULAR_MOTOR_ENCODER_ID_BL = 4;
-					ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.894674329501916;
+					ANGULAR_MOTOR_ENCODER_ID_BL = 2;
+					ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.634033203125;
 
 					// Back right
 					VELOCITY_MOTOR_ID_BR = 10;
 					ANGULAR_MOTOR_ID_BR = 11;
 					ANGULAR_MOTOR_ENCODER_ID_BR = 1;
-					ANGULAR_MOTOR_ENCODER_OFFSET_BR = -0.530498084291188;
+					ANGULAR_MOTOR_ENCODER_OFFSET_BR = -0.54736328125;
 					break;
 
 				// case COMPETITION_BOT:

From df938625d1b071e7abc9707ba7853a43d68ecf69 Mon Sep 17 00:00:00 2001
From: Aleah H <aleahjham@gmail.com>
Date: Mon, 19 Feb 2024 14:56:58 -0800
Subject: [PATCH 72/99] Add somewhat working arm code

---
 src/main/java/frc/robot/Constants.java        | 28 ++++++++--------
 src/main/java/frc/robot/RobotContainer.java   | 22 +++++++++----
 .../frc/robot/commands/ArmRemoteControl.java  |  2 +-
 src/main/java/frc/robot/subsystems/Arm.java   | 32 ++++++++++++-------
 4 files changed, 53 insertions(+), 31 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 99267d1..845c184 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -70,18 +70,20 @@ public static class ArmConstants {
         public static final double MAXIMUM_ARM_DEGREES = 1;
         public static final double MINIMUM_ARM_DEGREES = 0;
 
-        public static final double ARM_AMP_SHOOTING_DEGREES = 0;
-        public static final double ARM_SPEAKER_SHOOTING_DEGREES = 0;
-        public static final double ARM_INTAKE_DEGREES = 0;
+        public static final double ARM_AMP_SHOOTING_DEGREES = -20;
+        public static final double ARM_SPEAKER_SHOOTING_DEGREES = 45;
+        public static final double ARM_INTAKE_DEGREES = -40;
 
-        public static final int LEFT_MOTOR_ID = 0;
+        public static final int LEFT_MOTOR_ID = 5;
         //public static final int LEFT_ENCODER_ID = 0;
-        public static final int RIGHT_MOTOR_ID = 0;
-        public static final int RIGHT_ENCODER_ID = 0;
+        public static final int RIGHT_MOTOR_ID = 19;
+        public static final int RIGHT_ENCODER_ID = 6;
+
+		public static final boolean ARE_MOTORS_REVERSED = false;
 
         public static final double DEGREES_PER_SECOND = 2.0;
 
-		public static final double ELEVATION_PID_P = 0;
+		public static final double ELEVATION_PID_P = 1;
 		public static final double ELEVATION_PID_I = 0;
 		public static final double ELEVATION_PID_D = 0;
 
@@ -143,25 +145,25 @@ public static class SwerveModuleConstants {
 					VELOCITY_MOTOR_ID_FL = 2;
 					ANGULAR_MOTOR_ID_FL = 3;
 					ANGULAR_MOTOR_ENCODER_ID_FL = 3;
-					ANGULAR_MOTOR_ENCODER_OFFSET_FL = -0.256015325670498;
+					ANGULAR_MOTOR_ENCODER_OFFSET_FL = -0.364013671875;
 
 					// Front right
 					VELOCITY_MOTOR_ID_FR = 16;
 					ANGULAR_MOTOR_ID_FR = 17;
-					ANGULAR_MOTOR_ENCODER_ID_FR = 2;
-					ANGULAR_MOTOR_ENCODER_OFFSET_FR = -0.248045977011494;
+					ANGULAR_MOTOR_ENCODER_ID_FR = 4;
+					ANGULAR_MOTOR_ENCODER_OFFSET_FR = -0.42114257812;
 
 					// Back left
 					VELOCITY_MOTOR_ID_BL = 8;
 					ANGULAR_MOTOR_ID_BL = 9;
-					ANGULAR_MOTOR_ENCODER_ID_BL = 4;
-					ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.894674329501916;
+					ANGULAR_MOTOR_ENCODER_ID_BL = 2;
+					ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.634033203125;
 
 					// Back right
 					VELOCITY_MOTOR_ID_BR = 10;
 					ANGULAR_MOTOR_ID_BR = 11;
 					ANGULAR_MOTOR_ENCODER_ID_BR = 1;
-					ANGULAR_MOTOR_ENCODER_OFFSET_BR = -0.530498084291188;
+					ANGULAR_MOTOR_ENCODER_OFFSET_BR = -0.54736328125;
 					break;
 
 				// case COMPETITION_BOT:
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index b214531..3373dc1 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -76,7 +76,8 @@ public class RobotContainer {
     private final Arm arm = new Arm(
         ArmConstants.LEFT_MOTOR_ID,
         ArmConstants.RIGHT_MOTOR_ID,
-        ArmConstants.RIGHT_ENCODER_ID);
+        ArmConstants.RIGHT_ENCODER_ID,
+		ArmConstants.ARE_MOTORS_REVERSED);
 
 	private final AHRS gyro = new AHRS(I2C.Port.kOnboard);
 
@@ -107,6 +108,8 @@ public void setUpDriveController() {
 		final GenericHID genericHID = new GenericHID(DriverConstants.DRIVER_JOYSTICK_PORT);
 		final HIDType genericHIDType = genericHID.getType();
 
+		final CommandJoystick operatorJoystick = new CommandJoystick(1);
+
 		SmartDashboard.putString("Drive Controller", genericHIDType.toString());
 		SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " - " + Constants.serialNumber);
 
@@ -129,18 +132,22 @@ public void setUpDriveController() {
 					new OptionButtonInput(joystick,11, ActivationMode.HOLD), 
 					new OptionButtonInput(joystick,12, ActivationMode.HOLD), 
 
-					new OptionButtonInput(joystick::povLeft, ActivationMode.HOLD), 
-					new OptionButtonInput(joystick::povRight, ActivationMode.HOLD), 
-					new OptionButtonInput(joystick::povDown, ActivationMode.HOLD)
+					new OptionButtonInput(joystick, 4, ActivationMode.HOLD), 
+					new OptionButtonInput(joystick, 5, ActivationMode.HOLD), 
+					new OptionButtonInput(joystick, 6, ActivationMode.HOLD)
 					);
 
 			preciseModeButton = new OptionButtonInput(joystick, 2, ActivationMode.TOGGLE);
 			boostModeButton = new OptionButtonInput(joystick, 1, ActivationMode.HOLD);
 			fieldRelativeButton = new OptionButtonInput(joystick, 3, ActivationMode.TOGGLE);
 
-			joystick.button(10).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
-			joystick.button(11).onTrue(Commands.run(drivetrain::toDefaultStates, drivetrain));
 
+			operatorJoystick.button(4).onTrue(Commands.run(() -> arm.setArmToAmpPosition(), arm));
+			operatorJoystick.button(5).onTrue(Commands.run(() -> arm.setArmToIntakePosition(), arm));
+			operatorJoystick.button(6).onTrue(Commands.run(() -> arm.setArmToSpeakerPosition(), arm));
+
+			joystick.button(9).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
+			joystick.button(10).onTrue(Commands.run(drivetrain::toDefaultStates, drivetrain));
 		} else {
 			final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());
 
@@ -159,6 +166,9 @@ public void setUpDriveController() {
 					new OptionButtonInput(xbox::povDown, ActivationMode.HOLD)
 					);
 
+
+			
+
 			preciseModeButton = new OptionButtonInput(xbox::b, ActivationMode.TOGGLE);
 			boostModeButton = new OptionButtonInput(xbox::leftStick, ActivationMode.HOLD);
 			fieldRelativeButton = new OptionButtonInput(xbox::povUp, ActivationMode.TOGGLE);
diff --git a/src/main/java/frc/robot/commands/ArmRemoteControl.java b/src/main/java/frc/robot/commands/ArmRemoteControl.java
index b37bc38..2fb6ab8 100644
--- a/src/main/java/frc/robot/commands/ArmRemoteControl.java
+++ b/src/main/java/frc/robot/commands/ArmRemoteControl.java
@@ -68,7 +68,7 @@ public void execute() {
 
     @Override
 	public boolean isFinished() {
-		return true;
+		return false;
 	}
 
 	@Override
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 97be8f8..2c2ed1e 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -2,6 +2,8 @@
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.ArmConstants;
 
@@ -25,10 +27,10 @@ public class Arm extends SubsystemBase {
 
     private final PIDController armRaisePIDController;
 
-    private Rotation2d armRotation2d;
+    private Rotation2d armSetpoint;
 
     /** Constructor. Creates a new Arm Subsystem. */
-    public Arm(int leftMotorId, int rightMotorId, int rightEncoderId) {
+    public Arm(int leftMotorId, int rightMotorId, int rightEncoderId, boolean areMotorsReversed) {
 
         leftArmMotor = new CANSparkMax(leftMotorId,MotorType.kBrushless);
         //leftArmEncoder = new CANcoder(leftEncoderId);
@@ -43,9 +45,13 @@ public Arm(int leftMotorId, int rightMotorId, int rightEncoderId) {
         );
 
         armPosition = rightArmEncoder.getAbsolutePosition();
+        armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_INTAKE_DEGREES);
 
-        leftArmMotor.setIdleMode(IdleMode.kBrake);
-        rightArmMotor.setIdleMode(IdleMode.kBrake);
+        leftArmMotor.setIdleMode(IdleMode.kCoast);
+        rightArmMotor.setIdleMode(IdleMode.kCoast);
+
+        leftArmMotor.setInverted(areMotorsReversed);
+        rightArmMotor.setInverted(!areMotorsReversed);
 
     }
 
@@ -57,22 +63,22 @@ public Arm(int leftMotorId, int rightMotorId, int rightEncoderId) {
     // }
 
      public void changeArmAngleDegreesBy(double desiredDegrees) {
-        if (armRotation2d.getDegrees() < ArmConstants.MAXIMUM_ARM_DEGREES || ArmConstants.MINIMUM_ARM_DEGREES > armRotation2d.getDegrees()) {
-            armRotation2d = Rotation2d.fromDegrees(armRotation2d.getDegrees() + desiredDegrees);
+        if (armSetpoint.getDegrees() < ArmConstants.MAXIMUM_ARM_DEGREES || ArmConstants.MINIMUM_ARM_DEGREES > armSetpoint.getDegrees()) {
+            armSetpoint = Rotation2d.fromDegrees(armSetpoint.getDegrees() + desiredDegrees);
         }
     }
 
 
     public void setArmToAmpPosition() {
-        armRotation2d = Rotation2d.fromRadians(ArmConstants.ARM_AMP_SHOOTING_DEGREES);
+        armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_AMP_SHOOTING_DEGREES);
     }
 
     public void setArmToSpeakerPosition() {
-        armRotation2d = Rotation2d.fromRadians(ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES);
+        armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES);
     }
 
     public void setArmToIntakePosition() {
-        armRotation2d = Rotation2d.fromRadians(ArmConstants.ARM_INTAKE_DEGREES);
+        armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_INTAKE_DEGREES);
     }
 
     /**
@@ -82,11 +88,15 @@ public void setArmToIntakePosition() {
     public void periodic() {
         // This method will be called once per scheduler run
 
-        
 
-        final double armSpeed = armRaisePIDController.calculate(armPosition.refresh().getValueAsDouble(),armRotation2d.getRotations());
+        double armSpeed = armRaisePIDController.calculate(armPosition.refresh().getValueAsDouble(),armSetpoint.getRotations());
+
+
         leftArmMotor.set(armSpeed);
         rightArmMotor.set(armSpeed);
+
+        SmartDashboard.putNumber("Arm Degrees", Rotation2d.fromRotations(rightArmEncoder.getAbsolutePosition().getValueAsDouble()).getDegrees());
+        SmartDashboard.putNumber("Arm Setpoint Degrees", armSetpoint.getDegrees());
             
         
     }

From 6ba116340eb6c44854a79c5fbd08a79c6a699984 Mon Sep 17 00:00:00 2001
From: Aleah H <aleahjham@gmail.com>
Date: Mon, 19 Feb 2024 15:04:36 -0800
Subject: [PATCH 73/99] Arm Code Working

---
 src/main/java/frc/robot/Constants.java      | 2 +-
 src/main/java/frc/robot/subsystems/Arm.java | 3 ++-
 2 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 845c184..9d8f8f5 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -83,7 +83,7 @@ public static class ArmConstants {
 
         public static final double DEGREES_PER_SECOND = 2.0;
 
-		public static final double ELEVATION_PID_P = 1;
+		public static final double ELEVATION_PID_P = 15;
 		public static final double ELEVATION_PID_I = 0;
 		public static final double ELEVATION_PID_D = 0;
 
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 2c2ed1e..2707b9c 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -90,7 +90,8 @@ public void periodic() {
 
 
         double armSpeed = armRaisePIDController.calculate(armPosition.refresh().getValueAsDouble(),armSetpoint.getRotations());
-
+        
+        SmartDashboard.putNumber("Arm Speed", armSpeed);
 
         leftArmMotor.set(armSpeed);
         rightArmMotor.set(armSpeed);

From 709282da6c0afd0d13e24b49a08d8b15e7da1bc0 Mon Sep 17 00:00:00 2001
From: Aleah H <aleahjham@gmail.com>
Date: Mon, 19 Feb 2024 15:15:21 -0800
Subject: [PATCH 74/99] Rewrite arm commands

---
 src/main/java/frc/robot/Constants.java        |  2 ++
 src/main/java/frc/robot/RobotContainer.java   | 16 +++++++----
 .../java/frc/robot/commands/ArmRotateTo.java  | 28 +++++++++++++++++++
 src/main/java/frc/robot/subsystems/Arm.java   |  8 ++++++
 4 files changed, 49 insertions(+), 5 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/ArmRotateTo.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 9d8f8f5..c78f7a6 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -53,6 +53,8 @@ public static enum Bot {
 	public static class DriverConstants {
 		public static final int DRIVER_JOYSTICK_PORT = 0;
 
+		public static final int OPERATOR_JOYSTICK_PORT = 1;
+
 		public static final double DEAD_ZONE = 0.25;
 
 		// Names of options for displaying
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 3373dc1..ff69bb2 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -7,6 +7,7 @@
 import frc.robot.subsystems.Arm;
 import frc.robot.commands.Autos;
 import frc.robot.commands.ArmRemoteControl;
+import frc.robot.commands.ArmRotateTo;
 import frc.robot.commands.ChassisRemoteControl;
 import frc.robot.Constants.VisionConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
@@ -88,6 +89,11 @@ public class RobotContainer {
 
 	private final Vision vision = new Vision(VisionConstants.CAMERA_NAME, Constants.VisionConstants.CAMERA_POSE);
 
+
+	private final ArmRotateTo armToIntake = new ArmRotateTo(arm, ArmConstants.ARM_INTAKE_DEGREES);
+	private final ArmRotateTo armToAmp = new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES);
+	private final ArmRotateTo armToSpeaker = new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES);
+
 	/**
 	 * The container for the robot. Contains subsystems, OI devices, and commands.
 	 */
@@ -108,7 +114,7 @@ public void setUpDriveController() {
 		final GenericHID genericHID = new GenericHID(DriverConstants.DRIVER_JOYSTICK_PORT);
 		final HIDType genericHIDType = genericHID.getType();
 
-		final CommandJoystick operatorJoystick = new CommandJoystick(1);
+		final CommandJoystick operatorJoystick = new CommandJoystick(DriverConstants.OPERATOR_JOYSTICK_PORT);
 
 		SmartDashboard.putString("Drive Controller", genericHIDType.toString());
 		SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " - " + Constants.serialNumber);
@@ -141,10 +147,10 @@ public void setUpDriveController() {
 			boostModeButton = new OptionButtonInput(joystick, 1, ActivationMode.HOLD);
 			fieldRelativeButton = new OptionButtonInput(joystick, 3, ActivationMode.TOGGLE);
 
-
-			operatorJoystick.button(4).onTrue(Commands.run(() -> arm.setArmToAmpPosition(), arm));
-			operatorJoystick.button(5).onTrue(Commands.run(() -> arm.setArmToIntakePosition(), arm));
-			operatorJoystick.button(6).onTrue(Commands.run(() -> arm.setArmToSpeakerPosition(), arm));
+			//This bypasses arm remote control, arm remote control is incompatible with autonomous commands
+			operatorJoystick.button(4).onTrue(armToIntake);
+			operatorJoystick.button(5).onTrue(armToAmp);
+			operatorJoystick.button(6).onTrue(armToSpeaker);
 
 			joystick.button(9).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
 			joystick.button(10).onTrue(Commands.run(drivetrain::toDefaultStates, drivetrain));
diff --git a/src/main/java/frc/robot/commands/ArmRotateTo.java b/src/main/java/frc/robot/commands/ArmRotateTo.java
new file mode 100644
index 0000000..0f7c524
--- /dev/null
+++ b/src/main/java/frc/robot/commands/ArmRotateTo.java
@@ -0,0 +1,28 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.Arm;
+
+public class ArmRotateTo extends Command{
+
+    private final double setpoint;
+    private final Arm arm;
+
+    public ArmRotateTo(Arm arm, double degree){
+        this.setpoint = degree;
+        this.arm = arm;
+
+        addRequirements(arm);
+
+    }
+
+    @Override
+    public void initialize() {
+        arm.setSetpoint(setpoint);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return arm.isAtDesiredPosition();
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 2707b9c..f1e0501 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -81,6 +81,14 @@ public void setArmToIntakePosition() {
         armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_INTAKE_DEGREES);
     }
 
+    public void setSetpoint(double degree){
+        armSetpoint = Rotation2d.fromDegrees(degree);
+    }
+
+    public boolean isAtDesiredPosition(){
+        return armRaisePIDController.atSetpoint();
+    }
+
     /**
      * This method is called periodically by the CommandScheduler, about every 20ms.
      */

From 858b80002f3db6a6cd94c4d3ba27c2df52b96ffb Mon Sep 17 00:00:00 2001
From: Aleah H <aleahjham@gmail.com>
Date: Mon, 19 Feb 2024 15:41:38 -0800
Subject: [PATCH 75/99] Im sorry micheal, the fancy code broke it, i just
 hardcoded it

---
 src/main/java/frc/robot/subsystems/SwerveDrivetrain.java | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index 06f8916..48c6479 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -99,8 +99,7 @@ public SwerveDrivetrain(AHRS gyro,
 		modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR };
 
 		// create kinematics object using swerve module distance from center
-		kinematics = new SwerveDriveKinematics(
-				modulesMap(SwerveModule::getDistanceFromCenter, Translation2d[]::new));
+		kinematics = new SwerveDriveKinematics(new Translation2d(.3, .3),new Translation2d(.3, -.3),new Translation2d(-.3, .3),new Translation2d(-.3, -.3));
 
 		// create starting state for odometry
 		odometry = new SwerveDriveOdometry(

From 89853248945aa951a4ab111313f1993152fc2696 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Fri, 23 Feb 2024 17:07:40 -0800
Subject: [PATCH 76/99] Remove unused imports

---
 src/main/java/frc/robot/RobotContainer.java                | 6 ------
 src/main/java/frc/robot/commands/ArmRemoteControl.java     | 3 ---
 src/main/java/frc/robot/commands/ChassisRemoteControl.java | 3 ---
 src/main/java/frc/robot/subsystems/Arm.java                | 1 -
 4 files changed, 13 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index f2f2dcf..06e9473 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -13,8 +13,6 @@
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.SwerveModule;
 import frc.robot.subsystems.Vision;
-import frc.robot.commands.Autos;
-import frc.robot.commands.ChassisRemoteControl;
 import frc.robot.inputs.ChassisDriveInputs;
 import frc.robot.inputs.OptionButtonInput;
 import frc.robot.inputs.OptionButtonInput.ActivationMode;
@@ -22,8 +20,6 @@
 import com.kauailabs.navx.frc.AHRS;
 
 import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.I2C;
-import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.GenericHID.HIDType;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -178,8 +174,6 @@ public void setUpDriveController() {
 					);
 
 
-			
-
 			preciseModeButton = new OptionButtonInput(xbox::b, ActivationMode.TOGGLE);
 			boostModeButton = new OptionButtonInput(xbox::leftStick, ActivationMode.HOLD);
 			fieldRelativeButton = new OptionButtonInput(xbox::povUp, ActivationMode.TOGGLE);
diff --git a/src/main/java/frc/robot/commands/ArmRemoteControl.java b/src/main/java/frc/robot/commands/ArmRemoteControl.java
index 2fb6ab8..bb30a02 100644
--- a/src/main/java/frc/robot/commands/ArmRemoteControl.java
+++ b/src/main/java/frc/robot/commands/ArmRemoteControl.java
@@ -2,12 +2,9 @@
 
 import frc.robot.Constants.ArmConstants;
 import frc.robot.subsystems.Arm;
-import frc.robot.subsystems.ExampleSubsystem;
 import frc.robot.inputs.OptionButtonInput;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.Subsystem;
 
 
 public class ArmRemoteControl extends Command {
diff --git a/src/main/java/frc/robot/commands/ChassisRemoteControl.java b/src/main/java/frc/robot/commands/ChassisRemoteControl.java
index e989e0a..f00fa09 100644
--- a/src/main/java/frc/robot/commands/ChassisRemoteControl.java
+++ b/src/main/java/frc/robot/commands/ChassisRemoteControl.java
@@ -1,12 +1,9 @@
 package frc.robot.commands;
 
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants.ArmConstants;
 import frc.robot.Constants.DriverConstants;
-import frc.robot.subsystems.Arm;
 import frc.robot.inputs.OptionButtonInput;
 import frc.robot.inputs.ChassisDriveInputs;
 import frc.robot.subsystems.SwerveDrivetrain;
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index f1e0501..ecb55a3 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -2,7 +2,6 @@
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants.ArmConstants;

From 2c5f12c3b2993f6faeaee86aab003f14624e36c6 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Sat, 24 Feb 2024 10:45:27 -0800
Subject: [PATCH 77/99] delete unused code

---
 src/main/java/frc/robot/RobotContainer.java   | 21 -------------------
 .../java/frc/robot/commands/AutoRotateTo.java |  3 +--
 .../java/frc/robot/commands/FollowTag.java    |  3 +--
 3 files changed, 2 insertions(+), 25 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 06e9473..126243b 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -6,7 +6,6 @@
 import frc.robot.Constants.ArmConstants;
 import frc.robot.subsystems.Arm;
 import frc.robot.commands.Autos;
-import frc.robot.commands.ArmRemoteControl;
 import frc.robot.commands.ArmRotateTo;
 import frc.robot.commands.ChassisRemoteControl;
 import frc.robot.Constants.VisionConstants;
@@ -122,7 +121,6 @@ public void setUpDriveController() {
 
 		drivetrain.removeDefaultCommand();
 
-		ArmRemoteControl armInputs;
 		ChassisDriveInputs inputs;
 		OptionButtonInput preciseModeButton, boostModeButton, fieldRelativeButton;
 
@@ -135,15 +133,6 @@ public void setUpDriveController() {
 					joystick::getTwist, -1,
 					Constants.DriverConstants.DEAD_ZONE);
 
-			armInputs = new ArmRemoteControl(arm,
-					new OptionButtonInput(joystick,11, ActivationMode.HOLD), 
-					new OptionButtonInput(joystick,12, ActivationMode.HOLD), 
-
-					new OptionButtonInput(joystick, 4, ActivationMode.HOLD), 
-					new OptionButtonInput(joystick, 5, ActivationMode.HOLD), 
-					new OptionButtonInput(joystick, 6, ActivationMode.HOLD)
-					);
-
 			preciseModeButton = new OptionButtonInput(joystick, 2, ActivationMode.TOGGLE);
 			boostModeButton = new OptionButtonInput(joystick, 1, ActivationMode.HOLD);
 			fieldRelativeButton = new OptionButtonInput(joystick, 3, ActivationMode.TOGGLE);
@@ -164,16 +153,6 @@ public void setUpDriveController() {
 					xbox::getRightX, -1,
 					Constants.DriverConstants.DEAD_ZONE);
 
-			armInputs = new ArmRemoteControl(arm,
-					new OptionButtonInput(xbox::rightBumper, ActivationMode.HOLD), 
-					new OptionButtonInput(xbox::leftBumper, ActivationMode.HOLD), 
-
-					new OptionButtonInput(xbox::povLeft, ActivationMode.HOLD), 
-					new OptionButtonInput(xbox::povRight, ActivationMode.HOLD), 
-					new OptionButtonInput(xbox::povDown, ActivationMode.HOLD)
-					);
-
-
 			preciseModeButton = new OptionButtonInput(xbox::b, ActivationMode.TOGGLE);
 			boostModeButton = new OptionButtonInput(xbox::leftStick, ActivationMode.HOLD);
 			fieldRelativeButton = new OptionButtonInput(xbox::povUp, ActivationMode.TOGGLE);
diff --git a/src/main/java/frc/robot/commands/AutoRotateTo.java b/src/main/java/frc/robot/commands/AutoRotateTo.java
index eeef2ba..68ecf31 100644
--- a/src/main/java/frc/robot/commands/AutoRotateTo.java
+++ b/src/main/java/frc/robot/commands/AutoRotateTo.java
@@ -46,8 +46,7 @@ public AutoRotateTo(SwerveDrivetrain drivetrain, Rotation2d direction) {
 
 	@Override
 	public void initialize() {
-		currentAngleGoal = relative ? drivetrain.getHeading().getRadians()
-				: 0;
+		currentAngleGoal = relative ? drivetrain.getHeading().getRadians() : 0;
 		currentAngleGoal += angleGoal;
 		SmartDashboard.putNumber("Target Angle Auto", currentAngleGoal);
 	}
diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java
index 098d481..9c7d2c1 100644
--- a/src/main/java/frc/robot/commands/FollowTag.java
+++ b/src/main/java/frc/robot/commands/FollowTag.java
@@ -52,8 +52,7 @@ public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Transform2d targetD
 	 * 
 	 * @param drivetrain          the drivetrain of the robot
 	 * @param vision              the vision subsystem of the robot
-	 * @param tagID               the numerical ID of the the tag to follow, null
-	 *                            for whatever best is
+	 * @param tagID               the numerical ID of the the tag to follow, null for whatever best is
 	 * @param targetDistanceToTag the target distance away from the tag to be
 	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover
 	 *                            tag, set to null to never finish

From 1d52dcb5960b5c34683be30e8efe9e58b8022dfa Mon Sep 17 00:00:00 2001
From: Aleah H <aleahjham@gmail.com>
Date: Sat, 24 Feb 2024 11:00:12 -0800
Subject: [PATCH 78/99] Make rotateTo variables less ambiguous

---
 src/main/java/frc/robot/commands/AutoRotateTo.java | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/commands/AutoRotateTo.java b/src/main/java/frc/robot/commands/AutoRotateTo.java
index 68ecf31..db68558 100644
--- a/src/main/java/frc/robot/commands/AutoRotateTo.java
+++ b/src/main/java/frc/robot/commands/AutoRotateTo.java
@@ -14,7 +14,7 @@ public class AutoRotateTo extends Command {
 
 	private final PIDController rotatePID;
 	private final double angleGoal;
-	private final boolean relative;
+	private final boolean fieldRelative;
 
 	private double currentAngleGoal;
 
@@ -24,7 +24,7 @@ public class AutoRotateTo extends Command {
 	 * @param drivetrain The robot drivetrain
 	 * @param direction  Rotation2d class to execute
 	 */
-	public AutoRotateTo(SwerveDrivetrain drivetrain, Rotation2d direction, boolean relative) {
+	public AutoRotateTo(SwerveDrivetrain drivetrain, Rotation2d direction, boolean fieldRelative) {
 
 		rotatePID = new PIDController(
 				RobotMovementConstants.ROTATION_PID_P,
@@ -34,7 +34,7 @@ public AutoRotateTo(SwerveDrivetrain drivetrain, Rotation2d direction, boolean r
 		rotatePID.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
 
 		this.drivetrain = drivetrain;
-		this.relative = relative;
+		this.fieldRelative = fieldRelative;
 		this.angleGoal = direction.getRadians();
 
 		addRequirements(this.drivetrain);
@@ -46,7 +46,7 @@ public AutoRotateTo(SwerveDrivetrain drivetrain, Rotation2d direction) {
 
 	@Override
 	public void initialize() {
-		currentAngleGoal = relative ? drivetrain.getHeading().getRadians() : 0;
+		currentAngleGoal = !fieldRelative ? drivetrain.getHeading().getRadians() : 0;
 		currentAngleGoal += angleGoal;
 		SmartDashboard.putNumber("Target Angle Auto", currentAngleGoal);
 	}

From 2f6b8eb351885a790df1eb8df7e1c0a668ebe5d4 Mon Sep 17 00:00:00 2001
From: AikiKapila <aikikapila@seattleacademy.org>
Date: Sat, 24 Feb 2024 12:23:28 -0800
Subject: [PATCH 79/99] Fixed Swerve Module Kinematics

Co-Authored-By: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
---
 src/main/java/frc/robot/Constants.java                   | 4 ++--
 src/main/java/frc/robot/subsystems/SwerveDrivetrain.java | 3 ++-
 2 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 4449491..b972594 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -232,8 +232,8 @@ public static class SwerveDrivetrainConstants {
 
 				case PRACTICE_BOT:
 				default: // Temporary default to practice bot
-					MODULE_LOCATION_X = 54 / 100;
-					MODULE_LOCATION_Y = 54 / 100;
+					MODULE_LOCATION_X = 54.0 / 100;
+					MODULE_LOCATION_Y = 54.0 / 100;
 					break;
 
 				// case COMPETITION_BOT:
diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index 48c6479..9a04d5e 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -99,7 +99,8 @@ public SwerveDrivetrain(AHRS gyro,
 		modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR };
 
 		// create kinematics object using swerve module distance from center
-		kinematics = new SwerveDriveKinematics(new Translation2d(.3, .3),new Translation2d(.3, -.3),new Translation2d(-.3, .3),new Translation2d(-.3, -.3));
+		kinematics = new SwerveDriveKinematics(
+			modulesMap(SwerveModule::getDistanceFromCenter, Translation2d[]::new));
 
 		// create starting state for odometry
 		odometry = new SwerveDriveOdometry(

From ea9f0392cdf23cce935a36c9459b8f7ff6bc2912 Mon Sep 17 00:00:00 2001
From: Aleah H <aleahjham@gmail.com>
Date: Sat, 24 Feb 2024 14:07:39 -0800
Subject: [PATCH 80/99] Change offset

---
 src/main/java/frc/robot/Constants.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index b972594..2047741 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -156,7 +156,7 @@ public static class SwerveModuleConstants {
 					VELOCITY_MOTOR_ID_BL = 8;
 					ANGULAR_MOTOR_ID_BL = 9;
 					ANGULAR_MOTOR_ENCODER_ID_BL = 2;
-					ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.634033203125;
+					ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.13134765625 +0.5;
 
 					// Back right
 					VELOCITY_MOTOR_ID_BR = 10;

From 1847b5105c6a4991e78af0ddb6ea32d12a78a86c Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Sat, 24 Feb 2024 14:21:08 -0800
Subject: [PATCH 81/99] Add super basic turn to tag command

---
 .../java/frc/robot/commands/AutoRotateTo.java |  2 +-
 .../java/frc/robot/commands/TurnToTag.java    | 77 +++++++++++++++++++
 2 files changed, 78 insertions(+), 1 deletion(-)
 create mode 100644 src/main/java/frc/robot/commands/TurnToTag.java

diff --git a/src/main/java/frc/robot/commands/AutoRotateTo.java b/src/main/java/frc/robot/commands/AutoRotateTo.java
index db68558..604a7c3 100644
--- a/src/main/java/frc/robot/commands/AutoRotateTo.java
+++ b/src/main/java/frc/robot/commands/AutoRotateTo.java
@@ -46,7 +46,7 @@ public AutoRotateTo(SwerveDrivetrain drivetrain, Rotation2d direction) {
 
 	@Override
 	public void initialize() {
-		currentAngleGoal = !fieldRelative ? drivetrain.getHeading().getRadians() : 0;
+		currentAngleGoal = fieldRelative ? 0 : drivetrain.getHeading().getRadians();
 		currentAngleGoal += angleGoal;
 		SmartDashboard.putNumber("Target Angle Auto", currentAngleGoal);
 	}
diff --git a/src/main/java/frc/robot/commands/TurnToTag.java b/src/main/java/frc/robot/commands/TurnToTag.java
new file mode 100644
index 0000000..97c774f
--- /dev/null
+++ b/src/main/java/frc/robot/commands/TurnToTag.java
@@ -0,0 +1,77 @@
+package frc.robot.commands;
+
+import frc.robot.Constants.RobotMovementConstants;
+import frc.robot.subsystems.SwerveDrivetrain;
+import frc.robot.subsystems.Vision;
+
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/** Command to automatically drive a follow a tag a certain translation away */
+public class TurnToTag extends Command {
+	private final SwerveDrivetrain drivetrain;
+
+	private final Vision vision;
+	private final Integer tagID;  // Integer as opposed to int so it can be null for best tag
+
+	private final PIDController rotatePID;
+
+	/**
+	 * Create a new FollowTag command. Tries to follow a tag while staying a certain
+	 * distance away.
+	 * 
+	 * @param drivetrain          the drivetrain of the robot
+	 * @param vision              the vision subsystem of the robot
+	 * @param tagID               the numerical ID of the the tag to turn to, null for best tag
+	 */
+	public TurnToTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID) {
+		this.drivetrain = drivetrain;
+
+		this.vision = vision;
+		this.tagID = tagID;
+
+        rotatePID = new PIDController(
+            RobotMovementConstants.ROTATION_PID_P,
+            RobotMovementConstants.ROTATION_PID_I,
+            RobotMovementConstants.ROTATION_PID_D);
+        rotatePID.enableContinuousInput(-Math.PI, Math.PI);
+        rotatePID.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
+
+		addRequirements(drivetrain);
+	}
+
+	@Override
+	public void initialize() {
+		drivetrain.toDefaultStates();
+
+        final PhotonTrackedTarget tag = (tagID == null) ? vision.getTag() : vision.getTag(tagID);
+        rotatePID.setSetpoint(tag.getYaw());
+	}
+
+
+	@Override
+	public void execute() {
+    	final double currentAngle = drivetrain.getHeading().getRadians();
+
+		double turnsSeed = rotatePID.calculate(currentAngle);
+		SmartDashboard.putNumber("Turn Speed Auto", turnsSeed);
+
+		drivetrain.setDesiredState(new ChassisSpeeds(0, 0, turnsSeed));
+
+		drivetrain.updateSmartDashboard();
+	}
+
+	@Override
+	public boolean isFinished() {
+		return rotatePID.atSetpoint();
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		drivetrain.stop();
+	}
+}

From 8c54ac56fcc36cf663b04a2efe573163bd2714c5 Mon Sep 17 00:00:00 2001
From: AikiKapila <aikikapila@seattleacademy.org>
Date: Sat, 24 Feb 2024 15:34:10 -0800
Subject: [PATCH 82/99] Create FaceTag.java

---
 src/main/java/frc/robot/commands/FaceTag.java | 37 +++++++++++++++++++
 1 file changed, 37 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/FaceTag.java

diff --git a/src/main/java/frc/robot/commands/FaceTag.java b/src/main/java/frc/robot/commands/FaceTag.java
new file mode 100644
index 0000000..e493410
--- /dev/null
+++ b/src/main/java/frc/robot/commands/FaceTag.java
@@ -0,0 +1,37 @@
+package frc.robot.commands;
+import java.util.Map;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants.RobotMovementConstants;
+import frc.robot.subsystems.SwerveDrivetrain;
+import frc.robot.subsystems.Vision;
+
+public class FaceTag extends Command {
+    private final Vision vision;
+    private final SwerveDrivetrain drivetrain;
+    private final PIDController rotationPID;
+
+    public FaceTag(Vision vision, SwerveDrivetrain drivetrain, Map<Integer,Double> targetTagPos){
+        this.vision = vision;
+        this.drivetrain = drivetrain;
+        rotationPID = new PIDController(
+            RobotMovementConstants.ROTATION_PID_P,
+            RobotMovementConstants.ROTATION_PID_I,
+            RobotMovementConstants.ROTATION_PID_D);
+        rotationPID.enableContinuousInput(-Math.PI, Math.PI);
+        rotationPID.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
+
+        addRequirements(drivetrain);
+    }
+
+    @Override
+    public void initialize(){
+        drivetrain.toDefaultStates();
+    }
+
+    @Override
+    public void execute(){   
+    }
+
+}

From d306a5ccb000b0fe1b7b6e726848ff953d23d54f Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Sat, 24 Feb 2024 16:00:29 -0800
Subject: [PATCH 83/99] combine comp and test bot

---
 src/main/java/frc/robot/Constants.java | 53 +++++++++++++-------------
 1 file changed, 27 insertions(+), 26 deletions(-)

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 2047741..ed106ab 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -15,7 +15,7 @@
  */
 public final class Constants {
 	public static enum Bot {
-		WOOD_BOT, PRACTICE_BOT, COMPETITION_BOT
+		WOOD_BOT, COMP_BOT
 	}
 
 	public static final Bot currentBot;
@@ -40,12 +40,9 @@ public static enum Bot {
 				currentBot = Bot.WOOD_BOT;
 				break;
 
-			case "03238024": // Practice Bot Serial Number
-				currentBot = Bot.PRACTICE_BOT;
-				break;
-
-			default: // Also use competition bot as default
-				currentBot = Bot.COMPETITION_BOT;
+			case "03238024": // Practice (Now comp) Bot Serial Number
+			default:
+				currentBot = Bot.COMP_BOT;
 				break;
 		}
 	}
@@ -68,6 +65,20 @@ public static class DriverConstants {
 	}
 
     public static class ArmConstants {
+		static {
+			switch (currentBot) {
+				case WOOD_BOT:
+					HAR_ARM = false;
+					break;
+
+				case COMP_BOT:
+				default:
+					HAR_ARM = true;
+					break;
+			}
+		}
+
+		public static final boolean HAR_ARM;
 
         public static final double MAXIMUM_ARM_DEGREES = 1;
         public static final double MINIMUM_ARM_DEGREES = 0;
@@ -117,28 +128,28 @@ public static class SwerveModuleConstants {
 					VELOCITY_MOTOR_ID_FL = 41;
 					ANGULAR_MOTOR_ID_FL = 40;
 					ANGULAR_MOTOR_ENCODER_ID_FL = 1;
-					ANGULAR_MOTOR_ENCODER_OFFSET_FL = -0.611328125;
+					ANGULAR_MOTOR_ENCODER_OFFSET_FL = -0.136474609375;
 
 					// Front right
 					VELOCITY_MOTOR_ID_FR = 4;
 					ANGULAR_MOTOR_ID_FR = 5;
 					ANGULAR_MOTOR_ENCODER_ID_FR = 2;
-					ANGULAR_MOTOR_ENCODER_OFFSET_FR = -0.282958;
+					ANGULAR_MOTOR_ENCODER_OFFSET_FR = -0.8828125;
 
 					// Back left
-					VELOCITY_MOTOR_ID_BL = 3;
-					ANGULAR_MOTOR_ID_BL = 2;
+					VELOCITY_MOTOR_ID_BL = 2;
+					ANGULAR_MOTOR_ID_BL = 3;
 					ANGULAR_MOTOR_ENCODER_ID_BL = 4;
-					ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.91748046875;
+					ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.517333984375 + 0.5;
 
 					// Back right
 					VELOCITY_MOTOR_ID_BR = 42;
 					ANGULAR_MOTOR_ID_BR = 6;
 					ANGULAR_MOTOR_ENCODER_ID_BR = 3;
-					ANGULAR_MOTOR_ENCODER_OFFSET_BR = -0.8641367187;
+					ANGULAR_MOTOR_ENCODER_OFFSET_BR = -0.52001953125 + 0.5;
 					break;
 
-				case PRACTICE_BOT:
+				case COMP_BOT:
 				default: // Temporary default to practice bot
 					// Front Left
 					VELOCITY_MOTOR_ID_FL = 2;
@@ -164,11 +175,6 @@ public static class SwerveModuleConstants {
 					ANGULAR_MOTOR_ENCODER_ID_BR = 1;
 					ANGULAR_MOTOR_ENCODER_OFFSET_BR = -0.54736328125;
 					break;
-
-				// case COMPETITION_BOT:
-				// default:
-
-				// break;
 			}
 		}
 
@@ -230,16 +236,11 @@ public static class SwerveDrivetrainConstants {
 					MODULE_LOCATION_Y = 28.5 / 100;
 					break;
 
-				case PRACTICE_BOT:
-				default: // Temporary default to practice bot
+				case COMP_BOT:
+				default:
 					MODULE_LOCATION_X = 54.0 / 100;
 					MODULE_LOCATION_Y = 54.0 / 100;
 					break;
-
-				// case COMPETITION_BOT:
-				// default:
-
-				// break;
 			}
 		}
 

From 3af8400bfe2827e7e2482d17fff47226ed4aa0f3 Mon Sep 17 00:00:00 2001
From: Aceius E <aceiuselms@seattleacademy.org>
Date: Mon, 26 Feb 2024 15:46:06 -0800
Subject: [PATCH 84/99] Re-add thing aleah accidently deleted

---
 .vscode/settings.json | 72 +++++++++++++++++++++----------------------
 1 file changed, 36 insertions(+), 36 deletions(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index c0fc28e..0c4ad71 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,37 +1,37 @@
 {
-  "java.configuration.updateBuildConfiguration": "automatic",
-  "java.server.launchMode": "Standard",
-  "files.exclude": {
-    "**/.git": true,
-    "**/.svn": true,
-    "**/.hg": true,
-    "**/CVS": true,
-    "**/.DS_Store": true,
-    "bin/": true,
-    "**/.classpath": true,
-    "**/.project": true,
-    "**/.settings": true,
-    "**/.factorypath": true,
-    "**/*~": true
-  },
-  "java.test.config": [
-    {
-      "name": "WPIlibUnitTests",
-      "workingDirectory": "${workspaceFolder}/build/jni/release",
-      "vmargs": [ "-Djava.library.path=${workspaceFolder}/build/jni/release" ],
-      "env": {
-        "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" ,
-        "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release"
-      }
-    },
-  ],
-  "java.test.defaultConfig": "WPIlibUnitTests",
-  "cSpell.words": [
-    "AHRS",
-    "Deadzone",
-    "Odometry",
-    "setpoint",
-    "velociticities"
-  ]
-}
-
+	"java.configuration.updateBuildConfiguration": "automatic",
+	"java.server.launchMode": "Standard",
+	"files.exclude": {
+		"**/.git": true,
+		"**/.svn": true,
+		"**/.hg": true,
+		"**/CVS": true,
+		"**/.DS_Store": true,
+		"bin/": true,
+		"**/.classpath": true,
+		"**/.project": true,
+		"**/.settings": true,
+		"**/.factorypath": true,
+		"**/*~": true
+	},
+	"java.test.config": [
+		{
+			"name": "WPIlibUnitTests",
+			"workingDirectory": "${workspaceFolder}/build/jni/release",
+			"vmargs": [
+				"-Djava.library.path=${workspaceFolder}/build/jni/release"
+			],
+			"env": {
+				"LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release",
+				"DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release"
+			}
+		},
+	],
+	"java.test.defaultConfig": "WPIlibUnitTests",
+	// Style guide - most of this came from clicking buttons and then copying from my user settings.json
+	// General editor stuff
+	"editor.formatOnSave": true, // So you cant forget
+	"editor.tabSize": 4, // 4 char width indentation
+	"editor.detectIndentation": false, // Prevent above from being overridden
+	"editor.insertSpaces": false // use tab character instead of spaces
+}
\ No newline at end of file

From 250fb5ee1658af61f0ee369978c83a0698cad295 Mon Sep 17 00:00:00 2001
From: Aceius E <aceiuselms@seattleacademy.org>
Date: Mon, 26 Feb 2024 15:52:58 -0800
Subject: [PATCH 85/99] Always fix everything

---
 .vscode/settings.json                         |  8 ++-
 src/main/java/frc/robot/commands/FaceTag.java | 55 ++++++++++---------
 2 files changed, 35 insertions(+), 28 deletions(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 0c4ad71..ccc8f02 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -28,10 +28,12 @@
 		},
 	],
 	"java.test.defaultConfig": "WPIlibUnitTests",
-	// Style guide - most of this came from clicking buttons and then copying from my user settings.json
-	// General editor stuff
+	// Auto Formatter
 	"editor.formatOnSave": true, // So you cant forget
 	"editor.tabSize": 4, // 4 char width indentation
 	"editor.detectIndentation": false, // Prevent above from being overridden
-	"editor.insertSpaces": false // use tab character instead of spaces
+	"editor.insertSpaces": false, // use tab character instead of spaces
+	"editor.codeActionsOnSave": {
+		"source.fixAll": "always"
+	}
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/FaceTag.java b/src/main/java/frc/robot/commands/FaceTag.java
index e493410..24e5e3f 100644
--- a/src/main/java/frc/robot/commands/FaceTag.java
+++ b/src/main/java/frc/robot/commands/FaceTag.java
@@ -1,4 +1,5 @@
 package frc.robot.commands;
+
 import java.util.Map;
 
 import edu.wpi.first.math.controller.PIDController;
@@ -8,30 +9,34 @@
 import frc.robot.subsystems.Vision;
 
 public class FaceTag extends Command {
-    private final Vision vision;
-    private final SwerveDrivetrain drivetrain;
-    private final PIDController rotationPID;
-
-    public FaceTag(Vision vision, SwerveDrivetrain drivetrain, Map<Integer,Double> targetTagPos){
-        this.vision = vision;
-        this.drivetrain = drivetrain;
-        rotationPID = new PIDController(
-            RobotMovementConstants.ROTATION_PID_P,
-            RobotMovementConstants.ROTATION_PID_I,
-            RobotMovementConstants.ROTATION_PID_D);
-        rotationPID.enableContinuousInput(-Math.PI, Math.PI);
-        rotationPID.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
-
-        addRequirements(drivetrain);
-    }
-
-    @Override
-    public void initialize(){
-        drivetrain.toDefaultStates();
-    }
-
-    @Override
-    public void execute(){   
-    }
+	private final Vision vision;
+	private final SwerveDrivetrain drivetrain;
+	private final PIDController rotationPID;
+
+	public FaceTag(Vision vision, SwerveDrivetrain drivetrain, Map<Integer, Double> targetTagPos) {
+		this.vision = vision;
+		this.drivetrain = drivetrain;
+		rotationPID = new PIDController(
+				RobotMovementConstants.ROTATION_PID_P,
+				RobotMovementConstants.ROTATION_PID_I,
+				RobotMovementConstants.ROTATION_PID_D);
+		rotationPID.enableContinuousInput(-Math.PI, Math.PI);
+		rotationPID.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
+
+		addRequirements(drivetrain);
+	}
+
+	@Override
+	public void initialize() {
+		drivetrain.toDefaultStates();
+	}
+
+	public void a(String amog) {
+		return;
+	}
+
+	@Override
+	public void execute() {
+	}
 
 }

From edcf8628993c0fe7359c310abe91643c4eea42db Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Mon, 26 Feb 2024 16:28:27 -0800
Subject: [PATCH 86/99] Fix aim to tag and rename

---
 .../java/frc/robot/commands/AimAtTag.java     | 101 ++++++++++++++++++
 .../java/frc/robot/commands/TurnToTag.java    |  77 -------------
 2 files changed, 101 insertions(+), 77 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/AimAtTag.java
 delete mode 100644 src/main/java/frc/robot/commands/TurnToTag.java

diff --git a/src/main/java/frc/robot/commands/AimAtTag.java b/src/main/java/frc/robot/commands/AimAtTag.java
new file mode 100644
index 0000000..4a01e0c
--- /dev/null
+++ b/src/main/java/frc/robot/commands/AimAtTag.java
@@ -0,0 +1,101 @@
+package frc.robot.commands;
+
+import frc.robot.Constants.DriverConstants;
+import frc.robot.Constants.RobotMovementConstants;
+import frc.robot.inputs.ChassisDriveInputs;
+import frc.robot.subsystems.SwerveDrivetrain;
+import frc.robot.subsystems.Vision;
+
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/** Command to automatically aim at a tag, ends once facing the tag */
+public class AimAtTag extends Command {
+	private final SwerveDrivetrain drivetrain;
+	private final ChassisDriveInputs chassisDriveInputs;
+
+	private final Vision vision;
+	private final Integer tagID;  // Integer as opposed to int so it can be null for best tag
+
+	private final PIDController rotatePID;
+
+	/**
+	 * Create a new AimAtTag command. Tries to constants aim at a tag while still allowing driver to control robot.
+	 * 
+	 * @param drivetrain          the drivetrain of the robot
+	 * @param vision              the vision subsystem of the robot
+	 * @param tagID               the numerical ID of the the tag to turn to, null for best tag
+	 * @param chassisDriveControl collection of inputs for driving 
+	 */
+	public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID, ChassisDriveInputs chassisDriveInputs) {
+		this.drivetrain = drivetrain;
+
+		this.vision = vision;
+		this.tagID = tagID;
+		this.chassisDriveInputs = chassisDriveInputs;
+
+        rotatePID = new PIDController(
+            1,
+            0,
+            0);
+        rotatePID.enableContinuousInput(-1, 1);
+        rotatePID.setTolerance(Units.radiansToDegrees(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS));
+		rotatePID.setSetpoint(0);
+
+		addRequirements(drivetrain);
+	}
+
+	/**
+	 * Create a new AimAtTag command. Tries to aim at a tag.
+	 * 
+	 * @param drivetrain          the drivetrain of the robot
+	 * @param vision              the vision subsystem of the robot
+	 * @param tagID               the numerical ID of the the tag to turn to, null for best tag
+	 */
+	public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID) {
+		this(drivetrain, vision, tagID, null);
+	}
+
+	@Override
+	public void initialize() {
+		drivetrain.toDefaultStates();
+	}
+
+	@Override
+	public void execute() {
+		PhotonTrackedTarget tag = (tagID == null) ? vision.getTag() : vision.getTag(tagID);
+
+		double tagYawRadians = 0;
+		if (tag != null) {
+			tagYawRadians = Units.degreesToRadians(tag.getYaw());
+		}
+
+		double xSpeed = 0;
+		double ySpeed = 0;
+		if (chassisDriveInputs != null) {
+			xSpeed = chassisDriveInputs.getX() * DriverConstants.maxSpeedOptionsTranslation[0];
+			ySpeed = chassisDriveInputs.getY() * DriverConstants.maxSpeedOptionsTranslation[0];
+		}
+		double rotateSpeed = rotatePID.calculate(tagYawRadians) * DriverConstants.maxSpeedOptionsRotation[0]; 
+		
+		ChassisSpeeds desiredSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rotateSpeed);
+
+		drivetrain.setDesiredState(desiredSpeeds, false, true);
+
+		drivetrain.updateSmartDashboard();
+	}
+
+	@Override
+	public boolean isFinished() {
+		return (chassisDriveInputs == null) && (rotatePID.atSetpoint());
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		drivetrain.stop();
+	}
+}
diff --git a/src/main/java/frc/robot/commands/TurnToTag.java b/src/main/java/frc/robot/commands/TurnToTag.java
deleted file mode 100644
index 97c774f..0000000
--- a/src/main/java/frc/robot/commands/TurnToTag.java
+++ /dev/null
@@ -1,77 +0,0 @@
-package frc.robot.commands;
-
-import frc.robot.Constants.RobotMovementConstants;
-import frc.robot.subsystems.SwerveDrivetrain;
-import frc.robot.subsystems.Vision;
-
-import org.photonvision.targeting.PhotonTrackedTarget;
-
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-
-/** Command to automatically drive a follow a tag a certain translation away */
-public class TurnToTag extends Command {
-	private final SwerveDrivetrain drivetrain;
-
-	private final Vision vision;
-	private final Integer tagID;  // Integer as opposed to int so it can be null for best tag
-
-	private final PIDController rotatePID;
-
-	/**
-	 * Create a new FollowTag command. Tries to follow a tag while staying a certain
-	 * distance away.
-	 * 
-	 * @param drivetrain          the drivetrain of the robot
-	 * @param vision              the vision subsystem of the robot
-	 * @param tagID               the numerical ID of the the tag to turn to, null for best tag
-	 */
-	public TurnToTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID) {
-		this.drivetrain = drivetrain;
-
-		this.vision = vision;
-		this.tagID = tagID;
-
-        rotatePID = new PIDController(
-            RobotMovementConstants.ROTATION_PID_P,
-            RobotMovementConstants.ROTATION_PID_I,
-            RobotMovementConstants.ROTATION_PID_D);
-        rotatePID.enableContinuousInput(-Math.PI, Math.PI);
-        rotatePID.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
-
-		addRequirements(drivetrain);
-	}
-
-	@Override
-	public void initialize() {
-		drivetrain.toDefaultStates();
-
-        final PhotonTrackedTarget tag = (tagID == null) ? vision.getTag() : vision.getTag(tagID);
-        rotatePID.setSetpoint(tag.getYaw());
-	}
-
-
-	@Override
-	public void execute() {
-    	final double currentAngle = drivetrain.getHeading().getRadians();
-
-		double turnsSeed = rotatePID.calculate(currentAngle);
-		SmartDashboard.putNumber("Turn Speed Auto", turnsSeed);
-
-		drivetrain.setDesiredState(new ChassisSpeeds(0, 0, turnsSeed));
-
-		drivetrain.updateSmartDashboard();
-	}
-
-	@Override
-	public boolean isFinished() {
-		return rotatePID.atSetpoint();
-	}
-
-	@Override
-	public void end(boolean interrupted) {
-		drivetrain.stop();
-	}
-}

From a4d5f8143620eae9818651c8154dc07490eab8f5 Mon Sep 17 00:00:00 2001
From: Aceius E <144858100+AceiusRedshift@users.noreply.github.com>
Date: Mon, 26 Feb 2024 16:45:15 -0800
Subject: [PATCH 87/99] Convert Arm class to interface (#3)

* Arm as interface

* Fix type error relating to arm interface
---
 src/main/java/frc/robot/Constants.java        |  32 ++---
 src/main/java/frc/robot/RobotContainer.java   |  23 ++--
 .../frc/robot/commands/ArmRemoteControl.java  |  64 +++++-----
 .../java/frc/robot/commands/ArmRotateTo.java  |  34 +++---
 src/main/java/frc/robot/commands/Autos.java   |   4 +-
 src/main/java/frc/robot/subsystems/Arm.java   | 111 -----------------
 .../robot/subsystems/arm/ArmInterface.java    |  21 ++++
 .../frc/robot/subsystems/arm/DummyArm.java    |  30 +++++
 .../frc/robot/subsystems/arm/RealArm.java     | 113 ++++++++++++++++++
 9 files changed, 245 insertions(+), 187 deletions(-)
 delete mode 100644 src/main/java/frc/robot/subsystems/Arm.java
 create mode 100644 src/main/java/frc/robot/subsystems/arm/ArmInterface.java
 create mode 100644 src/main/java/frc/robot/subsystems/arm/DummyArm.java
 create mode 100644 src/main/java/frc/robot/subsystems/arm/RealArm.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index ed106ab..395e686 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -64,43 +64,43 @@ public static class DriverConstants {
 		public static final double[] maxSpeedOptionsRotation = { 0.25, 0.75, 1 };
 	}
 
-    public static class ArmConstants {
+	public static class ArmConstants {
 		static {
 			switch (currentBot) {
 				case WOOD_BOT:
-					HAR_ARM = false;
+					HAS_ARM = false;
 					break;
 
 				case COMP_BOT:
 				default:
-					HAR_ARM = true;
+					HAS_ARM = true;
 					break;
 			}
 		}
 
-		public static final boolean HAR_ARM;
+		public static final boolean HAS_ARM;
 
-        public static final double MAXIMUM_ARM_DEGREES = 1;
-        public static final double MINIMUM_ARM_DEGREES = 0;
+		public static final double MAXIMUM_ARM_DEGREES = 1;
+		public static final double MINIMUM_ARM_DEGREES = 0;
 
-        public static final double ARM_AMP_SHOOTING_DEGREES = -20;
-        public static final double ARM_SPEAKER_SHOOTING_DEGREES = 45;
-        public static final double ARM_INTAKE_DEGREES = -40;
+		public static final double ARM_AMP_SHOOTING_DEGREES = -20;
+		public static final double ARM_SPEAKER_SHOOTING_DEGREES = 45;
+		public static final double ARM_INTAKE_DEGREES = -40;
 
-        public static final int LEFT_MOTOR_ID = 5;
-        //public static final int LEFT_ENCODER_ID = 0;
-        public static final int RIGHT_MOTOR_ID = 19;
-        public static final int RIGHT_ENCODER_ID = 6;
+		public static final int LEFT_MOTOR_ID = 5;
+		// public static final int LEFT_ENCODER_ID = 0;
+		public static final int RIGHT_MOTOR_ID = 19;
+		public static final int RIGHT_ENCODER_ID = 6;
 
 		public static final boolean ARE_MOTORS_REVERSED = false;
 
-        public static final double DEGREES_PER_SECOND = 2.0;
+		public static final double DEGREES_PER_SECOND = 2.0;
 
 		public static final double ELEVATION_PID_P = 15;
 		public static final double ELEVATION_PID_I = 0;
 		public static final double ELEVATION_PID_D = 0;
 
-    }
+	}
 
 	public static class RobotMovementConstants {
 		public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(0.0001);
@@ -167,7 +167,7 @@ public static class SwerveModuleConstants {
 					VELOCITY_MOTOR_ID_BL = 8;
 					ANGULAR_MOTOR_ID_BL = 9;
 					ANGULAR_MOTOR_ENCODER_ID_BL = 2;
-					ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.13134765625 +0.5;
+					ANGULAR_MOTOR_ENCODER_OFFSET_BL = -0.13134765625 + 0.5;
 
 					// Back right
 					VELOCITY_MOTOR_ID_BR = 10;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 126243b..47100e2 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,7 +4,6 @@
 import frc.robot.Constants.SwerveDrivetrainConstants;
 import frc.robot.Constants.SwerveModuleConstants;
 import frc.robot.Constants.ArmConstants;
-import frc.robot.subsystems.Arm;
 import frc.robot.commands.Autos;
 import frc.robot.commands.ArmRotateTo;
 import frc.robot.commands.ChassisRemoteControl;
@@ -12,6 +11,9 @@
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.SwerveModule;
 import frc.robot.subsystems.Vision;
+import frc.robot.subsystems.arm.ArmInterface;
+import frc.robot.subsystems.arm.DummyArm;
+import frc.robot.subsystems.arm.RealArm;
 import frc.robot.inputs.ChassisDriveInputs;
 import frc.robot.inputs.OptionButtonInput;
 import frc.robot.inputs.OptionButtonInput.ActivationMode;
@@ -70,12 +72,17 @@ public class RobotContainer {
 					-SwerveDrivetrainConstants.MODULE_LOCATION_Y));
 
 	private final AHRS gyro = new AHRS();
-    private final Arm arm = new Arm(
-        ArmConstants.LEFT_MOTOR_ID,
-        ArmConstants.RIGHT_MOTOR_ID,
-        ArmConstants.RIGHT_ENCODER_ID,
-		ArmConstants.ARE_MOTORS_REVERSED);
 
+	/**
+	 * This is the robot arm. In some situations, the robot may not have an arm, so
+	 * if ArmConstants.HAS_ARM is false, a dummy class implementing the arm's API is
+	 * created instead to prevent errors.
+	 */
+	private final ArmInterface arm = Constants.ArmConstants.HAS_ARM ? new RealArm(
+			ArmConstants.LEFT_MOTOR_ID,
+			ArmConstants.RIGHT_MOTOR_ID,
+			ArmConstants.RIGHT_ENCODER_ID,
+			ArmConstants.ARE_MOTORS_REVERSED) : new DummyArm();
 
 	private final SwerveDrivetrain drivetrain = new SwerveDrivetrain(gyro, swerveModuleFL, swerveModuleFR,
 			swerveModuleBL, swerveModuleBR);
@@ -84,7 +91,6 @@ public class RobotContainer {
 
 	private final Vision vision = new Vision(VisionConstants.CAMERA_NAME, VisionConstants.CAMERA_POSE);
 
-
 	private final ArmRotateTo armToIntake = new ArmRotateTo(arm, ArmConstants.ARM_INTAKE_DEGREES);
 	private final ArmRotateTo armToAmp = new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES);
 	private final ArmRotateTo armToSpeaker = new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES);
@@ -137,7 +143,8 @@ public void setUpDriveController() {
 			boostModeButton = new OptionButtonInput(joystick, 1, ActivationMode.HOLD);
 			fieldRelativeButton = new OptionButtonInput(joystick, 3, ActivationMode.TOGGLE);
 
-			//This bypasses arm remote control, arm remote control is incompatible with autonomous commands
+			// This bypasses arm remote control, arm remote control is incompatible with
+			// autonomous commands
 			operatorJoystick.button(4).onTrue(armToIntake);
 			operatorJoystick.button(5).onTrue(armToAmp);
 			operatorJoystick.button(6).onTrue(armToSpeaker);
diff --git a/src/main/java/frc/robot/commands/ArmRemoteControl.java b/src/main/java/frc/robot/commands/ArmRemoteControl.java
index bb30a02..5384f00 100644
--- a/src/main/java/frc/robot/commands/ArmRemoteControl.java
+++ b/src/main/java/frc/robot/commands/ArmRemoteControl.java
@@ -1,40 +1,36 @@
 package frc.robot.commands;
 
 import frc.robot.Constants.ArmConstants;
-import frc.robot.subsystems.Arm;
 import frc.robot.inputs.OptionButtonInput;
+import frc.robot.subsystems.arm.ArmInterface;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 
-
 public class ArmRemoteControl extends Command {
-	private final Arm arm;
+	private final ArmInterface arm;
 
 	private final OptionButtonInput raiseArmButton;
 	private final OptionButtonInput lowerArmButton;
 
-    private final OptionButtonInput speakerPositionButton;
-    private final OptionButtonInput ampPositionButton;
-    private final OptionButtonInput intakePositionButton;
-
-
-	public ArmRemoteControl(Arm arm, OptionButtonInput raiseArmButton, OptionButtonInput lowerArmButton,
-            OptionButtonInput speakerPositionButton, OptionButtonInput ampPositionButton, OptionButtonInput intakePositionButton) {
-        this.arm = arm;
+	private final OptionButtonInput speakerPositionButton;
+	private final OptionButtonInput ampPositionButton;
+	private final OptionButtonInput intakePositionButton;
 
-        this.raiseArmButton = raiseArmButton;
-        this.lowerArmButton = lowerArmButton;
+	public ArmRemoteControl(ArmInterface arm, OptionButtonInput raiseArmButton, OptionButtonInput lowerArmButton,
+			OptionButtonInput speakerPositionButton, OptionButtonInput ampPositionButton,
+			OptionButtonInput intakePositionButton) {
+		this.arm = arm;
 
-        this.speakerPositionButton = speakerPositionButton;
-        this.ampPositionButton = ampPositionButton;
-        this.intakePositionButton = intakePositionButton;
+		this.raiseArmButton = raiseArmButton;
+		this.lowerArmButton = lowerArmButton;
 
+		this.speakerPositionButton = speakerPositionButton;
+		this.ampPositionButton = ampPositionButton;
+		this.intakePositionButton = intakePositionButton;
 
 		addRequirements(arm);
 	}
 
-
-	
 	@Override
 	public void initialize() {
 	}
@@ -46,24 +42,26 @@ public void initialize() {
 	@Override
 	public void execute() {
 
-        // Arm Motor
-		arm.changeArmAngleDegreesBy(Double.valueOf(raiseArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
-        arm.changeArmAngleDegreesBy(Double.valueOf(-lowerArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod * ArmConstants.DEGREES_PER_SECOND);
-
-        // Arm Povs
-        if (speakerPositionButton.getState()) {
-            arm.setArmToSpeakerPosition();
-        }
-        if (ampPositionButton.getState()) {
-            arm.setArmToAmpPosition();
-        }
-        if (intakePositionButton.getState()) {
-            arm.setArmToIntakePosition();
-        }
+		// Arm Motor
+		arm.changeArmAngleDegreesBy(Double.valueOf(raiseArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod
+				* ArmConstants.DEGREES_PER_SECOND);
+		arm.changeArmAngleDegreesBy(Double.valueOf(-lowerArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod
+				* ArmConstants.DEGREES_PER_SECOND);
+
+		// Arm Povs
+		if (speakerPositionButton.getState()) {
+			arm.setArmToSpeakerPosition();
+		}
+		if (ampPositionButton.getState()) {
+			arm.setArmToAmpPosition();
+		}
+		if (intakePositionButton.getState()) {
+			arm.setArmToIntakePosition();
+		}
 
 	}
 
-    @Override
+	@Override
 	public boolean isFinished() {
 		return false;
 	}
diff --git a/src/main/java/frc/robot/commands/ArmRotateTo.java b/src/main/java/frc/robot/commands/ArmRotateTo.java
index 0f7c524..6927824 100644
--- a/src/main/java/frc/robot/commands/ArmRotateTo.java
+++ b/src/main/java/frc/robot/commands/ArmRotateTo.java
@@ -1,28 +1,28 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.Arm;
+import frc.robot.subsystems.arm.ArmInterface;
 
-public class ArmRotateTo extends Command{
+public class ArmRotateTo extends Command {
 
-    private final double setpoint;
-    private final Arm arm;
+	private final double setpoint;
+	private final ArmInterface arm;
 
-    public ArmRotateTo(Arm arm, double degree){
-        this.setpoint = degree;
-        this.arm = arm;
+	public ArmRotateTo(ArmInterface arm, double degree) {
+		this.setpoint = degree;
+		this.arm = arm;
 
-        addRequirements(arm);
+		addRequirements(arm);
 
-    }
+	}
 
-    @Override
-    public void initialize() {
-        arm.setSetpoint(setpoint);
-    }
+	@Override
+	public void initialize() {
+		arm.setSetpoint(setpoint);
+	}
 
-    @Override
-    public boolean isFinished() {
-        return arm.isAtDesiredPosition();
-    }
+	@Override
+	public boolean isFinished() {
+		return arm.isAtDesiredPosition();
+	}
 }
diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java
index 2aee1e2..57eaeef 100644
--- a/src/main/java/frc/robot/commands/Autos.java
+++ b/src/main/java/frc/robot/commands/Autos.java
@@ -2,12 +2,12 @@
 
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.Vision;
+import frc.robot.subsystems.arm.ArmInterface;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.Constants.SwerveDrivetrainConstants;
-import frc.robot.subsystems.Arm;
 
 /**
  * This class just contains a bunch of auto-modes. Do not call this class
@@ -50,7 +50,7 @@ public static Command tagFollowAuto(SwerveDrivetrain drivetrain, Vision camera,
 	}
 
 	/** Linden did this */
-	public static Command startingAuto(Arm arm, SwerveDrivetrain drivetrain, boolean invertY) {
+	public static Command startingAuto(ArmInterface arm, SwerveDrivetrain drivetrain, boolean invertY) {
 
 		// assumes start position in corner
 		double invert = 1;
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
deleted file mode 100644
index ecb55a3..0000000
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ /dev/null
@@ -1,111 +0,0 @@
-package frc.robot.subsystems;
-
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc.robot.Constants.ArmConstants;
-
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkBase.IdleMode;
-import com.revrobotics.CANSparkLowLevel.MotorType;
-import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.hardware.CANcoder;
-
-public class Arm extends SubsystemBase {
-
-    private final CANSparkMax leftArmMotor;
-    //private final CANcoder leftArmEncoder;
-
-    //leftArmEncoder may be useless, as the right arm equivelent is used for the position for both.</p>
-
-    private final CANSparkMax rightArmMotor;
-    private final CANcoder rightArmEncoder;
-
-    private final StatusSignal<Double> armPosition;
-
-    private final PIDController armRaisePIDController;
-
-    private Rotation2d armSetpoint;
-
-    /** Constructor. Creates a new Arm Subsystem. */
-    public Arm(int leftMotorId, int rightMotorId, int rightEncoderId, boolean areMotorsReversed) {
-
-        leftArmMotor = new CANSparkMax(leftMotorId,MotorType.kBrushless);
-        //leftArmEncoder = new CANcoder(leftEncoderId);
-
-        rightArmMotor = new CANSparkMax(rightMotorId,MotorType.kBrushless);
-        rightArmEncoder = new CANcoder(rightEncoderId);
-
-        armRaisePIDController = new PIDController(
-            ArmConstants.ELEVATION_PID_P,
-            ArmConstants.ELEVATION_PID_I,
-            ArmConstants.ELEVATION_PID_D
-        );
-
-        armPosition = rightArmEncoder.getAbsolutePosition();
-        armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_INTAKE_DEGREES);
-
-        leftArmMotor.setIdleMode(IdleMode.kCoast);
-        rightArmMotor.setIdleMode(IdleMode.kCoast);
-
-        leftArmMotor.setInverted(areMotorsReversed);
-        rightArmMotor.setInverted(!areMotorsReversed);
-
-    }
-
-    // public void setArmAngleDegrees(double desiredDegree) {
-    //     //maximum should ALWAYS be a greater value then minimum
-    //     if (desiredDegree < ArmConstants.MAXIMUM_ARM_DEGREES || ArmConstants.MINIMUM_ARM_DEGREES > desiredDegree) {
-    //         armRotation2d = Rotation2d.fromDegrees(desiredDegree);
-    //     }
-    // }
-
-     public void changeArmAngleDegreesBy(double desiredDegrees) {
-        if (armSetpoint.getDegrees() < ArmConstants.MAXIMUM_ARM_DEGREES || ArmConstants.MINIMUM_ARM_DEGREES > armSetpoint.getDegrees()) {
-            armSetpoint = Rotation2d.fromDegrees(armSetpoint.getDegrees() + desiredDegrees);
-        }
-    }
-
-
-    public void setArmToAmpPosition() {
-        armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_AMP_SHOOTING_DEGREES);
-    }
-
-    public void setArmToSpeakerPosition() {
-        armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES);
-    }
-
-    public void setArmToIntakePosition() {
-        armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_INTAKE_DEGREES);
-    }
-
-    public void setSetpoint(double degree){
-        armSetpoint = Rotation2d.fromDegrees(degree);
-    }
-
-    public boolean isAtDesiredPosition(){
-        return armRaisePIDController.atSetpoint();
-    }
-
-    /**
-     * This method is called periodically by the CommandScheduler, about every 20ms.
-     */
-    @Override
-    public void periodic() {
-        // This method will be called once per scheduler run
-
-
-        double armSpeed = armRaisePIDController.calculate(armPosition.refresh().getValueAsDouble(),armSetpoint.getRotations());
-        
-        SmartDashboard.putNumber("Arm Speed", armSpeed);
-
-        leftArmMotor.set(armSpeed);
-        rightArmMotor.set(armSpeed);
-
-        SmartDashboard.putNumber("Arm Degrees", Rotation2d.fromRotations(rightArmEncoder.getAbsolutePosition().getValueAsDouble()).getDegrees());
-        SmartDashboard.putNumber("Arm Setpoint Degrees", armSetpoint.getDegrees());
-            
-        
-    }
-}
diff --git a/src/main/java/frc/robot/subsystems/arm/ArmInterface.java b/src/main/java/frc/robot/subsystems/arm/ArmInterface.java
new file mode 100644
index 0000000..e49ac11
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/arm/ArmInterface.java
@@ -0,0 +1,21 @@
+package frc.robot.subsystems.arm;
+
+import edu.wpi.first.wpilibj2.command.Subsystem;
+
+/**
+ * This interface represnets the arm. This interface is implemented by the
+ * RealArm (formerly known as Arm) class.
+ */
+public interface ArmInterface extends Subsystem {
+	public void changeArmAngleDegreesBy(double desiredDegrees);
+
+	public void setArmToAmpPosition();
+
+	public void setArmToSpeakerPosition();
+
+	public void setArmToIntakePosition();
+
+	public void setSetpoint(double degree);
+
+	public boolean isAtDesiredPosition();
+}
diff --git a/src/main/java/frc/robot/subsystems/arm/DummyArm.java b/src/main/java/frc/robot/subsystems/arm/DummyArm.java
new file mode 100644
index 0000000..6989509
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/arm/DummyArm.java
@@ -0,0 +1,30 @@
+package frc.robot.subsystems.arm;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * This is a dummy stand in class that represents the arm, in the event that the
+ * robot does not have an arm in order to prevent errors.
+ */
+public class DummyArm extends SubsystemBase implements ArmInterface {
+	public void changeArmAngleDegreesBy(double desiredDegrees) {
+		System.out.println(desiredDegrees);
+	}
+
+	public void setArmToAmpPosition() {
+	}
+
+	public void setArmToSpeakerPosition() {
+	}
+
+	public void setArmToIntakePosition() {
+	}
+
+	public void setSetpoint(double degree) {
+		System.out.println(degree);
+	}
+
+	public boolean isAtDesiredPosition() {
+		return true;
+	}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/arm/RealArm.java b/src/main/java/frc/robot/subsystems/arm/RealArm.java
new file mode 100644
index 0000000..a3f50c8
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/arm/RealArm.java
@@ -0,0 +1,113 @@
+package frc.robot.subsystems.arm;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.ArmConstants;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.hardware.CANcoder;
+
+/** This is the subsystem that represents the arm. */
+public class RealArm extends SubsystemBase implements ArmInterface {
+
+	private final CANSparkMax leftArmMotor;
+	// private final CANcoder leftArmEncoder;
+
+	// leftArmEncoder may be useless, as the right arm equivelent is used for the
+	// position for both.</p>
+
+	private final CANSparkMax rightArmMotor;
+	private final CANcoder rightArmEncoder;
+
+	private final StatusSignal<Double> armPosition;
+
+	private final PIDController armRaisePIDController;
+
+	private Rotation2d armSetpoint;
+
+	/** Constructor. Creates a new Arm Subsystem. */
+	public RealArm(int leftMotorId, int rightMotorId, int rightEncoderId, boolean areMotorsReversed) {
+
+		leftArmMotor = new CANSparkMax(leftMotorId, MotorType.kBrushless);
+		// leftArmEncoder = new CANcoder(leftEncoderId);
+
+		rightArmMotor = new CANSparkMax(rightMotorId, MotorType.kBrushless);
+		rightArmEncoder = new CANcoder(rightEncoderId);
+
+		armRaisePIDController = new PIDController(
+				ArmConstants.ELEVATION_PID_P,
+				ArmConstants.ELEVATION_PID_I,
+				ArmConstants.ELEVATION_PID_D);
+
+		armPosition = rightArmEncoder.getAbsolutePosition();
+		armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_INTAKE_DEGREES);
+
+		leftArmMotor.setIdleMode(IdleMode.kCoast);
+		rightArmMotor.setIdleMode(IdleMode.kCoast);
+
+		leftArmMotor.setInverted(areMotorsReversed);
+		rightArmMotor.setInverted(!areMotorsReversed);
+
+	}
+
+	// public void setArmAngleDegrees(double desiredDegree) {
+	// //maximum should ALWAYS be a greater value then minimum
+	// if (desiredDegree < ArmConstants.MAXIMUM_ARM_DEGREES ||
+	// ArmConstants.MINIMUM_ARM_DEGREES > desiredDegree) {
+	// armRotation2d = Rotation2d.fromDegrees(desiredDegree);
+	// }
+	// }
+
+	public void changeArmAngleDegreesBy(double desiredDegrees) {
+		if (armSetpoint.getDegrees() < ArmConstants.MAXIMUM_ARM_DEGREES
+				|| ArmConstants.MINIMUM_ARM_DEGREES > armSetpoint.getDegrees()) {
+			armSetpoint = Rotation2d.fromDegrees(armSetpoint.getDegrees() + desiredDegrees);
+		}
+	}
+
+	public void setArmToAmpPosition() {
+		armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_AMP_SHOOTING_DEGREES);
+	}
+
+	public void setArmToSpeakerPosition() {
+		armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES);
+	}
+
+	public void setArmToIntakePosition() {
+		armSetpoint = Rotation2d.fromDegrees(ArmConstants.ARM_INTAKE_DEGREES);
+	}
+
+	public void setSetpoint(double degree) {
+		armSetpoint = Rotation2d.fromDegrees(degree);
+	}
+
+	public boolean isAtDesiredPosition() {
+		return armRaisePIDController.atSetpoint();
+	}
+
+	/**
+	 * This method is called periodically by the CommandScheduler, about every 20ms.
+	 */
+	@Override
+	public void periodic() {
+		// This method will be called once per scheduler run
+
+		double armSpeed = armRaisePIDController.calculate(armPosition.refresh().getValueAsDouble(),
+				armSetpoint.getRotations());
+
+		SmartDashboard.putNumber("Arm Speed", armSpeed);
+
+		leftArmMotor.set(armSpeed);
+		rightArmMotor.set(armSpeed);
+
+		SmartDashboard.putNumber("Arm Degrees",
+				Rotation2d.fromRotations(rightArmEncoder.getAbsolutePosition().getValueAsDouble()).getDegrees());
+		SmartDashboard.putNumber("Arm Setpoint Degrees", armSetpoint.getDegrees());
+
+	}
+}

From 13f183873114a45c6e5f7af21ecc6981cd1c78b0 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Mon, 26 Feb 2024 17:12:09 -0800
Subject: [PATCH 88/99] Add aim at tag

---
 .../java/frc/robot/commands/AimAtTag.java     |  4 +-
 src/main/java/frc/robot/commands/FaceTag.java | 37 -------------------
 2 files changed, 2 insertions(+), 39 deletions(-)
 delete mode 100644 src/main/java/frc/robot/commands/FaceTag.java

diff --git a/src/main/java/frc/robot/commands/AimAtTag.java b/src/main/java/frc/robot/commands/AimAtTag.java
index 4a01e0c..dbf785f 100644
--- a/src/main/java/frc/robot/commands/AimAtTag.java
+++ b/src/main/java/frc/robot/commands/AimAtTag.java
@@ -39,7 +39,7 @@ public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID, Chass
 		this.chassisDriveInputs = chassisDriveInputs;
 
         rotatePID = new PIDController(
-            1,
+            2,
             0,
             0);
         rotatePID.enableContinuousInput(-1, 1);
@@ -80,7 +80,7 @@ public void execute() {
 			xSpeed = chassisDriveInputs.getX() * DriverConstants.maxSpeedOptionsTranslation[0];
 			ySpeed = chassisDriveInputs.getY() * DriverConstants.maxSpeedOptionsTranslation[0];
 		}
-		double rotateSpeed = rotatePID.calculate(tagYawRadians) * DriverConstants.maxSpeedOptionsRotation[0]; 
+		double rotateSpeed = rotatePID.calculate(tagYawRadians) * DriverConstants.maxSpeedOptionsRotation[1]; 
 		
 		ChassisSpeeds desiredSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rotateSpeed);
 
diff --git a/src/main/java/frc/robot/commands/FaceTag.java b/src/main/java/frc/robot/commands/FaceTag.java
deleted file mode 100644
index e493410..0000000
--- a/src/main/java/frc/robot/commands/FaceTag.java
+++ /dev/null
@@ -1,37 +0,0 @@
-package frc.robot.commands;
-import java.util.Map;
-
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants.RobotMovementConstants;
-import frc.robot.subsystems.SwerveDrivetrain;
-import frc.robot.subsystems.Vision;
-
-public class FaceTag extends Command {
-    private final Vision vision;
-    private final SwerveDrivetrain drivetrain;
-    private final PIDController rotationPID;
-
-    public FaceTag(Vision vision, SwerveDrivetrain drivetrain, Map<Integer,Double> targetTagPos){
-        this.vision = vision;
-        this.drivetrain = drivetrain;
-        rotationPID = new PIDController(
-            RobotMovementConstants.ROTATION_PID_P,
-            RobotMovementConstants.ROTATION_PID_I,
-            RobotMovementConstants.ROTATION_PID_D);
-        rotationPID.enableContinuousInput(-Math.PI, Math.PI);
-        rotationPID.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
-
-        addRequirements(drivetrain);
-    }
-
-    @Override
-    public void initialize(){
-        drivetrain.toDefaultStates();
-    }
-
-    @Override
-    public void execute(){   
-    }
-
-}

From 6649b3be86063cf213e1ad51c5411905e3d2fce9 Mon Sep 17 00:00:00 2001
From: Aleah H <aleahjham@gmail.com>
Date: Mon, 26 Feb 2024 17:30:09 -0800
Subject: [PATCH 89/99] Fix Xbox Joystick being inversed

---
 src/main/java/frc/robot/RobotContainer.java | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 47100e2..c97d2ed 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -155,8 +155,8 @@ public void setUpDriveController() {
 			final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());
 
 			inputs = new ChassisDriveInputs(
-					xbox::getLeftY, +1,
-					xbox::getLeftX, +1,
+					xbox::getLeftX, -1,
+					xbox::getLeftY, -1,
 					xbox::getRightX, -1,
 					Constants.DriverConstants.DEAD_ZONE);
 

From 4ca5a7ce7c97bb5f158016767fa4f3c7ad414991 Mon Sep 17 00:00:00 2001
From: Linden <50274080+liulinden@users.noreply.github.com>
Date: Mon, 26 Feb 2024 17:35:59 -0800
Subject: [PATCH 90/99] created autoposition file

---
 .../java/frc/robot/commands/AutoPosition.java | 89 +++++++++++++++++++
 1 file changed, 89 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/AutoPosition.java

diff --git a/src/main/java/frc/robot/commands/AutoPosition.java b/src/main/java/frc/robot/commands/AutoPosition.java
new file mode 100644
index 0000000..46ced32
--- /dev/null
+++ b/src/main/java/frc/robot/commands/AutoPosition.java
@@ -0,0 +1,89 @@
+package frc.robot.commands;
+
+import frc.robot.subsystems.ExampleSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+
+// How to make Command: https://compendium.readthedocs.io/en/latest/tasks/commands/commands.html (ignore image instructions, code is out of date, just look at written general instructions)
+// Command based programming: https://docs.wpilib.org/en/stable/docs/software/commandbased/what-is-command-based.html
+// Code documentations https://docs.wpilib.org/en/stable/docs/software/commandbased/commands.html 
+
+/** An example command that uses an example subsystem. */
+public class AutoPosition extends Command {
+	private final ExampleSubsystem subsystem;
+
+	/**
+	 * The constructor creates a new command and is automatically called one time
+	 * when the command is created (with 'new' keyword).
+	 * It should set up the initial state and properties of the object to ensure
+	 * it's ready for use.
+	 * This can take in any arguments you need. It normally uses 1 subsystem (but an
+	 * take multiple when necessary),
+	 * as wells as arguments for what to do, such as a joystick in the drive command
+	 * or a desired position in an auto command.
+	 * Example uses include saving parameters passed to the command, creating and
+	 * configuring objects for the class like PID controllers, and adding subsystem
+	 * requirements
+	 */
+	public AutoPosition(ExampleSubsystem subsystem) {
+		// use "this" to access member variable subsystem rather than local subsystem
+		this.subsystem = subsystem;
+
+		// Use addRequirements() here to declare subsystem dependencies.
+		// This makes sure no other commands try to do stuff with your subsystem while
+		// you are using it.
+		addRequirements(this.subsystem);
+	}
+
+	/**
+	 * initialize() is used to prepare a command for execution and is called once
+	 * when the command is scheduled.
+	 * It should reset the command's state since command objects can be used
+	 * multiple times.
+	 * Example uses include setting motor to constant speed, setting a solenoid to a
+	 * certain state, and resetting variables
+	 */
+	@Override
+	public void initialize() {
+	}
+
+	/**
+	 * execute() is called repeatedly while a command is scheduled, about every
+	 * 20ms.
+	 * It should handle continuous tasks specific to the command, like updating
+	 * motor outputs based on joystick inputs or utilizing control loop results.
+	 * Example uses include adjusting motor speeds for real-time control, processing
+	 * sensor data within a scheduled command, and using the output of a control
+	 * loop.
+	 */
+	@Override
+	public void execute() {
+	}
+
+	/**
+	 * isFinished() finished is called repeatedly while a command is scheduled,
+	 * right after execute.
+	 * It should return true when you want the command to finish. end(false) is
+	 * called directly after isFinished() returns true.
+	 * Example uses include checking if control loop is at set point, and always
+	 * returning false to end after just 1 call to execute.
+	 */
+	@Override
+	public boolean isFinished() {
+		return true;
+	}
+
+	/**
+	 * end(boolean interrupted) is called once when a command ends, regardless of
+	 * whether it finishes normally or is interrupted.
+	 * It should wrap up the command since other commands might use the same
+	 * subsystems.
+	 * Once end runs the command will no longer be in the command scheduler loop.
+	 * It takes in a boolean interrupted which is set to true when the command is
+	 * ended without isFinished() returning true.
+	 * Example uses include setting motor speeds back to zero, and setting a
+	 * solenoid back to a "default" state.
+	 */
+	@Override
+	public void end(boolean interrupted) {
+	}
+}

From e8464cbf01a6ed5324f3983408682f08ddb68dd0 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Mon, 26 Feb 2024 17:47:31 -0800
Subject: [PATCH 91/99] Add Auto Aim To Tag

---
 shuffleboard.json                             | 47 ++++++++++------
 src/main/java/frc/robot/Constants.java        |  2 +
 src/main/java/frc/robot/RobotContainer.java   | 53 +++++++++----------
 src/main/java/frc/robot/commands/Autos.java   | 20 ++++---
 .../robot/commands/ChassisRemoteControl.java  | 38 +++----------
 .../frc/robot/inputs/ChassisDriveInputs.java  | 36 ++++++++++++-
 .../robot/subsystems/SwerveDrivetrain.java    | 16 +++---
 7 files changed, 122 insertions(+), 90 deletions(-)

diff --git a/shuffleboard.json b/shuffleboard.json
index 1c010c3..9c3e998 100644
--- a/shuffleboard.json
+++ b/shuffleboard.json
@@ -39,23 +39,6 @@
               "Colors/Color when false": "#8B0000FF"
             }
           },
-          "1,0": {
-            "size": [
-              2,
-              2
-            ],
-            "content": {
-              "_type": "Gyro",
-              "_source0": "network_table:///SmartDashboard/ Degrees",
-              "_title": "Heading Degrees",
-              "_glyph": 139,
-              "_showGlyph": true,
-              "Visuals/Major tick spacing": 45.0,
-              "Visuals/Starting angle": 180.0,
-              "Visuals/Show tick mark ring": true,
-              "Visuals/Counter clockwise": false
-            }
-          },
           "9,5": {
             "size": [
               1,
@@ -278,6 +261,23 @@
               "Colors/Color when true": "#7CFC00FF",
               "Colors/Color when false": "#8B0000FF"
             }
+          },
+          "1,0": {
+            "size": [
+              2,
+              2
+            ],
+            "content": {
+              "_type": "Gyro",
+              "_source0": "network_table:///SmartDashboard/Heading Degrees",
+              "_title": "Heading Degrees",
+              "_glyph": 148,
+              "_showGlyph": false,
+              "Visuals/Major tick spacing": 45.0,
+              "Visuals/Starting angle": 180.0,
+              "Visuals/Show tick mark ring": true,
+              "Visuals/Counter clockwise": false
+            }
           }
         }
       }
@@ -294,6 +294,19 @@
         "titleType": 0,
         "tiles": {}
       }
+    },
+    {
+      "title": "Tab 3",
+      "autoPopulate": false,
+      "autoPopulatePrefix": "",
+      "widgetPane": {
+        "gridSize": 128.0,
+        "showGrid": true,
+        "hgap": 16.0,
+        "vgap": 16.0,
+        "titleType": 0,
+        "tiles": {}
+      }
     }
   ],
   "windowGeometry": {
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index ed106ab..27cccca 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -54,6 +54,8 @@ public static class DriverConstants {
 
 		public static final double DEAD_ZONE = 0.25;
 
+		public static final int numberOfSpeedOptions = 2;
+
 		// Names of options for displaying
 		public static final String[] maxSpeedOptionsNames = { "Precise", "Normal", "Boost" };
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 126243b..4c507e5 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -6,6 +6,7 @@
 import frc.robot.Constants.ArmConstants;
 import frc.robot.subsystems.Arm;
 import frc.robot.commands.Autos;
+import frc.robot.commands.AimAtTag;
 import frc.robot.commands.ArmRotateTo;
 import frc.robot.commands.ChassisRemoteControl;
 import frc.robot.Constants.VisionConstants;
@@ -13,8 +14,6 @@
 import frc.robot.subsystems.SwerveModule;
 import frc.robot.subsystems.Vision;
 import frc.robot.inputs.ChassisDriveInputs;
-import frc.robot.inputs.OptionButtonInput;
-import frc.robot.inputs.OptionButtonInput.ActivationMode;
 
 import com.kauailabs.navx.frc.AHRS;
 
@@ -70,11 +69,11 @@ public class RobotContainer {
 					-SwerveDrivetrainConstants.MODULE_LOCATION_Y));
 
 	private final AHRS gyro = new AHRS();
-    private final Arm arm = new Arm(
-        ArmConstants.LEFT_MOTOR_ID,
-        ArmConstants.RIGHT_MOTOR_ID,
-        ArmConstants.RIGHT_ENCODER_ID,
-		ArmConstants.ARE_MOTORS_REVERSED);
+    // private final Arm arm = new Arm(
+    //     ArmConstants.LEFT_MOTOR_ID,
+    //     ArmConstants.RIGHT_MOTOR_ID,
+    //     ArmConstants.RIGHT_ENCODER_ID,
+	// 	ArmConstants.ARE_MOTORS_REVERSED);
 
 
 	private final SwerveDrivetrain drivetrain = new SwerveDrivetrain(gyro, swerveModuleFL, swerveModuleFR,
@@ -85,15 +84,14 @@ public class RobotContainer {
 	private final Vision vision = new Vision(VisionConstants.CAMERA_NAME, VisionConstants.CAMERA_POSE);
 
 
-	private final ArmRotateTo armToIntake = new ArmRotateTo(arm, ArmConstants.ARM_INTAKE_DEGREES);
-	private final ArmRotateTo armToAmp = new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES);
-	private final ArmRotateTo armToSpeaker = new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES);
+	// private final ArmRotateTo armToIntake = new ArmRotateTo(arm, ArmConstants.ARM_INTAKE_DEGREES);
+	// private final ArmRotateTo armToAmp = new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES);
+	// private final ArmRotateTo armToSpeaker = new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES);
 
 	/**
 	 * The container for the robot. Contains subsystems, OI devices, and commands.
 	 */
 	public RobotContainer() {
-		autoChooser.setDefaultOption("Testing Auto", Autos.testingAuto(drivetrain));
 		autoChooser.addOption("Follow Tag", Autos.tagFollowAuto(drivetrain, vision, 1));
 		autoChooser.addOption("Rotate by 90", Autos.rotateBy90Auto(drivetrain));
 		autoChooser.addOption("Rotate to 90", Autos.rotateTo90Auto(drivetrain));
@@ -122,7 +120,6 @@ public void setUpDriveController() {
 		drivetrain.removeDefaultCommand();
 
 		ChassisDriveInputs inputs;
-		OptionButtonInput preciseModeButton, boostModeButton, fieldRelativeButton;
 
 		if (genericHIDType.equals(GenericHID.HIDType.kHIDJoystick)) {
 			final CommandJoystick joystick = new CommandJoystick(genericHID.getPort());
@@ -133,33 +130,35 @@ public void setUpDriveController() {
 					joystick::getTwist, -1,
 					Constants.DriverConstants.DEAD_ZONE);
 
-			preciseModeButton = new OptionButtonInput(joystick, 2, ActivationMode.TOGGLE);
-			boostModeButton = new OptionButtonInput(joystick, 1, ActivationMode.HOLD);
-			fieldRelativeButton = new OptionButtonInput(joystick, 3, ActivationMode.TOGGLE);
+			joystick.button(2).onTrue(Commands.run(inputs::speedDown));
+			joystick.button(1).onTrue(Commands.run(inputs::speedUp));
+			joystick.button(3).onTrue(Commands.run(inputs::toggleFieldRelative));
 
 			//This bypasses arm remote control, arm remote control is incompatible with autonomous commands
-			operatorJoystick.button(4).onTrue(armToIntake);
-			operatorJoystick.button(5).onTrue(armToAmp);
-			operatorJoystick.button(6).onTrue(armToSpeaker);
-
-			joystick.button(9).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
-			joystick.button(10).onTrue(Commands.run(drivetrain::toDefaultStates, drivetrain));
+			// operatorJoystick.button(4).onTrue(armToIntake);
+			// operatorJoystick.button(5).onTrue(armToAmp);
+			// operatorJoystick.button(6).onTrue(armToSpeaker);
 		} else {
 			final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());
 
 			inputs = new ChassisDriveInputs(
-					xbox::getLeftY, +1,
-					xbox::getLeftX, +1,
+					xbox::getLeftX, -1,
+					xbox::getLeftY, -1,
 					xbox::getRightX, -1,
 					Constants.DriverConstants.DEAD_ZONE);
 
-			preciseModeButton = new OptionButtonInput(xbox::b, ActivationMode.TOGGLE);
-			boostModeButton = new OptionButtonInput(xbox::leftStick, ActivationMode.HOLD);
-			fieldRelativeButton = new OptionButtonInput(xbox::povUp, ActivationMode.TOGGLE);
+			// xbox.povDown().whileTrue(Commands.run(drivetrain::brakeMode, drivetrain));
+			// xbox.povLeft().whileTrue(Commands.run(drivetrain::toDefaultStates, drivetrain));
+
+			xbox.b().onTrue(Commands.run(inputs::speedDown));
+			xbox.povUp().onTrue(Commands.run(inputs::speedUp));
+			xbox.button(3).onTrue(Commands.run(inputs::toggleFieldRelative));
+
+			xbox.a().whileTrue(new AimAtTag(drivetrain, vision, 1, inputs));
 		}
 
 		drivetrain.setDefaultCommand(
-				new ChassisRemoteControl(drivetrain, inputs, preciseModeButton, boostModeButton, fieldRelativeButton));
+				new ChassisRemoteControl(drivetrain, inputs));
 	}
 
 	/** Use this method to define your trigger->command mappings. */
diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java
index 2aee1e2..446e6b1 100644
--- a/src/main/java/frc/robot/commands/Autos.java
+++ b/src/main/java/frc/robot/commands/Autos.java
@@ -15,11 +15,6 @@
  */
 public final class Autos {
 	/** Example static factory for an autonomous command. */
-	public static Command testingAuto(SwerveDrivetrain drivetrain) {
-		return Commands.sequence(
-				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(90), false));
-	}
-
 	public static Command rotateTo90Auto(SwerveDrivetrain drivetrain) {
 		return Commands.sequence(
 				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(90), true));
@@ -41,7 +36,20 @@ public static Command rotateByNegative90Auto(SwerveDrivetrain drivetrain) {
 	}
 
 	public static Command rotateBy10Auto(SwerveDrivetrain drivetrain) {
-		return Commands.sequence(new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(10), true));
+		return Commands.sequence(
+			new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(10), true));
+	}
+
+	public static Command driveForward(SwerveDrivetrain drivetrain) {
+		return Commands.sequence(
+			new DriveTransform(drivetrain, new Translation2d(1, 0), new Rotation2d())
+		);
+	}
+
+	public static Command driveBackward(SwerveDrivetrain drivetrain) {
+		return Commands.sequence(
+			new DriveTransform(drivetrain, new Translation2d(-1, 0), new Rotation2d())
+		);
 	}
 
 	/** Auto-mode that attempts to follow an april tag. */
diff --git a/src/main/java/frc/robot/commands/ChassisRemoteControl.java b/src/main/java/frc/robot/commands/ChassisRemoteControl.java
index f00fa09..d905083 100644
--- a/src/main/java/frc/robot/commands/ChassisRemoteControl.java
+++ b/src/main/java/frc/robot/commands/ChassisRemoteControl.java
@@ -3,8 +3,6 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants.DriverConstants;
-import frc.robot.inputs.OptionButtonInput;
 import frc.robot.inputs.ChassisDriveInputs;
 import frc.robot.subsystems.SwerveDrivetrain;
 
@@ -15,31 +13,17 @@
 public class ChassisRemoteControl extends Command {
 	protected final SwerveDrivetrain drivetrain;
 
-	private final OptionButtonInput preciseModeButton, boostModeButton, fieldRelativeButton;
-
 	private final ChassisDriveInputs chassisDriveInputs;
 
 	/**
 	 * Creates a new SwerveDriveBaseControl Command.
-	 *
-	 * @param drivetrain       The drivetrain of the robot
-	 * @param driverController The device used to control drivetrain
 	 */
-	public ChassisRemoteControl(SwerveDrivetrain drivetrain, ChassisDriveInputs chassisDriveInputs,
-			OptionButtonInput preciseModeButton, OptionButtonInput boostModeButton, OptionButtonInput fieldRelativeButton) {
+	public ChassisRemoteControl(SwerveDrivetrain drivetrain, ChassisDriveInputs chassisDriveInputs) {
 
 		this.chassisDriveInputs = chassisDriveInputs;
 
-		this.preciseModeButton = preciseModeButton;
-		this.boostModeButton = boostModeButton;
-		this.fieldRelativeButton = fieldRelativeButton;
-
 		this.drivetrain = drivetrain;
 
-		
-
-		
-
 		// Tell the command schedular we are using the drivetrain
 		addRequirements(drivetrain);
 	}
@@ -68,26 +52,18 @@ public void execute() {
 
 		final double speedRotation = chassisDriveInputs.getRotation();
 
-		final boolean isFieldRelative = fieldRelativeButton.getState();
-
-		final int speedLevel = 1
-				- preciseModeButton.getStateAsInt()
-				+ boostModeButton.getStateAsInt();
+		final boolean isFieldRelative = chassisDriveInputs.isFieldRelative();
 
 		final ChassisSpeeds speeds = new ChassisSpeeds(
-				speedX * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-				speedY * DriverConstants.maxSpeedOptionsTranslation[speedLevel],
-				speedRotation * DriverConstants.maxSpeedOptionsRotation[speedLevel]);
-
+				speedX,
+				speedY,
+				speedRotation);
 
-		SmartDashboard.putNumber("SpeedX", speeds.vxMetersPerSecond);
-		SmartDashboard.putNumber("SpeedY", speeds.vyMetersPerSecond);
-		SmartDashboard.putNumber("Spin", speeds.omegaRadiansPerSecond);
 
-		drivetrain.setDesiredState(speeds, isFieldRelative, true);
+		drivetrain.setDesiredState(speeds, chassisDriveInputs.isFieldRelative(), false);
 
 		// Display relevant data on shuffleboard.
-		SmartDashboard.putString("Speed Mode", DriverConstants.maxSpeedOptionsNames[speedLevel]);
+		SmartDashboard.putString("Speed Mode", chassisDriveInputs.getSpeedLevelName());
 		SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
 
 		drivetrain.updateSmartDashboard();
diff --git a/src/main/java/frc/robot/inputs/ChassisDriveInputs.java b/src/main/java/frc/robot/inputs/ChassisDriveInputs.java
index 59b76b8..30c62a7 100644
--- a/src/main/java/frc/robot/inputs/ChassisDriveInputs.java
+++ b/src/main/java/frc/robot/inputs/ChassisDriveInputs.java
@@ -2,6 +2,8 @@
 
 import java.util.function.Supplier;
 
+import frc.robot.Constants.DriverConstants;
+
 /** Class that stores supplies for main controls of ChassisSpeeds */
 public class ChassisDriveInputs {
 
@@ -11,6 +13,9 @@ public class ChassisDriveInputs {
 
 	private final double deadzone;
 
+	private int speedLevel = 0;
+	private boolean isFieldRelative = false;
+
 	/**
 	 * Create a new ChassisDriveInputs
 	 * 
@@ -29,7 +34,8 @@ public ChassisDriveInputs(
 			Supplier<Double> getForward, double forwardCoefficient, 
 			Supplier<Double> getLeft, double leftCoefficient,
 			Supplier<Double> getRotation, double rotationCoefficient,
-			double deadzone) {
+			double deadzone) 
+		{
 
 		this.ySupplier = getForward;
 		this.xSupplier = getLeft;
@@ -57,6 +63,34 @@ public double getRotation() {
 		return applyJoystickDeadzone(rotationSupplier.get(), deadzone) * rotationCoefficient;
 	}
 
+	public void speedUp() {
+		speedLevel = Math.min(speedLevel + 1, DriverConstants.numberOfSpeedOptions);
+	}
+
+	public void speedDown() {
+		speedLevel = Math.max(speedLevel - 1, 0);
+	}
+
+	public void enableFieldRelative() {
+		isFieldRelative = true;
+	}
+
+	public void disableFieldRelative() {
+		isFieldRelative = false;
+	}
+
+	public void toggleFieldRelative() {
+		isFieldRelative = !isFieldRelative;
+	}
+
+	public boolean isFieldRelative() {
+		return isFieldRelative;
+	}
+
+	public String getSpeedLevelName() {
+		return DriverConstants.maxSpeedOptionsNames[speedLevel];
+	}
+
 	/**
 	 * Utility method. Apply a deadzone to the joystick output to account for stick
 	 * drift and small bumps.
diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index 9a04d5e..843364c 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -142,10 +142,6 @@ public void periodic() {
 					yController.calculate(pose.getY(), desiredPose.getY()),
 					rotationController.calculate(pose.getRotation().getRadians(), desiredPose.getRotation().getRadians()));
 			
-			SmartDashboard.putNumber("SpeedX", speeds.vxMetersPerSecond);
-			SmartDashboard.putNumber("SpeedY", speeds.vyMetersPerSecond);
-			SmartDashboard.putNumber("Spin", speeds.omegaRadiansPerSecond);
-
 			// Set those speeds
 			setDesiredState(speeds);
 		}
@@ -272,9 +268,9 @@ public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative) {
 	 * Set speeds of robot.
 	 * 
 	 * <p>
-	 * Vx: the velocity of the robot in the x (forward) direction in meter per second.
-	 * Vy: the velocity of the robot in the y (sideways) direction in meter per second. (Positive values mean the robot is moving to the left).
-	 * Omega: the angular velocity of the robot in radians per second.
+	 * Vx: the velocity of the robot in the x (forward) direction in meter per second. (Positive is forward)
+	 * Vy: the velocity of the robot in the y (sideways) direction in meter per second. (Positive is left).
+	 * Omega: the angular velocity of the robot in radians per second. (Positive is counterclockwise)
 	 * 
 	 * <p>
 	 * If field relative, forward will be directly away from driver, no matter the rotation of the robot.
@@ -292,6 +288,10 @@ public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative) {
 	 */
 	public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative, boolean powerDriveMode) {
 
+		SmartDashboard.putNumber("SpeedX", speeds.vxMetersPerSecond);
+		SmartDashboard.putNumber("SpeedY", speeds.vyMetersPerSecond);
+		SmartDashboard.putNumber("Spin", speeds.omegaRadiansPerSecond);
+
 		if (fieldRelative)
 			speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getHeading());
 
@@ -352,7 +352,7 @@ public Rotation3d getHeading3d() {
 
 	// --- Util ---
 
-	/** Update SmartDashboard with */
+	/** Update SmartDashboard with robot values */
 	public void updateSmartDashboard() {
 		// Position display
 		final Pose2d robotPosition = getPosition();

From d438249c9d1202df584c87b20a5f22523497eaab Mon Sep 17 00:00:00 2001
From: RamiRedShift <155695553+RamiRedShift@users.noreply.github.com>
Date: Mon, 26 Feb 2024 17:52:05 -0800
Subject: [PATCH 92/99] Origin/led sub system (#4)

* LED strips

* Update LED.java

* Update LED.java

* Update LED.java

* Changed class into command

* finished led command

* Format document

* add a return true

* Add @override

---------

Co-authored-by: Aceius E <aceiuselms@seattleacademy.org>
---
 src/main/java/frc/robot/commands/LED.java | 50 +++++++++++++++++++++++
 1 file changed, 50 insertions(+)
 create mode 100644 src/main/java/frc/robot/commands/LED.java

diff --git a/src/main/java/frc/robot/commands/LED.java b/src/main/java/frc/robot/commands/LED.java
new file mode 100644
index 0000000..b7df19f
--- /dev/null
+++ b/src/main/java/frc/robot/commands/LED.java
@@ -0,0 +1,50 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class LED extends Command {
+
+	private AddressableLED m_led;
+	private AddressableLEDBuffer m_ledBuffer;
+	private int h = 0;
+	private int s = 0;
+	private int v = 0;
+
+	private void setColor(int h, int s, int v) {
+		for (int i = 0; i < m_ledBuffer.getLength(); i++) {
+			m_ledBuffer.setHSV(i, h, s, v);
+		}
+	}
+
+	public LED(AddressableLED m_led, AddressableLEDBuffer m_ledBuffer, int h, int s, int v) {
+		this.m_led = m_led;
+		this.m_ledBuffer = m_ledBuffer;
+		this.h = h;
+		this.s = s;
+		this.v = v;
+	}
+
+	@Override
+	public void initialize() {
+		// 9 is the PWM port
+		// m_led = new AddressableLED(9);
+		// Default length of 60 starting with empty output
+		// m_ledBuffer = new AddressableLEDBuffer(60);
+
+		m_led.setLength(m_ledBuffer.getLength());
+
+		// seting the data
+		m_led.setData(m_ledBuffer);
+		m_led.start();
+
+		setColor(h, s, v);
+
+		m_led.setData(m_ledBuffer);
+	}
+
+	@Override
+	public boolean isFinished() { return true; }
+
+}

From 301260ed8b7d6d7f0ade2fb2d3ad15d29ac3e506 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Mon, 26 Feb 2024 21:33:03 -0800
Subject: [PATCH 93/99] Clean up driving and autos and fix speed selector

---
 simgui.json                                   |   3 -
 src/main/java/frc/robot/Constants.java        |   2 +-
 src/main/java/frc/robot/RobotContainer.java   |  33 +++---
 .../frc/robot/commands/ArmRemoteControl.java  |  72 ------------
 src/main/java/frc/robot/commands/Autos.java   |  36 +-----
 .../robot/commands/ChassisRemoteControl.java  |   6 +-
 .../commands/deprecated/ArmRemoteControl.java |  73 ++++++++++++
 .../frc/robot/inputs/ChassisDriveInputs.java  |  33 +++---
 .../frc/robot/inputs/OptionButtonInput.java   | 108 ------------------
 .../robot/subsystems/SwerveDrivetrain.java    |  17 +--
 10 files changed, 122 insertions(+), 261 deletions(-)
 delete mode 100644 src/main/java/frc/robot/commands/ArmRemoteControl.java
 create mode 100644 src/main/java/frc/robot/commands/deprecated/ArmRemoteControl.java
 delete mode 100644 src/main/java/frc/robot/inputs/OptionButtonInput.java

diff --git a/simgui.json b/simgui.json
index 89db2a0..4d48afe 100644
--- a/simgui.json
+++ b/simgui.json
@@ -15,9 +15,6 @@
     "Connections": {
       "open": true
     },
-    "Server": {
-      "open": true
-    },
     "visible": true
   },
   "NetworkTables View": {
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 8126c23..d219809 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -54,7 +54,7 @@ public static class DriverConstants {
 
 		public static final double DEAD_ZONE = 0.25;
 
-		public static final int numberOfSpeedOptions = 2;
+		public static final int NUMBER_OF_SPEED_OPTIONS = 2;
 
 		// Names of options for displaying
 		public static final String[] maxSpeedOptionsNames = { "Precise", "Normal", "Boost" };
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index fb6232f..66670f9 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -98,12 +98,7 @@ public class RobotContainer {
 	 * The container for the robot. Contains subsystems, OI devices, and commands.
 	 */
 	public RobotContainer() {
-		autoChooser.addOption("Follow Tag", Autos.tagFollowAuto(drivetrain, vision, 1));
-		autoChooser.addOption("Rotate by 90", Autos.rotateBy90Auto(drivetrain));
-		autoChooser.addOption("Rotate to 90", Autos.rotateTo90Auto(drivetrain));
-		autoChooser.addOption("Rotate by -90", Autos.rotateByNegative90Auto(drivetrain));
-		autoChooser.addOption("Rotate to -90", Autos.rotateToNegative90Auto(drivetrain));
-		autoChooser.addOption("Rotate by 10", Autos.rotateBy10Auto(drivetrain));
+		autoChooser.addOption("Rotate by 90", Autos.rotateTestAuto(drivetrain, 90, false));
 		SmartDashboard.putData("Auto Chooser", autoChooser);
 
 		configureBindings();
@@ -136,9 +131,13 @@ public void setUpDriveController() {
 					joystick::getTwist, -1,
 					Constants.DriverConstants.DEAD_ZONE);
 
-			joystick.button(2).onTrue(Commands.run(inputs::speedDown));
-			joystick.button(1).onTrue(Commands.run(inputs::speedUp));
-			joystick.button(3).onTrue(Commands.run(inputs::toggleFieldRelative));
+			joystick.button(1).onTrue(Commands.runOnce(inputs::increaseSpeedLevel));
+			// joystick.button(1).onFalse(Commands.runOnce(inputs::decreaseSpeedLevel));
+			
+			joystick.button(2).onTrue(Commands.runOnce(inputs::decreaseSpeedLevel));
+			// joystick.button(2).onFalse(Commands.runOnce(inputs::increaseSpeedLevel));
+			
+			joystick.button(3).onTrue(Commands.runOnce(inputs::toggleFieldRelative));
 
 			// This bypasses arm remote control, arm remote control is incompatible with
 			// autonomous commands
@@ -146,8 +145,8 @@ public void setUpDriveController() {
 			operatorJoystick.button(5).onTrue(armToAmp);
 			operatorJoystick.button(6).onTrue(armToSpeaker);
 
-			joystick.button(9).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
-			joystick.button(10).onTrue(Commands.run(drivetrain::toDefaultStates, drivetrain));
+			// joystick.button(9).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
+			// joystick.button(10).onTrue(Commands.run(drivetrain::toDefaultStates, drivetrain));
 		} else {
 			final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());
 
@@ -158,17 +157,17 @@ public void setUpDriveController() {
 					Constants.DriverConstants.DEAD_ZONE);
 
 			// xbox.povDown().whileTrue(Commands.run(drivetrain::brakeMode, drivetrain));
-			// xbox.povLeft().whileTrue(Commands.run(drivetrain::toDefaultStates, drivetrain));
+			// xbox.povLeft().whileTrue(Commands.run(drivetrain::toDefaultStates,
+			// drivetrain));
 
-			xbox.b().onTrue(Commands.run(inputs::speedDown));
-			xbox.povUp().onTrue(Commands.run(inputs::speedUp));
-			xbox.button(3).onTrue(Commands.run(inputs::toggleFieldRelative));
+			xbox.b().onTrue(Commands.runOnce(inputs::decreaseSpeedLevel));
+			xbox.povUp().onTrue(Commands.runOnce(inputs::increaseSpeedLevel));
+			xbox.button(3).onTrue(Commands.runOnce(inputs::toggleFieldRelative));
 
 			xbox.a().whileTrue(new AimAtTag(drivetrain, vision, 1, inputs));
 		}
 
-		drivetrain.setDefaultCommand(
-				new ChassisRemoteControl(drivetrain, inputs));
+		drivetrain.setDefaultCommand(new ChassisRemoteControl(drivetrain, inputs));
 	}
 
 	/** Use this method to define your trigger->command mappings. */
diff --git a/src/main/java/frc/robot/commands/ArmRemoteControl.java b/src/main/java/frc/robot/commands/ArmRemoteControl.java
deleted file mode 100644
index 5384f00..0000000
--- a/src/main/java/frc/robot/commands/ArmRemoteControl.java
+++ /dev/null
@@ -1,72 +0,0 @@
-package frc.robot.commands;
-
-import frc.robot.Constants.ArmConstants;
-import frc.robot.inputs.OptionButtonInput;
-import frc.robot.subsystems.arm.ArmInterface;
-import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj2.command.Command;
-
-public class ArmRemoteControl extends Command {
-	private final ArmInterface arm;
-
-	private final OptionButtonInput raiseArmButton;
-	private final OptionButtonInput lowerArmButton;
-
-	private final OptionButtonInput speakerPositionButton;
-	private final OptionButtonInput ampPositionButton;
-	private final OptionButtonInput intakePositionButton;
-
-	public ArmRemoteControl(ArmInterface arm, OptionButtonInput raiseArmButton, OptionButtonInput lowerArmButton,
-			OptionButtonInput speakerPositionButton, OptionButtonInput ampPositionButton,
-			OptionButtonInput intakePositionButton) {
-		this.arm = arm;
-
-		this.raiseArmButton = raiseArmButton;
-		this.lowerArmButton = lowerArmButton;
-
-		this.speakerPositionButton = speakerPositionButton;
-		this.ampPositionButton = ampPositionButton;
-		this.intakePositionButton = intakePositionButton;
-
-		addRequirements(arm);
-	}
-
-	@Override
-	public void initialize() {
-	}
-
-	/**
-	 * execute() is called repeatedly while a command is scheduled, about every
-	 * 20ms.
-	 */
-	@Override
-	public void execute() {
-
-		// Arm Motor
-		arm.changeArmAngleDegreesBy(Double.valueOf(raiseArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod
-				* ArmConstants.DEGREES_PER_SECOND);
-		arm.changeArmAngleDegreesBy(Double.valueOf(-lowerArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod
-				* ArmConstants.DEGREES_PER_SECOND);
-
-		// Arm Povs
-		if (speakerPositionButton.getState()) {
-			arm.setArmToSpeakerPosition();
-		}
-		if (ampPositionButton.getState()) {
-			arm.setArmToAmpPosition();
-		}
-		if (intakePositionButton.getState()) {
-			arm.setArmToIntakePosition();
-		}
-
-	}
-
-	@Override
-	public boolean isFinished() {
-		return false;
-	}
-
-	@Override
-	public void end(boolean interrupted) {
-	}
-}
diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java
index 8dbe1ca..6ec5e02 100644
--- a/src/main/java/frc/robot/commands/Autos.java
+++ b/src/main/java/frc/robot/commands/Autos.java
@@ -15,41 +15,9 @@
  */
 public final class Autos {
 	/** Example static factory for an autonomous command. */
-	public static Command rotateTo90Auto(SwerveDrivetrain drivetrain) {
+	public static Command rotateTestAuto(SwerveDrivetrain drivetrain, double degrees, boolean fieldRelative) {
 		return Commands.sequence(
-				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(90), true));
-	}
-
-	public static Command rotateBy90Auto(SwerveDrivetrain drivetrain) {
-		return Commands.sequence(
-				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(90), false));
-	}
-
-	public static Command rotateToNegative90Auto(SwerveDrivetrain drivetrain) {
-		return Commands.sequence(
-				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(-90), true));
-	}
-
-	public static Command rotateByNegative90Auto(SwerveDrivetrain drivetrain) {
-		return Commands.sequence(
-				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(-90), false));
-	}
-
-	public static Command rotateBy10Auto(SwerveDrivetrain drivetrain) {
-		return Commands.sequence(
-			new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(10), true));
-	}
-
-	public static Command driveForward(SwerveDrivetrain drivetrain) {
-		return Commands.sequence(
-			new DriveTransform(drivetrain, new Translation2d(1, 0), new Rotation2d())
-		);
-	}
-
-	public static Command driveBackward(SwerveDrivetrain drivetrain) {
-		return Commands.sequence(
-			new DriveTransform(drivetrain, new Translation2d(-1, 0), new Rotation2d())
-		);
+				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(90), fieldRelative));
 	}
 
 	/** Auto-mode that attempts to follow an april tag. */
diff --git a/src/main/java/frc/robot/commands/ChassisRemoteControl.java b/src/main/java/frc/robot/commands/ChassisRemoteControl.java
index d905083..cbc4f92 100644
--- a/src/main/java/frc/robot/commands/ChassisRemoteControl.java
+++ b/src/main/java/frc/robot/commands/ChassisRemoteControl.java
@@ -60,11 +60,7 @@ public void execute() {
 				speedRotation);
 
 
-		drivetrain.setDesiredState(speeds, chassisDriveInputs.isFieldRelative(), false);
-
-		// Display relevant data on shuffleboard.
-		SmartDashboard.putString("Speed Mode", chassisDriveInputs.getSpeedLevelName());
-		SmartDashboard.putBoolean("Field Relieve", isFieldRelative);
+		drivetrain.setDesiredState(speeds, isFieldRelative, true);
 
 		drivetrain.updateSmartDashboard();
 	}
diff --git a/src/main/java/frc/robot/commands/deprecated/ArmRemoteControl.java b/src/main/java/frc/robot/commands/deprecated/ArmRemoteControl.java
new file mode 100644
index 0000000..4ca2fd4
--- /dev/null
+++ b/src/main/java/frc/robot/commands/deprecated/ArmRemoteControl.java
@@ -0,0 +1,73 @@
+package frc.robot.commands.deprecated;
+// package frc.robot.commands;
+
+// import frc.robot.Constants.ArmConstants;
+// import frc.robot.inputs.OptionButtonInput;
+// import frc.robot.subsystems.arm.ArmInterface;
+// import edu.wpi.first.wpilibj.TimedRobot;
+// import edu.wpi.first.wpilibj2.command.Command;
+
+// public class ArmRemoteControl extends Command {
+// 	private final ArmInterface arm;
+
+// 	private final OptionButtonInput raiseArmButton;
+// 	private final OptionButtonInput lowerArmButton;
+
+// 	private final OptionButtonInput speakerPositionButton;
+// 	private final OptionButtonInput ampPositionButton;
+// 	private final OptionButtonInput intakePositionButton;
+
+// 	public ArmRemoteControl(ArmInterface arm, OptionButtonInput raiseArmButton, OptionButtonInput lowerArmButton,
+// 			OptionButtonInput speakerPositionButton, OptionButtonInput ampPositionButton,
+// 			OptionButtonInput intakePositionButton) {
+// 		this.arm = arm;
+
+// 		this.raiseArmButton = raiseArmButton;
+// 		this.lowerArmButton = lowerArmButton;
+
+// 		this.speakerPositionButton = speakerPositionButton;
+// 		this.ampPositionButton = ampPositionButton;
+// 		this.intakePositionButton = intakePositionButton;
+
+// 		addRequirements(arm);
+// 	}
+
+// 	@Override
+// 	public void initialize() {
+// 	}
+
+// 	/**
+// 	 * execute() is called repeatedly while a command is scheduled, about every
+// 	 * 20ms.
+// 	 */
+// 	@Override
+// 	public void execute() {
+
+// 		// Arm Motor
+// 		arm.changeArmAngleDegreesBy(Double.valueOf(raiseArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod
+// 				* ArmConstants.DEGREES_PER_SECOND);
+// 		arm.changeArmAngleDegreesBy(Double.valueOf(-lowerArmButton.getStateAsInt()) * TimedRobot.kDefaultPeriod
+// 				* ArmConstants.DEGREES_PER_SECOND);
+
+// 		// Arm Povs
+// 		if (speakerPositionButton.getState()) {
+// 			arm.setArmToSpeakerPosition();
+// 		}
+// 		if (ampPositionButton.getState()) {
+// 			arm.setArmToAmpPosition();
+// 		}
+// 		if (intakePositionButton.getState()) {
+// 			arm.setArmToIntakePosition();
+// 		}
+
+// 	}
+
+// 	@Override
+// 	public boolean isFinished() {
+// 		return false;
+// 	}
+
+// 	@Override
+// 	public void end(boolean interrupted) {
+// 	}
+// }
diff --git a/src/main/java/frc/robot/inputs/ChassisDriveInputs.java b/src/main/java/frc/robot/inputs/ChassisDriveInputs.java
index 30c62a7..d5f0d4b 100644
--- a/src/main/java/frc/robot/inputs/ChassisDriveInputs.java
+++ b/src/main/java/frc/robot/inputs/ChassisDriveInputs.java
@@ -2,6 +2,7 @@
 
 import java.util.function.Supplier;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.DriverConstants;
 
 /** Class that stores supplies for main controls of ChassisSpeeds */
@@ -13,29 +14,31 @@ public class ChassisDriveInputs {
 
 	private final double deadzone;
 
-	private int speedLevel = 0;
+	private int speedLevel = DriverConstants.NUMBER_OF_SPEED_OPTIONS / 2;
 	private boolean isFieldRelative = false;
 
 	/**
 	 * Create a new ChassisDriveInputs
 	 * 
-	 * @param getForward Get the value mapped to X, -1 full backward to +1 full forward
-	 * @param forwardCoefficient Coefficient that forward (X) multiplied by
+	 * @param getForward          Get the value mapped to X, -1 full backward to +1
+	 *                            full forward
+	 * @param forwardCoefficient  Coefficient that forward (X) multiplied by
 	 * 
-	 * @param getLeft Get the value mapped to Y, -1 full right to +1 full left
-	 * @param leftCoefficient Coefficient that forward left (Y) are multiplied by
+	 * @param getLeft             Get the value mapped to Y, -1 full right to +1
+	 *                            full left
+	 * @param leftCoefficient     Coefficient that forward left (Y) are multiplied
+	 *                            by
 	 * 
-	 * @param getRotation Get the value mapped to rotation, -1 full clock
+	 * @param getRotation         Get the value mapped to rotation, -1 full clock
 	 * @param rotationCoefficient Coefficient that rotation is multiplied by
 	 * 
-	 * @param deadzone Deadzone for all axises
+	 * @param deadzone            Deadzone for all axises
 	 */
 	public ChassisDriveInputs(
-			Supplier<Double> getForward, double forwardCoefficient, 
+			Supplier<Double> getForward, double forwardCoefficient,
 			Supplier<Double> getLeft, double leftCoefficient,
 			Supplier<Double> getRotation, double rotationCoefficient,
-			double deadzone) 
-		{
+			double deadzone) {
 
 		this.ySupplier = getForward;
 		this.xSupplier = getLeft;
@@ -46,6 +49,8 @@ public ChassisDriveInputs(
 		this.rotationCoefficient = rotationCoefficient;
 
 		this.deadzone = deadzone;
+
+		SmartDashboard.putString("Speed Mode", getSpeedLevelName());
 	}
 
 	/** @return Joystick X with the deadzone applied */
@@ -63,12 +68,14 @@ public double getRotation() {
 		return applyJoystickDeadzone(rotationSupplier.get(), deadzone) * rotationCoefficient;
 	}
 
-	public void speedUp() {
-		speedLevel = Math.min(speedLevel + 1, DriverConstants.numberOfSpeedOptions);
+	public void increaseSpeedLevel() {
+		speedLevel = Math.min(speedLevel + 1, DriverConstants.NUMBER_OF_SPEED_OPTIONS);
+		SmartDashboard.putString("Speed Mode", getSpeedLevelName());
 	}
 
-	public void speedDown() {
+	public void decreaseSpeedLevel() {
 		speedLevel = Math.max(speedLevel - 1, 0);
+		SmartDashboard.putString("Speed Mode", getSpeedLevelName());
 	}
 
 	public void enableFieldRelative() {
diff --git a/src/main/java/frc/robot/inputs/OptionButtonInput.java b/src/main/java/frc/robot/inputs/OptionButtonInput.java
deleted file mode 100644
index 73d2e5e..0000000
--- a/src/main/java/frc/robot/inputs/OptionButtonInput.java
+++ /dev/null
@@ -1,108 +0,0 @@
-package frc.robot.inputs;
-
-import java.util.function.Supplier;
-
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.button.CommandGenericHID;
-import edu.wpi.first.wpilibj2.command.button.Trigger;
-
-/**
- * Button with different types of control
- */
-public class OptionButtonInput {
-
-	private final Supplier<Trigger> button;
-
-	private final ActivationMode mode;
-
-	private boolean isToggled;
-
-	/**
-	 * Different modes which a OptionButton can be in
-	 */
-	public static enum ActivationMode {
-		/**
-		 * Like toggle, but stays on until manually toggled off with .toggleOff().
-		 * Probably just use .onTrue() instead
-		 */
-		TAP,
-
-		/** standard button behavior, on when pressed, off when released */
-		HOLD,
-
-		/** makes button into on/off toggle */
-		TOGGLE
-	}
-
-	/**
-	 * Create Option button.
-	 * 
-	 * @param controller    device that button is on
-	 * @param button        button number
-	 * @param mode          whether we want button to act as toggle or hold
-	 *                      button
-	 */
-	public OptionButtonInput(CommandGenericHID controller, int button, ActivationMode mode) {
-		this(() -> controller.button(button), mode);
-	}
-
-	/**
-	 * Create Option button.
-	 *
-	 * @param button        button number
-	 * @param mode          whether we want button to act as toggle or hold button
-	 */
-	public OptionButtonInput(Supplier<Trigger> button, ActivationMode mode) {
-		this.button = button;
-		this.mode = mode;
-
-		if (mode == ActivationMode.TOGGLE) {
-			button.get().onTrue(new InstantCommand(this::toggle));
-		} else if (mode == ActivationMode.TAP) {
-			button.get().onTrue(new InstantCommand(this::toggleOn));
-		}
-	}
-
-	/** Manually toggle state, has no effect if it is hold button */
-	public void toggle() {
-		isToggled = !isToggled;
-	}
-
-	/** Manually toggle state on, has no effect if it is hold button */
-	public void toggleOn() {
-		isToggled = true;
-	}
-
-	/** Manually toggle state off, has no effect if it is hold button */
-	public void toggleOff() {
-		isToggled = false;
-	}
-
-	/**
-	 * Get the state of the button
-	 * 
-	 * <li>for TAP buttons this returns true if the button has been tapped
-	 * 
-	 * <li>for HOLD buttons this returns true if the button is currently being held down
-	 * 
-	 * <li> for TOGGLE buttons this returns true if the button has been tapped an odd number of times
-	 * 
-	 * @return boolean with true being the button is active
-	 */
-	public boolean getState() {
-		switch (mode) {
-			case HOLD: return button.get().getAsBoolean();
-			case TAP: case TOGGLE: return isToggled;
-			default: return false;
-		}
-	}
-
-	/**
-	 * Get state, but return it as a int instead of a boolean
-	 * 
-	 * @return 1 if true, 0 is false
-	 */
-	public int getStateAsInt() {
-		return getState() ? 1 : 0;
-	}
-}
diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index 843364c..4c1befe 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -291,6 +291,7 @@ public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative, boolean
 		SmartDashboard.putNumber("SpeedX", speeds.vxMetersPerSecond);
 		SmartDashboard.putNumber("SpeedY", speeds.vyMetersPerSecond);
 		SmartDashboard.putNumber("Spin", speeds.omegaRadiansPerSecond);
+		SmartDashboard.putBoolean("Field Relieve", fieldRelative);
 
 		if (fieldRelative)
 			speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getHeading());
@@ -370,14 +371,14 @@ public void updateSmartDashboard() {
 		SmartDashboard.putNumber("Heading Degrees", getHeading().getDegrees());
 
 
-		final boolean hasTargetPose = desiredPose != null;
-		final Pose2d targetPose = hasTargetPose ? desiredPose : new Pose2d();
-		SmartDashboard.putBoolean("tPoseActive", hasTargetPose);
-		if (hasTargetPose) {
-			SmartDashboard.putNumber("tPoseX", targetPose.getX());
-			SmartDashboard.putNumber("tPoseY", targetPose.getY());
-			SmartDashboard.putNumber("tPoseDegrees", targetPose.getRotation().getDegrees());
-		}
+		// final boolean hasTargetPose = desiredPose != null;
+		// final Pose2d targetPose = hasTargetPose ? desiredPose : new Pose2d();
+		// SmartDashboard.putBoolean("tPoseActive", hasTargetPose);
+		// if (hasTargetPose) {
+		// 	SmartDashboard.putNumber("tPoseX", targetPose.getX());
+		// 	SmartDashboard.putNumber("tPoseY", targetPose.getY());
+		// 	SmartDashboard.putNumber("tPoseDegrees", targetPose.getRotation().getDegrees());
+		// }
 	}
 
 	/**

From 44d9f6bbb0def3c49b569555f61ba10ab0e47053 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Tue, 27 Feb 2024 21:04:45 -0800
Subject: [PATCH 94/99] Finish changes to ChassisDriveInputs

---
 src/main/java/frc/robot/commands/AimAtTag.java         | 7 +++----
 src/main/java/frc/robot/inputs/ChassisDriveInputs.java | 6 +++---
 2 files changed, 6 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/commands/AimAtTag.java b/src/main/java/frc/robot/commands/AimAtTag.java
index dbf785f..c348bc2 100644
--- a/src/main/java/frc/robot/commands/AimAtTag.java
+++ b/src/main/java/frc/robot/commands/AimAtTag.java
@@ -1,6 +1,5 @@
 package frc.robot.commands;
 
-import frc.robot.Constants.DriverConstants;
 import frc.robot.Constants.RobotMovementConstants;
 import frc.robot.inputs.ChassisDriveInputs;
 import frc.robot.subsystems.SwerveDrivetrain;
@@ -77,10 +76,10 @@ public void execute() {
 		double xSpeed = 0;
 		double ySpeed = 0;
 		if (chassisDriveInputs != null) {
-			xSpeed = chassisDriveInputs.getX() * DriverConstants.maxSpeedOptionsTranslation[0];
-			ySpeed = chassisDriveInputs.getY() * DriverConstants.maxSpeedOptionsTranslation[0];
+			xSpeed = chassisDriveInputs.getX();
+			ySpeed = chassisDriveInputs.getY();
 		}
-		double rotateSpeed = rotatePID.calculate(tagYawRadians) * DriverConstants.maxSpeedOptionsRotation[1]; 
+		double rotateSpeed = rotatePID.calculate(tagYawRadians); 
 		
 		ChassisSpeeds desiredSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rotateSpeed);
 
diff --git a/src/main/java/frc/robot/inputs/ChassisDriveInputs.java b/src/main/java/frc/robot/inputs/ChassisDriveInputs.java
index d5f0d4b..d3cbf8a 100644
--- a/src/main/java/frc/robot/inputs/ChassisDriveInputs.java
+++ b/src/main/java/frc/robot/inputs/ChassisDriveInputs.java
@@ -55,17 +55,17 @@ public ChassisDriveInputs(
 
 	/** @return Joystick X with the deadzone applied */
 	public double getX() {
-		return applyJoystickDeadzone(xSupplier.get(), deadzone) * xCoefficient;
+		return applyJoystickDeadzone(xSupplier.get(), deadzone) * xCoefficient * DriverConstants.maxSpeedOptionsTranslation[speedLevel];
 	}
 
 	/** @return Joystick Y with the deadzone applied */
 	public double getY() {
-		return applyJoystickDeadzone(ySupplier.get(), deadzone) * yCoefficient;
+		return applyJoystickDeadzone(ySupplier.get(), deadzone) * yCoefficient * DriverConstants.maxSpeedOptionsTranslation[speedLevel];
 	}
 
 	/** @return Joystick rotation with deadzone applied */
 	public double getRotation() {
-		return applyJoystickDeadzone(rotationSupplier.get(), deadzone) * rotationCoefficient;
+		return applyJoystickDeadzone(rotationSupplier.get(), deadzone) * rotationCoefficient * DriverConstants.maxSpeedOptionsRotation[speedLevel];
 	}
 
 	public void increaseSpeedLevel() {

From 601fe95525a096157bd36befa3c2732b026a687d Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Wed, 28 Feb 2024 10:28:22 -0800
Subject: [PATCH 95/99] Fix units issues

---
 src/main/java/frc/robot/commands/AimAtTag.java  | 2 +-
 src/main/java/frc/robot/commands/FollowTag.java | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/main/java/frc/robot/commands/AimAtTag.java b/src/main/java/frc/robot/commands/AimAtTag.java
index c348bc2..30f00de 100644
--- a/src/main/java/frc/robot/commands/AimAtTag.java
+++ b/src/main/java/frc/robot/commands/AimAtTag.java
@@ -42,7 +42,7 @@ public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID, Chass
             0,
             0);
         rotatePID.enableContinuousInput(-1, 1);
-        rotatePID.setTolerance(Units.radiansToDegrees(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS));
+        rotatePID.setTolerance(Units.radiansToRotations(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS));
 		rotatePID.setSetpoint(0);
 
 		addRequirements(drivetrain);
diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java
index 9c7d2c1..65a8b68 100644
--- a/src/main/java/frc/robot/commands/FollowTag.java
+++ b/src/main/java/frc/robot/commands/FollowTag.java
@@ -86,7 +86,7 @@ public void execute() {
 			final Transform2d tagPosition = new Transform2d(
 					tagPosition3d.getZ(),
 					tagPosition3d.getX(),
-					Rotation2d.fromRadians(tag.getYaw()));
+					Rotation2d.fromDegrees(-tag.getYaw()));
 
 			final Transform2d driveTransform = tagPosition.plus(targetDistance.inverse());
 

From 967796b099272e948d6a9d87e78cb7ae6376429b Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Wed, 28 Feb 2024 17:09:28 -0800
Subject: [PATCH 96/99] Update vision subsystem

---
 .../java/frc/robot/commands/AimAtTag.java     | 46 +++++++++--------
 .../java/frc/robot/commands/FollowTag.java    | 30 ++++++------
 .../java/frc/robot/subsystems/Vision.java     | 49 +++++++++----------
 3 files changed, 64 insertions(+), 61 deletions(-)

diff --git a/src/main/java/frc/robot/commands/AimAtTag.java b/src/main/java/frc/robot/commands/AimAtTag.java
index 30f00de..8e1b842 100644
--- a/src/main/java/frc/robot/commands/AimAtTag.java
+++ b/src/main/java/frc/robot/commands/AimAtTag.java
@@ -5,9 +5,9 @@
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.Vision;
 
-import org.photonvision.targeting.PhotonTrackedTarget;
-
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -18,17 +18,19 @@ public class AimAtTag extends Command {
 	private final ChassisDriveInputs chassisDriveInputs;
 
 	private final Vision vision;
-	private final Integer tagID;  // Integer as opposed to int so it can be null for best tag
+	private final Integer tagID; // Integer as opposed to int so it can be null for best tag
 
 	private final PIDController rotatePID;
 
 	/**
-	 * Create a new AimAtTag command. Tries to constants aim at a tag while still allowing driver to control robot.
+	 * Create a new AimAtTag command. Tries to constants aim at a tag while still
+	 * allowing driver to control robot.
 	 * 
 	 * @param drivetrain          the drivetrain of the robot
 	 * @param vision              the vision subsystem of the robot
-	 * @param tagID               the numerical ID of the the tag to turn to, null for best tag
-	 * @param chassisDriveControl collection of inputs for driving 
+	 * @param tagID               the numerical ID of the the tag to turn to, null
+	 *                            for best tag
+	 * @param chassisDriveControl collection of inputs for driving
 	 */
 	public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID, ChassisDriveInputs chassisDriveInputs) {
 		this.drivetrain = drivetrain;
@@ -37,12 +39,12 @@ public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID, Chass
 		this.tagID = tagID;
 		this.chassisDriveInputs = chassisDriveInputs;
 
-        rotatePID = new PIDController(
-            2,
-            0,
-            0);
-        rotatePID.enableContinuousInput(-1, 1);
-        rotatePID.setTolerance(Units.radiansToRotations(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS));
+		rotatePID = new PIDController(
+				2,
+				0,
+				0);
+		rotatePID.enableContinuousInput(-1, 1);
+		rotatePID.setTolerance(Units.radiansToDegrees(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS));
 		rotatePID.setSetpoint(0);
 
 		addRequirements(drivetrain);
@@ -51,9 +53,10 @@ public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID, Chass
 	/**
 	 * Create a new AimAtTag command. Tries to aim at a tag.
 	 * 
-	 * @param drivetrain          the drivetrain of the robot
-	 * @param vision              the vision subsystem of the robot
-	 * @param tagID               the numerical ID of the the tag to turn to, null for best tag
+	 * @param drivetrain the drivetrain of the robot
+	 * @param vision     the vision subsystem of the robot
+	 * @param tagID      the numerical ID of the the tag to turn to, null for best
+	 *                   tag
 	 */
 	public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID) {
 		this(drivetrain, vision, tagID, null);
@@ -66,11 +69,12 @@ public void initialize() {
 
 	@Override
 	public void execute() {
-		PhotonTrackedTarget tag = (tagID == null) ? vision.getTag() : vision.getTag(tagID);
+		Transform3d distToTag = (tagID == null) ? vision.getDistToTag() : vision.getDistToTag(tagID);
 
 		double tagYawRadians = 0;
-		if (tag != null) {
-			tagYawRadians = Units.degreesToRadians(tag.getYaw());
+		if (distToTag != null) {
+			Rotation2d angleToTag = new Rotation2d(distToTag.getX(), distToTag.getY());
+			tagYawRadians = angleToTag.getRadians();
 		}
 
 		double xSpeed = 0;
@@ -79,8 +83,8 @@ public void execute() {
 			xSpeed = chassisDriveInputs.getX();
 			ySpeed = chassisDriveInputs.getY();
 		}
-		double rotateSpeed = rotatePID.calculate(tagYawRadians); 
-		
+		double rotateSpeed = rotatePID.calculate(tagYawRadians);
+
 		ChassisSpeeds desiredSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rotateSpeed);
 
 		drivetrain.setDesiredState(desiredSpeeds, false, true);
@@ -97,4 +101,4 @@ public boolean isFinished() {
 	public void end(boolean interrupted) {
 		drivetrain.stop();
 	}
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/FollowTag.java b/src/main/java/frc/robot/commands/FollowTag.java
index 65a8b68..80e4ef5 100644
--- a/src/main/java/frc/robot/commands/FollowTag.java
+++ b/src/main/java/frc/robot/commands/FollowTag.java
@@ -3,8 +3,6 @@
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.Vision;
 
-import org.photonvision.targeting.PhotonTrackedTarget;
-
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Transform3d;
@@ -17,10 +15,10 @@ public class FollowTag extends Command {
 	private final SwerveDrivetrain drivetrain;
 
 	private final Vision vision;
-	private final Integer tagID;  // Integer as opposed to int so it can be null for best tag
+	private final Integer tagID; // Integer as opposed to int so it can be null for best tag
 	private final Transform2d targetDistance;
 
-	private final Double loseTagAfterSeconds;  // Double as opposed to double so it can be null for never lose mode.
+	private final Double loseTagAfterSeconds; // Double as opposed to double so it can be null for never lose mode.
 	private double secondsSinceTagLastSeen;
 
 	/**
@@ -29,9 +27,11 @@ public class FollowTag extends Command {
 	 * 
 	 * @param drivetrain          the drivetrain of the robot
 	 * @param vision              the vision subsystem of the robot
-	 * @param tagID               the numerical ID of the the tag to follow, null for best tag
+	 * @param tagID               the numerical ID of the the tag to follow, null
+	 *                            for best tag
 	 * @param targetDistanceToTag the target distance away from the tag to be
-	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover tag, set to null to never finish
+	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover
+	 *                            tag, set to null to never finish
 	 */
 	public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Transform2d targetDistanceToTag, Integer tagID,
 			Double loseTagAfterSeconds) {
@@ -52,7 +52,8 @@ public FollowTag(SwerveDrivetrain drivetrain, Vision vision, Transform2d targetD
 	 * 
 	 * @param drivetrain          the drivetrain of the robot
 	 * @param vision              the vision subsystem of the robot
-	 * @param tagID               the numerical ID of the the tag to follow, null for whatever best is
+	 * @param tagID               the numerical ID of the the tag to follow, null
+	 *                            for whatever best is
 	 * @param targetDistanceToTag the target distance away from the tag to be
 	 * @param loseTagAfterSeconds how long to wait before giving up on rediscover
 	 *                            tag, set to null to never finish
@@ -70,23 +71,22 @@ public void initialize() {
 
 	@Override
 	public void execute() {
-		
-		final PhotonTrackedTarget tag = (tagID == null) ? vision.getTag() : vision.getTag(tagID);
 
-		if (tag == null) {
+		final Transform3d distToTag = (tagID == null) ? vision.getDistToTag() : vision.getDistToTag(tagID);
+
+		if (distToTag == null) {
 			secondsSinceTagLastSeen += TimedRobot.kDefaultPeriod;
 			drivetrain.stop();
 		} else {
-			final Transform3d tagPosition3d = vision.getDistanceToTarget(tag);
 
 			secondsSinceTagLastSeen = 0;
 
 			// https://docs.photonvision.org/en/latest/docs/apriltag-pipelines/coordinate-systems.html
 			// https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/geometry/Rotation3d.html#getZ()
 			final Transform2d tagPosition = new Transform2d(
-					tagPosition3d.getZ(),
-					tagPosition3d.getX(),
-					Rotation2d.fromDegrees(-tag.getYaw()));
+					distToTag.getZ(),
+					distToTag.getX(),
+					Rotation2d.fromRadians(distToTag.getRotation().getZ()));
 
 			final Transform2d driveTransform = tagPosition.plus(targetDistance.inverse());
 
@@ -106,4 +106,4 @@ public void end(boolean interrupted) {
 		drivetrain.clearDesiredPosition();
 		drivetrain.stop();
 	}
-}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java
index c685379..58ffec8 100644
--- a/src/main/java/frc/robot/subsystems/Vision.java
+++ b/src/main/java/frc/robot/subsystems/Vision.java
@@ -18,52 +18,51 @@ public class Vision extends SubsystemBase {
 	private final static boolean DEBUG_INFO = false;
 
 	final PhotonCamera camera;
-	final Transform3d cameraToFrontCenter;
+	final Transform3d robotToCamera;
+	final Transform3d cameraToRobot;
 
 	/**
 	 * Create new PhotonCamera subsystem
 	 * 
-	 * @param cameraName          name of PhotonCamera
-	 * @param cameraToFrontCenter distance from the camera to the front center point
-	 *                            of the robot
+	 * @param cameraName    name of PhotonCamera
+	 * @param robotToCamera distance from the camera to the center of the robot
 	 */
-	public Vision(String cameraName, Transform3d cameraToFrontCenter) {
+	public Vision(String cameraName, Transform3d robotToCamera) {
 		camera = new PhotonCamera(cameraName);
-		this.cameraToFrontCenter = cameraToFrontCenter;
+		this.robotToCamera = robotToCamera;
+		cameraToRobot = robotToCamera.inverse();
 	}
 
 	/**
-	 * Get best april tag target
+	 * Get distance to the distance to the best tag found by the camera
+	 * 
+	 * @return The position of the tag (translation and rotation) based on the
+	 *         center of the robot. Returns null if no tag found.
 	 * 
-	 * @return Object of best target
 	 */
-	public PhotonTrackedTarget getTag() {
-		return camera
-				.getLatestResult()
-				.getBestTarget();
+	public Transform3d getDistToTag() {
+		PhotonTrackedTarget target = camera.getLatestResult().getBestTarget();
+		if (target == null) {
+			return null;
+		}
+		return target.getBestCameraToTarget().plus(cameraToRobot);
 	}
 
 	/**
-	 * Gives Transform3d from robot center to the desired target
+	 * Get distance to the desired tag
 	 * 
-	 * @param tagID The fiducial ID of the desired April Tag
-	 * @return returns first tag with matching ID, null if None are found
+	 * @param tagID the fiducial ID of the desired tag
+	 * @return the position of the tag (translation and rotation) based on the
+	 *         center of the robot. Returns null if no tag found
 	 */
-	public PhotonTrackedTarget getTag(int tagID) {
+	public Transform3d getDistToTag(int tagID) {
 		for (PhotonTrackedTarget target : camera.getLatestResult().getTargets()) {
 			if (target.getFiducialId() == tagID)
-				return target;
+				return target.getBestCameraToTarget().plus(cameraToRobot);
 		}
 		return null;
 	}
 
-	public Transform3d getDistanceToTarget(PhotonTrackedTarget tag) {
-		return tag
-				.getBestCameraToTarget()
-				.plus(cameraToFrontCenter);
-	}
-
-
 	@Override
 	public void periodic() {
 		if (!DEBUG_INFO)
@@ -96,4 +95,4 @@ public void periodic() {
 		SmartDashboard.putNumber("Tag Pose Pitch", tagPoseRotation.getY());
 		SmartDashboard.putNumber("Tag Pose Roll", tagPoseRotation.getX());
 	}
-}
+}
\ No newline at end of file

From c2bc797de36c6d9670d181f2a53eabf20e1bf396 Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Wed, 28 Feb 2024 17:19:27 -0800
Subject: [PATCH 97/99] fix minor bug with aim at tag after update

---
 src/main/java/frc/robot/commands/AimAtTag.java | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/java/frc/robot/commands/AimAtTag.java b/src/main/java/frc/robot/commands/AimAtTag.java
index 8e1b842..ca8c9bc 100644
--- a/src/main/java/frc/robot/commands/AimAtTag.java
+++ b/src/main/java/frc/robot/commands/AimAtTag.java
@@ -85,7 +85,7 @@ public void execute() {
 		}
 		double rotateSpeed = rotatePID.calculate(tagYawRadians);
 
-		ChassisSpeeds desiredSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rotateSpeed);
+		ChassisSpeeds desiredSpeeds = new ChassisSpeeds(xSpeed, ySpeed, -rotateSpeed);
 
 		drivetrain.setDesiredState(desiredSpeeds, false, true);
 

From 3ffa7e8f792cf107a28e8ede328bd7a2932f7390 Mon Sep 17 00:00:00 2001
From: Aceius E <144858100+AceiusRedshift@users.noreply.github.com>
Date: Wed, 28 Feb 2024 18:28:44 -0800
Subject: [PATCH 98/99] Create subsystem & test command for LED strip (#5)

* LED Subsys

* Add lightstrip color command & auto to test it

* Clarify meaning of number 60
---
 src/main/java/frc/robot/Constants.java        |  5 +++
 src/main/java/frc/robot/RobotContainer.java   | 14 +++++--
 .../robot/commands/SetLightstripColor.java    | 42 +++++++++++++++++++
 .../robot/commands/{ => deprecated}/LED.java  |  6 ++-
 .../java/frc/robot/subsystems/LightStrip.java | 34 +++++++++++++++
 5 files changed, 96 insertions(+), 5 deletions(-)
 create mode 100644 src/main/java/frc/robot/commands/SetLightstripColor.java
 rename src/main/java/frc/robot/commands/{ => deprecated}/LED.java (92%)
 create mode 100644 src/main/java/frc/robot/subsystems/LightStrip.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index d219809..6ce196c 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -256,4 +256,9 @@ public static class VisionConstants {
 		public static final Transform3d CAMERA_POSE = new Transform3d(0.5, 0, 0.25, new Rotation3d());
 		public static final String CAMERA_NAME = "Arducam_OV9281_USB_Camera";
 	}
+
+	public static class LightConstants {
+		public static final int LED_CONTROLLER_PWM_SLOT = 0;
+		public static final int LED_QUANTITY = 60;
+	}
 }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 66670f9..81f73d1 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -1,6 +1,7 @@
 package frc.robot;
 
 import frc.robot.Constants.DriverConstants;
+import frc.robot.Constants.LightConstants;
 import frc.robot.Constants.SwerveDrivetrainConstants;
 import frc.robot.Constants.SwerveModuleConstants;
 import frc.robot.Constants.ArmConstants;
@@ -8,7 +9,9 @@
 import frc.robot.commands.AimAtTag;
 import frc.robot.commands.ArmRotateTo;
 import frc.robot.commands.ChassisRemoteControl;
+import frc.robot.commands.SetLightstripColor;
 import frc.robot.Constants.VisionConstants;
+import frc.robot.subsystems.LightStrip;
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.SwerveModule;
 import frc.robot.subsystems.Vision;
@@ -19,6 +22,7 @@
 
 import com.kauailabs.navx.frc.AHRS;
 
+import edu.wpi.first.wpilibj.AddressableLED;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.GenericHID.HIDType;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
@@ -94,11 +98,14 @@ public class RobotContainer {
 	private final ArmRotateTo armToAmp = new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES);
 	private final ArmRotateTo armToSpeaker = new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES);
 
+	private final LightStrip lightStrip = new LightStrip(new AddressableLED(LightConstants.LED_CONTROLLER_PWM_SLOT));
+
 	/**
 	 * The container for the robot. Contains subsystems, OI devices, and commands.
 	 */
 	public RobotContainer() {
 		autoChooser.addOption("Rotate by 90", Autos.rotateTestAuto(drivetrain, 90, false));
+		autoChooser.addOption("Make LEDs blue", new SetLightstripColor(lightStrip, 0, 0, 200));
 		SmartDashboard.putData("Auto Chooser", autoChooser);
 
 		configureBindings();
@@ -133,10 +140,10 @@ public void setUpDriveController() {
 
 			joystick.button(1).onTrue(Commands.runOnce(inputs::increaseSpeedLevel));
 			// joystick.button(1).onFalse(Commands.runOnce(inputs::decreaseSpeedLevel));
-			
+
 			joystick.button(2).onTrue(Commands.runOnce(inputs::decreaseSpeedLevel));
 			// joystick.button(2).onFalse(Commands.runOnce(inputs::increaseSpeedLevel));
-			
+
 			joystick.button(3).onTrue(Commands.runOnce(inputs::toggleFieldRelative));
 
 			// This bypasses arm remote control, arm remote control is incompatible with
@@ -146,7 +153,8 @@ public void setUpDriveController() {
 			operatorJoystick.button(6).onTrue(armToSpeaker);
 
 			// joystick.button(9).onTrue(Commands.run(drivetrain::brakeMode, drivetrain));
-			// joystick.button(10).onTrue(Commands.run(drivetrain::toDefaultStates, drivetrain));
+			// joystick.button(10).onTrue(Commands.run(drivetrain::toDefaultStates,
+			// drivetrain));
 		} else {
 			final CommandXboxController xbox = new CommandXboxController(genericHID.getPort());
 
diff --git a/src/main/java/frc/robot/commands/SetLightstripColor.java b/src/main/java/frc/robot/commands/SetLightstripColor.java
new file mode 100644
index 0000000..8a3395c
--- /dev/null
+++ b/src/main/java/frc/robot/commands/SetLightstripColor.java
@@ -0,0 +1,42 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.LightStrip;
+
+
+public class SetLightstripColor extends Command {
+	private LightStrip lightStrip;
+
+	private int r;
+	private int g;
+	private int b;
+
+	/**
+	 * Set the color of the robot's lightstrip
+	 * @param lightStrip The lightstrip you want to use (there should only be ONE)
+	 * @param r Red level
+	 * @param g Green level
+	 * @param b Blue level
+	 */
+	public SetLightstripColor(LightStrip lightStrip, int r, int g, int b) {
+		this.lightStrip = lightStrip;
+		this.r = r;
+		this.g = g;
+		this.b = b;
+	}
+
+	@Override
+	public void initialize() {
+		lightStrip.setColor(r, g, b);
+	}
+
+	@Override
+	public boolean isFinished() {
+		return true;
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		// Don't think I have to do anything
+	}
+}
diff --git a/src/main/java/frc/robot/commands/LED.java b/src/main/java/frc/robot/commands/deprecated/LED.java
similarity index 92%
rename from src/main/java/frc/robot/commands/LED.java
rename to src/main/java/frc/robot/commands/deprecated/LED.java
index b7df19f..66728d6 100644
--- a/src/main/java/frc/robot/commands/LED.java
+++ b/src/main/java/frc/robot/commands/deprecated/LED.java
@@ -1,4 +1,4 @@
-package frc.robot.commands;
+package frc.robot.commands.deprecated;
 
 import edu.wpi.first.wpilibj.AddressableLED;
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
@@ -45,6 +45,8 @@ public void initialize() {
 	}
 
 	@Override
-	public boolean isFinished() { return true; }
+	public boolean isFinished() {
+		return true;
+	}
 
 }
diff --git a/src/main/java/frc/robot/subsystems/LightStrip.java b/src/main/java/frc/robot/subsystems/LightStrip.java
new file mode 100644
index 0000000..bac5a85
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/LightStrip.java
@@ -0,0 +1,34 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.LightConstants;
+
+public class LightStrip extends SubsystemBase {
+	private AddressableLED ledStrip;
+	private AddressableLEDBuffer ledBuffer;
+
+	public LightStrip(AddressableLED ledstrip) {
+		ledStrip = ledstrip;
+		ledBuffer = new AddressableLEDBuffer(LightConstants.LED_QUANTITY);
+
+		// Docs say this is an expensive operation so future maintainers should avoid
+		// modifying this excessibely
+		ledStrip.setLength(ledBuffer.getLength());
+		ledStrip.setData(ledBuffer);
+
+		ledStrip.start();
+	}
+
+	public void setColor(int r, int g, int b) {
+		for (int i = 0; i < ledBuffer.getLength(); i++) {
+			ledBuffer.setRGB(i, r, g, b);
+		}
+	}
+
+	@Override
+	public void periodic() {
+		ledStrip.setData(ledBuffer);
+	}
+}

From 2ec3b752c18dfba6a155fb276059de574867bb6d Mon Sep 17 00:00:00 2001
From: michael-lesirge <100492377+michael-lesirge@users.noreply.github.com>
Date: Wed, 28 Feb 2024 21:19:37 -0800
Subject: [PATCH 99/99] Diagonal auto drive to and other changes

---
 shuffleboard.json                             | 56 +-----------
 src/main/java/frc/robot/Constants.java        |  3 +-
 src/main/java/frc/robot/RobotContainer.java   |  2 +
 .../java/frc/robot/commands/AimAtTag.java     |  6 +-
 .../java/frc/robot/commands/AutoDriveTo.java  | 28 ++----
 .../java/frc/robot/commands/AutoPosition.java | 89 -------------------
 src/main/java/frc/robot/commands/Autos.java   | 11 ++-
 .../java/frc/robot/commands/DriveToPose.java  | 72 ---------------
 .../frc/robot/commands/DriveTransform.java    | 68 --------------
 .../robot/subsystems/SwerveDrivetrain.java    | 10 ++-
 10 files changed, 27 insertions(+), 318 deletions(-)
 delete mode 100644 src/main/java/frc/robot/commands/AutoPosition.java
 delete mode 100644 src/main/java/frc/robot/commands/DriveToPose.java
 delete mode 100644 src/main/java/frc/robot/commands/DriveTransform.java

diff --git a/shuffleboard.json b/shuffleboard.json
index 9c3e998..2142459 100644
--- a/shuffleboard.json
+++ b/shuffleboard.json
@@ -140,7 +140,7 @@
           },
           "6,2": {
             "size": [
-              2,
+              3,
               1
             ],
             "content": {
@@ -208,60 +208,6 @@
               "Visuals/Orientation": "HORIZONTAL"
             }
           },
-          "7,3": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/tPoseX",
-              "_title": "tPoseX",
-              "_glyph": 110,
-              "_showGlyph": true
-            }
-          },
-          "6,3": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/tPoseY",
-              "_title": "tPoseY",
-              "_glyph": 111,
-              "_showGlyph": true
-            }
-          },
-          "8,3": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Text View",
-              "_source0": "network_table:///SmartDashboard/tPoseDegrees",
-              "_title": "tPoseDegrees",
-              "_glyph": 419,
-              "_showGlyph": true
-            }
-          },
-          "8,2": {
-            "size": [
-              1,
-              1
-            ],
-            "content": {
-              "_type": "Boolean Box",
-              "_source0": "network_table:///SmartDashboard/tPoseActive",
-              "_title": "tPoseActive",
-              "_glyph": 288,
-              "_showGlyph": true,
-              "Colors/Color when true": "#7CFC00FF",
-              "Colors/Color when false": "#8B0000FF"
-            }
-          },
           "1,0": {
             "size": [
               2,
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index d219809..3673208 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -105,7 +105,7 @@ public static class ArmConstants {
 	}
 
 	public static class RobotMovementConstants {
-		public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(0.0001);
+		public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(6);
 		public static final double ANGLE_TOLERANCE_RADIANS = Units.degreesToRadians(1);
 
 		public static final double ROTATION_PID_P = 5;
@@ -115,6 +115,7 @@ public static class RobotMovementConstants {
 		public static final double TRANSLATION_PID_P = 30;
 		public static final double TRANSLATION_PID_I = 0.5;
 		public static final double TRANSLATION_PID_D = 15;
+		public static final double MAX_TRANSLATION_SPEED = 0.2;
 	}
 
 	public static class OperatorConstants {
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 66670f9..f592e1d 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -99,6 +99,8 @@ public class RobotContainer {
 	 */
 	public RobotContainer() {
 		autoChooser.addOption("Rotate by 90", Autos.rotateTestAuto(drivetrain, 90, false));
+		autoChooser.addOption("Forward", Autos.driveAuto(drivetrain, +1));
+		autoChooser.addOption("Backward", Autos.driveAuto(drivetrain, -1));
 		SmartDashboard.putData("Auto Chooser", autoChooser);
 
 		configureBindings();
diff --git a/src/main/java/frc/robot/commands/AimAtTag.java b/src/main/java/frc/robot/commands/AimAtTag.java
index ca8c9bc..e89a75a 100644
--- a/src/main/java/frc/robot/commands/AimAtTag.java
+++ b/src/main/java/frc/robot/commands/AimAtTag.java
@@ -40,9 +40,9 @@ public AimAtTag(SwerveDrivetrain drivetrain, Vision vision, Integer tagID, Chass
 		this.chassisDriveInputs = chassisDriveInputs;
 
 		rotatePID = new PIDController(
-				2,
-				0,
-				0);
+				RobotMovementConstants.ROTATION_PID_P,
+				RobotMovementConstants.ROTATION_PID_I,
+				RobotMovementConstants.ROTATION_PID_D);
 		rotatePID.enableContinuousInput(-1, 1);
 		rotatePID.setTolerance(Units.radiansToDegrees(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS));
 		rotatePID.setSetpoint(0);
diff --git a/src/main/java/frc/robot/commands/AutoDriveTo.java b/src/main/java/frc/robot/commands/AutoDriveTo.java
index fd07c4c..2ddaac0 100644
--- a/src/main/java/frc/robot/commands/AutoDriveTo.java
+++ b/src/main/java/frc/robot/commands/AutoDriveTo.java
@@ -7,7 +7,6 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.RobotMovementConstants;
 
 public class AutoDriveTo extends Command {
@@ -16,8 +15,6 @@ public class AutoDriveTo extends Command {
 	private final PIDController xMovePID, yMovePID;
 	private double initX, initY, goalX, goalY;
 
-	private boolean xOnlyMode;
-
 	/***
 	 * Command to autonomously drive somewhere
 	 * @param subsystem The drivetrain
@@ -29,8 +26,6 @@ public AutoDriveTo(SwerveDrivetrain subsystem, Translation2d translation) {
 		goalX = translation.getX();
 		goalY = translation.getY();
 
-		xOnlyMode = Math.abs(goalX) > Math.abs(goalY);
-
 		xMovePID = new PIDController(
 				RobotMovementConstants.TRANSLATION_PID_P,
 				RobotMovementConstants.TRANSLATION_PID_I,
@@ -64,29 +59,18 @@ public void execute() {
 		double x = position.getX() - initX;
 		double y = position.getY() - initY;
 
-		SmartDashboard.putNumber("PoseY", position.getY());
-		SmartDashboard.putNumber("PoseX", position.getX());
-		SmartDashboard.putNumber("PoseDegrees", position.getRotation().getDegrees());
-
 		double xSpeed = xMovePID.calculate(x, goalX);
 		double ySpeed = yMovePID.calculate(y, goalY);
 
-		if (xOnlyMode)
-			ySpeed = 0;
-		else
-			xSpeed = 0;
+		double speedLimit = RobotMovementConstants.MAX_TRANSLATION_SPEED;
+		double maxSpeed = Math.max(Math.abs(x), Math.abs(y));
 
-		// TEMP FIX: LEAVE HERE UNTIL BUMPERS!!!
-		if (Math.abs(xSpeed) > 0.5) {
-			xSpeed = 0.5 * Math.signum(xSpeed);
+		if (maxSpeed > speedLimit) {
+			xSpeed /= speedLimit * maxSpeed;
+			ySpeed /= speedLimit * maxSpeed;
 		}
 
-		final ChassisSpeeds speeds = new ChassisSpeeds(
-				xSpeed,
-				ySpeed,
-				0);
-
-		drivetrain.setDesiredState(speeds);
+		drivetrain.setDesiredState(new ChassisSpeeds(xSpeed, ySpeed, 0));
 		drivetrain.updateSmartDashboard();
 	}
 
diff --git a/src/main/java/frc/robot/commands/AutoPosition.java b/src/main/java/frc/robot/commands/AutoPosition.java
deleted file mode 100644
index 46ced32..0000000
--- a/src/main/java/frc/robot/commands/AutoPosition.java
+++ /dev/null
@@ -1,89 +0,0 @@
-package frc.robot.commands;
-
-import frc.robot.subsystems.ExampleSubsystem;
-import edu.wpi.first.wpilibj2.command.Command;
-
-// How to make Command: https://compendium.readthedocs.io/en/latest/tasks/commands/commands.html (ignore image instructions, code is out of date, just look at written general instructions)
-// Command based programming: https://docs.wpilib.org/en/stable/docs/software/commandbased/what-is-command-based.html
-// Code documentations https://docs.wpilib.org/en/stable/docs/software/commandbased/commands.html 
-
-/** An example command that uses an example subsystem. */
-public class AutoPosition extends Command {
-	private final ExampleSubsystem subsystem;
-
-	/**
-	 * The constructor creates a new command and is automatically called one time
-	 * when the command is created (with 'new' keyword).
-	 * It should set up the initial state and properties of the object to ensure
-	 * it's ready for use.
-	 * This can take in any arguments you need. It normally uses 1 subsystem (but an
-	 * take multiple when necessary),
-	 * as wells as arguments for what to do, such as a joystick in the drive command
-	 * or a desired position in an auto command.
-	 * Example uses include saving parameters passed to the command, creating and
-	 * configuring objects for the class like PID controllers, and adding subsystem
-	 * requirements
-	 */
-	public AutoPosition(ExampleSubsystem subsystem) {
-		// use "this" to access member variable subsystem rather than local subsystem
-		this.subsystem = subsystem;
-
-		// Use addRequirements() here to declare subsystem dependencies.
-		// This makes sure no other commands try to do stuff with your subsystem while
-		// you are using it.
-		addRequirements(this.subsystem);
-	}
-
-	/**
-	 * initialize() is used to prepare a command for execution and is called once
-	 * when the command is scheduled.
-	 * It should reset the command's state since command objects can be used
-	 * multiple times.
-	 * Example uses include setting motor to constant speed, setting a solenoid to a
-	 * certain state, and resetting variables
-	 */
-	@Override
-	public void initialize() {
-	}
-
-	/**
-	 * execute() is called repeatedly while a command is scheduled, about every
-	 * 20ms.
-	 * It should handle continuous tasks specific to the command, like updating
-	 * motor outputs based on joystick inputs or utilizing control loop results.
-	 * Example uses include adjusting motor speeds for real-time control, processing
-	 * sensor data within a scheduled command, and using the output of a control
-	 * loop.
-	 */
-	@Override
-	public void execute() {
-	}
-
-	/**
-	 * isFinished() finished is called repeatedly while a command is scheduled,
-	 * right after execute.
-	 * It should return true when you want the command to finish. end(false) is
-	 * called directly after isFinished() returns true.
-	 * Example uses include checking if control loop is at set point, and always
-	 * returning false to end after just 1 call to execute.
-	 */
-	@Override
-	public boolean isFinished() {
-		return true;
-	}
-
-	/**
-	 * end(boolean interrupted) is called once when a command ends, regardless of
-	 * whether it finishes normally or is interrupted.
-	 * It should wrap up the command since other commands might use the same
-	 * subsystems.
-	 * Once end runs the command will no longer be in the command scheduler loop.
-	 * It takes in a boolean interrupted which is set to true when the command is
-	 * ended without isFinished() returning true.
-	 * Example uses include setting motor speeds back to zero, and setting a
-	 * solenoid back to a "default" state.
-	 */
-	@Override
-	public void end(boolean interrupted) {
-	}
-}
diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java
index 6ec5e02..31a93a2 100644
--- a/src/main/java/frc/robot/commands/Autos.java
+++ b/src/main/java/frc/robot/commands/Autos.java
@@ -1,7 +1,6 @@
 package frc.robot.commands;
 
 import frc.robot.subsystems.SwerveDrivetrain;
-import frc.robot.subsystems.Vision;
 import frc.robot.subsystems.arm.ArmInterface;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
@@ -15,14 +14,14 @@
  */
 public final class Autos {
 	/** Example static factory for an autonomous command. */
-	public static Command rotateTestAuto(SwerveDrivetrain drivetrain, double degrees, boolean fieldRelative) {
+	public static Command driveAuto(SwerveDrivetrain drivetrain, double meters) {
 		return Commands.sequence(
-				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(90), fieldRelative));
+				new AutoDriveTo(drivetrain, new Translation2d(meters, meters)));
 	}
 
-	/** Auto-mode that attempts to follow an april tag. */
-	public static Command tagFollowAuto(SwerveDrivetrain drivetrain, Vision camera, Integer tagId) {
-		return new FollowTag(drivetrain, camera, new Translation2d(1, 0), tagId, null);
+	public static Command rotateTestAuto(SwerveDrivetrain drivetrain, double degrees, boolean fieldRelative) {
+		return Commands.sequence(
+				new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(90), fieldRelative));
 	}
 
 	/** Linden did this */
diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java
deleted file mode 100644
index 0782cbe..0000000
--- a/src/main/java/frc/robot/commands/DriveToPose.java
+++ /dev/null
@@ -1,72 +0,0 @@
-package frc.robot.commands;
-
-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.wpilibj2.command.Command;
-import frc.robot.subsystems.SwerveDrivetrain;
-
-/** Command to automatically drive to a certain Pose on the field */
-public class DriveToPose extends Command {
-	private final SwerveDrivetrain drivetrain;
-	private final Pose2d targetPosition;
-
-	/**
-	 * Create a new DriveToPose command. Uses setDesiredPosition on drivetrain.
-	 * 
-	 * <p>
-	 * This drives relative to the robot starting position,
-	 * so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
-	 * where forward is whatever direction the robot started in
-	 * 
-	 * <p>
-	 * The last place the drivetrain position was reset counts as the starting position
-	 * 
-	 * @param drivetrain	the drivetrain of the robot
-	 * @param position		Target pose the robot is trying to drive to
-	 */
-	public DriveToPose(SwerveDrivetrain drivetrain, Pose2d position) {
-		this.drivetrain = drivetrain;
-		this.targetPosition = position;
-		
-		addRequirements(drivetrain);
-	}
-
-	/**
-	 * <p>
-	 * This drives relative to the robot starting position,
-	 * so a pose of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
-	 * where forward is whatever direction the robot started in
-	 * 
-	 * <p>
-	 * The last place the drivetrain position was reset counts as the starting position
-	 * 
-	 * @param drivetrain	The drivetrain of the robot
-	 * @param translation	Target transform to drive
-	 * @param rotation	 	Target rotation to drive
-	 */
-	public DriveToPose(SwerveDrivetrain drivetrain, Translation2d translation, Rotation2d rotation) {
-		this(drivetrain, new Pose2d(translation, rotation));
-	}
-
-	@Override
-	public void initialize() {
-		drivetrain.setDesiredPosition(targetPosition);
-	}
-
-	@Override
-	public void execute() {
-		drivetrain.updateSmartDashboard();
-	}
-
-	@Override
-	public boolean isFinished() {
-		return drivetrain.isAtDesiredPosition();
-	}
-
-	@Override
-	public void end(boolean interrupted) {
-		drivetrain.clearDesiredPosition();
-		drivetrain.stop();
-	}
-}
diff --git a/src/main/java/frc/robot/commands/DriveTransform.java b/src/main/java/frc/robot/commands/DriveTransform.java
deleted file mode 100644
index 866aa51..0000000
--- a/src/main/java/frc/robot/commands/DriveTransform.java
+++ /dev/null
@@ -1,68 +0,0 @@
-package frc.robot.commands;
-
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.SwerveDrivetrain;
-
-/** Command to automatically drive a certain transform */
-public class DriveTransform extends Command {
-	private final SwerveDrivetrain drivetrain;
-	private final Transform2d transform;
-
-	/**
-	 * Create a new DriveTransform command. Tries to drive a certain transform using the DriveToPose command.
-	 * 
-	 * <p>This drives relative to the robot starting position,
-	 * so a transform of +2x and +1y will drive to the position 2 meters forward and 1 meter left of whether the robot started,
-     * where forward is whatever direction the robot is currently facing</p>
-     *  
-	 * @param drivetrain the drivetrain of the robot
-	 * @param transform  target transform to drive to, will be added to current
-	 *                   position to get target pose
-	 */
-	public DriveTransform(SwerveDrivetrain drivetrain, Transform2d transform) {
-		this.drivetrain = drivetrain;
-		this.transform = transform;
-
-		addRequirements(drivetrain);
-	}
-
-	/**
-	 * Create a new DriveToTransform command. Uses setDesiredPosition on drivetrain.
-	 * 
-	 * <p>
-	 * This drives relative to the robot, so a transform of +2x and +1y will drive 2
-	 * meters forward and 1 meter left,
-	 * where forward is whatever direction the robot is currently facing
-	 * 
-	 * @param drivetrain  The drivetrain of the robot
-	 * @param translation Target transform to drive
-	 * @param rotation Target rotation to drive
-	 */
-	public DriveTransform(SwerveDrivetrain drivetrain, Translation2d translation, Rotation2d rotation) {
-		this(drivetrain, new Transform2d(translation, rotation));
-	}
-
-	@Override
-	public void initialize() {
-		drivetrain.setDesiredPosition(drivetrain.getPosition().plus(transform));
-	}
-
-	@Override
-	public void execute() {
-		drivetrain.updateSmartDashboard();
-	}
-
-	@Override
-	public boolean isFinished() {
-		return drivetrain.isAtDesiredPosition();
-	}
-
-	@Override
-	public void end(boolean interrupted) {
-		drivetrain.clearDesiredPosition();
-		drivetrain.stop();
-	}
-}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
index 4c1befe..8055869 100644
--- a/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/SwerveDrivetrain.java
@@ -293,10 +293,16 @@ public void setDesiredState(ChassisSpeeds speeds, boolean fieldRelative, boolean
 		SmartDashboard.putNumber("Spin", speeds.omegaRadiansPerSecond);
 		SmartDashboard.putBoolean("Field Relieve", fieldRelative);
 
-		if (fieldRelative)
+		if (fieldRelative) {
 			speeds = ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getHeading());
-
+		}
+		
 		SwerveModuleState[] states = kinematics.toSwerveModuleStates(speeds);
+		
+		if (powerDriveMode) {
+			SwerveDriveKinematics.desaturateWheelSpeeds(states, 1.0);
+		}
+
 		for (int i = 0; i < modules.length; i++) {
 			modules[i].setDesiredState(states[i], powerDriveMode);
 		}