added pid class in constants

.pathplanner/settings.json

@@ -9,4 +9,4 @@
   "defaultMaxAngVel": 540.0,
   "defaultMaxAngAccel": 720.0,
   "maxModuleSpeed": 4.0
-}
+}

src/main/deploy/pathplanner/autos/Move Backward + Apriltag.auto

@@ -22,4 +22,4 @@
   },
   "folder": null,
   "choreoAuto": false
-}
+}

src/main/deploy/pathplanner/paths/Really Short Path pt.1.path

@@ -46,4 +46,4 @@
   "folder": null,
   "previewStartingState": null,
   "useDefaultConstraints": false
-}
+}

src/main/deploy/pathplanner/paths/Really Short Path pt.2.path

@@ -68,4 +68,4 @@
   "folder": null,
   "previewStartingState": null,
   "useDefaultConstraints": false
-}
+}

src/main/java/frc/robot/Constants.java

@@ -42,28 +42,24 @@ public static class CAN {
   public static class Speeds {
     public static final double SWERVE_DRIVE_COEFF = 1;
 
-    public static final double SWERVE_PID_KP = 0.012;
-
     public static final double SWERVE_DRIVE_MAX_SPEED = 4.67; // TODO: Get this value
     public static final double SWERVE_DRIVE_MAX_ANGULAR_SPEED = Math.PI * 1; // Todo: Get this value
 
-    public static final double SHOOTER_MAX_SPEED = 1;
+    public static final double SHOOTER_MAX_SPEED = 1; // TODO: Get this
   }
 
   public static class Physical {
     // GEAR RATIOS ARE: DRIVEN GEAR TEETH / DRIVING GEAR TEETH
-
     public static final double SWERVE_DRIVE_BASE_RADIUS = 0.43;
 
     public static final double SWERVE_DRIVE_GEAR_RATIO = 6.75;
     public static final double SWERVE_DRIVE_WHEEL_RADIUS = 0.05;
     public static final double SWERVE_DRIVE_WHEEL_CIRCUMFERENCE = 2 * Math.PI * SWERVE_DRIVE_WHEEL_RADIUS;
 
-    public static final double SHOOTER_GEAR_RATIO = 1.45;
+    public static final double SHOOTER_GEAR_RATIO = 1.45; // TODO: FIND THIS
     public static final double SHOOTER_FLYWHEEL_RADIUS = 1; // TODO: FIND RADIUS
     public static final double SHOOTER_FLYWHEEL_CIRCUMFERENCE = 2 * Math.PI * SHOOTER_FLYWHEEL_RADIUS;
 
-
     public static final double TALON_TICKS_PER_REVOLUTION = 2048;
 
     public static final SwerveDriveKinematics SWERVE_KINEMATICS = new SwerveDriveKinematics(
@@ -72,7 +68,10 @@ public static class Physical {
       new Translation2d(-0.292, -0.292),
       new Translation2d(-0.292, 0.292)
     );
+  }
 
+  public static class PID {
+    public static final double SWERVE_PID_KP = 0.012;
     public static final double SHOOTER_PID_KP = 0;
   }
 

src/main/java/frc/robot/subsystems/ShooterSubsystem.java

@@ -23,7 +23,7 @@ public class ShooterSubsystem extends SubsystemBase {
 
   private final RelativeEncoder _leftEncoder = _leftMotor.getEncoder();
 
-  private final PIDController _shooterController = new PIDController(Constants.Physical.SHOOTER_PID_KP, 0, 0);
+  private final PIDController _shooterController = new PIDController(Constants.PID.SHOOTER_PID_KP, 0, 0);
 
 
   /** Creates a new ShooterSubsystem. */

src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java

@@ -35,41 +35,44 @@
  */
 public class SwerveDriveSubsystem extends SubsystemBase {
   // each swerve module
-  private final SwerveModule _frontLeft =
-      new SwerveModule(
-          Constants.CAN.DRIVE_FRONT_LEFT,
-          Constants.CAN.ROT_FRONT_LEFT,
-          Constants.CAN.ENC_FRONT_LEFT,
-          Constants.Offsets.ENCODER_FRONT_LEFT,
-          .05,
-          0.15);
-  private final SwerveModule _frontRight =
-      new SwerveModule(
-          Constants.CAN.DRIVE_FRONT_RIGHT,
-          Constants.CAN.ROT_FRONT_RIGHT,
-          Constants.CAN.ENC_FRONT_RIGHT,
-          Constants.Offsets.ENCODER_FRONT_RIGHT,
-          .05,
-          0.17);
-  private final SwerveModule _backRight =
-      new SwerveModule(
-          Constants.CAN.DRIVE_BACK_RIGHT,
-          Constants.CAN.ROT_BACK_RIGHT,
-          Constants.CAN.ENC_BACK_RIGHT,
-          Constants.Offsets.ENCODER_BACK_RIGHT,
-          .05,
-          0.18);
-  private final SwerveModule _backLeft =
-      new SwerveModule(
-          Constants.CAN.DRIVE_BACK_LEFT,
-          Constants.CAN.ROT_BACK_LEFT,
-          Constants.CAN.ENC_BACK_LEFT,
-          Constants.Offsets.ENCODER_BACK_LEFT,
-          .05,
-          0.17);
-
-  private final BNO055 _gyro =
-      BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS, BNO055.vector_type_t.VECTOR_EULER);
+  private final SwerveModule _frontLeft = new SwerveModule(
+    Constants.CAN.DRIVE_FRONT_LEFT,
+    Constants.CAN.ROT_FRONT_LEFT,
+    Constants.CAN.ENC_FRONT_LEFT,
+    Constants.Offsets.ENCODER_FRONT_LEFT,
+    .05,
+    0.15
+  );
+
+  private final SwerveModule _frontRight = new SwerveModule(
+    Constants.CAN.DRIVE_FRONT_RIGHT,
+    Constants.CAN.ROT_FRONT_RIGHT,
+    Constants.CAN.ENC_FRONT_RIGHT,
+    Constants.Offsets.ENCODER_FRONT_RIGHT,
+    .05,
+    0.17
+  );
+
+  private final SwerveModule _backRight = new SwerveModule(
+    Constants.CAN.DRIVE_BACK_RIGHT,
+    Constants.CAN.ROT_BACK_RIGHT,
+    Constants.CAN.ENC_BACK_RIGHT,
+    Constants.Offsets.ENCODER_BACK_RIGHT,
+    .05,
+    0.18
+  );
+
+  private final SwerveModule _backLeft = new SwerveModule(
+    Constants.CAN.DRIVE_BACK_LEFT,
+    Constants.CAN.ROT_BACK_LEFT,
+    Constants.CAN.ENC_BACK_LEFT,
+    Constants.Offsets.ENCODER_BACK_LEFT,
+    .05,
+    0.17
+  );
+
+  private final BNO055 _gyro = BNO055.getInstance(BNO055.opmode_t.OPERATION_MODE_IMUPLUS,
+      BNO055.vector_type_t.VECTOR_EULER);
 
   private VisionSubsystem _visionSubsystem;
 
@@ -86,22 +89,20 @@ public class SwerveDriveSubsystem extends SubsystemBase {
   /** A boolean for whether the swerve is field oriented or not. */
   public boolean fieldOriented = false;
 
-  private final BuiltInAccelerometer _imu = new BuiltInAccelerometer();
-
   // Pose Estimator -> Has built in odometry and uses supplied vision measurements
-  private final SwerveDrivePoseEstimator _estimator =
-      new SwerveDrivePoseEstimator(
-          Constants.Physical.SWERVE_KINEMATICS,
-          getHeadingRaw(),
-          new SwerveModulePosition[] {
-            _frontLeft.getPosition(),
-            _frontRight.getPosition(),
-            _backRight.getPosition(),
-            _backLeft.getPosition()
-          },
-          new Pose2d(),
-          VecBuilder.fill(0.008, 0.008, 0.0075),
-          VecBuilder.fill(0.2, .2, .75));
+  private final SwerveDrivePoseEstimator _estimator = new SwerveDrivePoseEstimator(
+    Constants.Physical.SWERVE_KINEMATICS,
+    getHeadingRaw(),
+    new SwerveModulePosition[] {
+        _frontLeft.getPosition(),
+        _frontRight.getPosition(),
+        _backRight.getPosition(),
+        _backLeft.getPosition()
+    },
+    new Pose2d(),
+    VecBuilder.fill(0.008, 0.008, 0.0075),
+    VecBuilder.fill(0.2, .2, .75)
+  );
 
   /** Return the estimated pose of the swerve chassis. */
   public Pose2d getPose() {
@@ -125,36 +126,40 @@ public SwerveDriveSubsystem(VisionSubsystem visionSubsystem) {
     modules[3] = _backLeft;
 
     // for (int i = 0; i < modules.length; i++) {
-    //   for (int j = 0; j < 2; j++) {
-    //     _orchestra.addInstrument(modules[i].returnTalons()[j]);
-    //   }
+    // for (int j = 0; j < 2; j++) {
+    // _orchestra.addInstrument(modules[i].returnTalons()[j]);
+    // }
     // }
 
     // _orchestra.loadMusic(song);
 
     // _orchestra.play();
 
     // pathplannerlib setup
+
     AutoBuilder.configureHolonomic(
-        this::getPose, 
-        this::resetPose,
-        this::getRobotRelativeSpeeds,
-        this::driveChassis,
-        new HolonomicPathFollowerConfig(
-            new PIDConstants(2.5, 0, 0),
-            new PIDConstants(2.8, 0, 0),
-            Constants.Speeds.SWERVE_DRIVE_MAX_SPEED,
-            Constants.Physical.SWERVE_DRIVE_BASE_RADIUS,
-            new ReplanningConfig()),
-        () -> {
-          Optional<Alliance> alliance = DriverStation.getAlliance();
-
-          if (alliance.isPresent()) {
-            return alliance.get() == DriverStation.Alliance.Red;
-          }
-          return false;
-        },
-        this);
+      this::getPose,
+      this::resetPose,
+      this::getRobotRelativeSpeeds,
+      this::driveChassis,
+      new HolonomicPathFollowerConfig(
+        new PIDConstants(2.5, 0, 0),
+        new PIDConstants(2.8, 0, 0),
+        Constants.Speeds.SWERVE_DRIVE_MAX_SPEED,
+        Constants.Physical.SWERVE_DRIVE_BASE_RADIUS,
+        new ReplanningConfig()
+      ),
+      () -> {
+        Optional<Alliance> alliance = DriverStation.getAlliance();
+
+        if (alliance.isPresent()) {
+          return alliance.get() == DriverStation.Alliance.Red;
+        }
+        return false;
+      },
+
+      this
+    );
   }
 
   @Override
@@ -174,17 +179,15 @@ public void periodic() {
     SmartDashboard.putNumber("Back Left Velocity", _backLeft.getDriveVelocity());
     SmartDashboard.putNumber("Back Right Velocity", _backRight.getDriveVelocity());
 
-
     // Update the bot's pose
-    _pose =
-        _estimator.update(
-            getHeadingRaw(),
-            new SwerveModulePosition[] {
-              _frontLeft.getPosition(),
-              _frontRight.getPosition(),
-              _backRight.getPosition(),
-              _backLeft.getPosition()
-            });
+    _pose = _estimator.update(
+        getHeadingRaw(),
+        new SwerveModulePosition[] {
+            _frontLeft.getPosition(),
+            _frontRight.getPosition(),
+            _backRight.getPosition(),
+            _backLeft.getPosition()
+        });
 
     if (_visionSubsystem.isApriltagVisible()) {
       _estimator.addVisionMeasurement(_visionSubsystem.get_botpose(), Timer.getFPGATimestamp());
@@ -193,19 +196,22 @@ public void periodic() {
     _field.setRobotPose(_pose);
     SmartDashboard.putData("FIELD", _field);
 
-    _robotSpeed =
-        Math.sqrt(
-            Math.pow(getRobotRelativeSpeeds().vxMetersPerSecond, 2)
-                + Math.pow(getRobotRelativeSpeeds().vyMetersPerSecond, 2));
+    _robotSpeed = Math.sqrt(
+        Math.pow(getRobotRelativeSpeeds().vxMetersPerSecond, 2)
+            + Math.pow(getRobotRelativeSpeeds().vyMetersPerSecond, 2));
 
-    // SmartDashboard.putNumber("ACTUAL X SPEED", getRobotRelativeSpeeds().vxMetersPerSecond);
-    // SmartDashboard.putNumber("ACTUAL Y SPEED", getRobotRelativeSpeeds().vyMetersPerSecond);
+    // SmartDashboard.putNumber("ACTUAL X SPEED",
+    // getRobotRelativeSpeeds().vxMetersPerSecond);
+    // SmartDashboard.putNumber("ACTUAL Y SPEED",
+    // getRobotRelativeSpeeds().vyMetersPerSecond);
   }
 
   /**
    * Set the chassis speed of the swerve drive.
    *
-   * <p>Chassis speed will be treated as field oriented if the fieldOriented class attribute is set
+   * <p>
+   * Chassis speed will be treated as field oriented if the fieldOriented class
+   * attribute is set
    * to true, otherwise it will be robot-relative.
    *
    * @see ChassisSpeeds (wpilib chassis speeds class)
@@ -216,15 +222,15 @@ public void driveChassis(ChassisSpeeds chassisSpeeds) {
       chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(chassisSpeeds, getHeading());
     }
 
-    SwerveModuleState[] moduleStates =
-        Constants.Physical.SWERVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds);
+    SwerveModuleState[] moduleStates = Constants.Physical.SWERVE_KINEMATICS.toSwerveModuleStates(chassisSpeeds);
     setStates(moduleStates);
   }
 
   /**
    * Sets the state of each SwerveModule through an array.
    *
-   * <p>Order -> front left, front right, back right, back left
+   * <p>
+   * Order -> front left, front right, back right, back left
    */
   public void setStates(SwerveModuleState[] states) {
     _frontLeft.setState(states[0]);
@@ -235,11 +241,10 @@ public void setStates(SwerveModuleState[] states) {
 
   /** Resets the pose estimator's heading of the drive to 0. */
   public void resetGyro() {
-    Pose2d new_pose =
-        new Pose2d(
-            _pose.getTranslation().getX(),
-            _pose.getTranslation().getY(),
-            Rotation2d.fromDegrees(0));
+    Pose2d new_pose = new Pose2d(
+        _pose.getTranslation().getX(),
+        _pose.getTranslation().getY(),
+        Rotation2d.fromDegrees(0));
 
     resetPose(new_pose);
   }
@@ -256,10 +261,10 @@ public void resetPose(Pose2d newPose) {
     _estimator.resetPosition(
         getHeadingRaw(),
         new SwerveModulePosition[] {
-          _frontLeft.getPosition(),
-          _frontRight.getPosition(),
-          _backRight.getPosition(),
-          _backLeft.getPosition()
+            _frontLeft.getPosition(),
+            _frontRight.getPosition(),
+            _backRight.getPosition(),
+            _backLeft.getPosition()
         },
         newPose);
   }

src/main/java/frc/robot/utils/NeoConfig.java

@@ -15,10 +15,10 @@
 
 /** For configuring Neos (with CANSparkMax). */
 public class NeoConfig {
-    /** 
+    /**
      * Basic Neo (with CANSparkMax) config, sets Falcon to factory defaults, sets encoder to 0,
      * and sets Neo to Brake neutral mode.
-     * 
+     *
      * @param neo - The CANSparkMax (with Neo) to configure.
      * @param invert - Whether to invert the motor or not.
      */
@@ -31,7 +31,7 @@ public static void configureNeo(CANSparkMax neo, boolean invert) {
 
     /**
      * Configure a follower of a master Neo motor.
-     * 
+     *
      * @param neo - The Neo (with CANSparkMax) to config.
      * @param master - The master motor.
      * @param opposeMaster - Whether to oppose the master or not.

src/main/java/frc/robot/utils/TalonFXConfig.java

@@ -22,8 +22,8 @@ public class TalonFXConfig {
    * deadband and sets Falcon to Coast neutral mode.
    *
    * @param falcon - The Falcon to config.
-   * 
-   * @return The configuration object applied to the Falcon. 
+   *
+   * @return The configuration object applied to the Falcon.
    */
   public static TalonFXConfiguration configureFalcon(TalonFX falcon, boolean invert) {
     // TODO: will prob need to add the code to zero encoder