Merge branch 'full-robot-test' into vision-odo-not-proto

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

@@ -181,10 +181,10 @@ public class BoomBoom extends SubsystemBase {
 
   public void setShooterGains() {
     m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX);
-    m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
-    m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kF, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kP, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kI, ShooterConstants.SHOOTER_TIMEOUT_MS);
+    m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.kD, ShooterConstants.SHOOTER_TIMEOUT_MS);
   }
 
   public void runDrumShooterVelocityPID(double targetVel) {

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

@@ -21,31 +21,31 @@ import frc4388.utility.Gains;
 public class Hood extends SubsystemBase {
   public BoomBoom m_shooterSubsystem;
   
-  // public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
+  public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless);
   public SparkMaxLimitSwitch m_hoodUpLimitSwitch;
   public SparkMaxLimitSwitch m_hoodDownLimitSwitch;
   public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS;
-  // public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder();
+  public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder();
 
-  // public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController();
+  public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController();
   
   
-  //public boolean m_isHoodReady = false;
+  public boolean m_isHoodReady = false;
 
 public double m_fireAngle;
   
 
   /** Creates a new Hood. */
   public Hood() {
-    //  m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
+     m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake);
 
-    // m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
-    // m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
-    // m_hoodUpLimitSwitch.enableLimitSwitch(true);
-    // m_hoodDownLimitSwitch.enableLimitSwitch(true);
+    m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen);
+    m_hoodUpLimitSwitch.enableLimitSwitch(true);
+    m_hoodDownLimitSwitch.enableLimitSwitch(true);
 
-    // m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT);
-    // m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT);
+    m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT);
+    m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT);
     setHoodSoftLimits(true);
   }
     
@@ -60,30 +60,30 @@ public double m_fireAngle;
    * @param set Boolean to set soft limits to.
    */
   public void setHoodSoftLimits(boolean set) {
-    // m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kForward, set);
-    // m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kReverse, set);
+    m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kForward, set);
+    m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kReverse, set);
   }
 
   public void runAngleAdjustPID(double targetAngle) 
   {
     //Set PID Coefficients
-    // m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP);
-    // m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI);
-    // m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD);
-    // m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone);
-    // m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF);
-    // m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput);
+    m_angleAdjusterPIDController.setP(m_angleAdjusterGains.kP);
+    m_angleAdjusterPIDController.setI(m_angleAdjusterGains.kI);
+    m_angleAdjusterPIDController.setD(m_angleAdjusterGains.kD);
+    m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.kIzone);
+    m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.kF);
+    m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.kPeakOutput);
 
-    // m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition);
+    m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition);
   }
   
 
   public void runHood(double input) {
-    // m_angleAdjusterMotor.set(input);
+    m_angleAdjusterMotor.set(input);
   }
 
   public void resetGyroAngleAdj(){
-    // m_angleEncoder.setPosition(0);
+    m_angleEncoder.setPosition(0);
   }
 
   public double getAnglePosition(){

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

@@ -38,19 +38,27 @@ public class SwerveDrive extends SubsystemBase {
 
   public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
 
-  Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(halfWidth));
-  Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(-halfWidth));
-  Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth));
-  Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth));
-      
-  public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
+  Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight),
+      Units.inchesToMeters(halfWidth));
+  Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight),
+      Units.inchesToMeters(-halfWidth));
+  Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight),
+      Units.inchesToMeters(halfWidth));
+  Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight),
+      Units.inchesToMeters(-halfWidth));
+
+  public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation,
+      m_backLeftLocation, m_backRightLocation);
 
   public SwerveModule[] modules;
   public WPI_PigeonIMU m_gyro;
   protected FusionStatus fstatus = new FusionStatus();
 
-  /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
-  below are robot specific, and should be tuned. */
+  /*
+   * Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings.
+   * The numbers used
+   * below are robot specific, and should be tuned.
+   */
   public SwerveDrivePoseEstimator m_poseEstimator;
   public SwerveDriveOdometry m_odometry;
   public VisionOdometry m_visionOdometry;
@@ -62,7 +70,8 @@ public class SwerveDrive extends SubsystemBase {
 
   private final Field2d m_field = new Field2d();
 
-  public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, WPI_PigeonIMU gyro) {
+  public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack,
+      WPI_PigeonIMU gyro) {
 
     m_leftFront = leftFront;
     m_leftBack = leftBack;
@@ -70,53 +79,55 @@ public class SwerveDrive extends SubsystemBase {
     m_rightBack = rightBack;
     m_gyro = gyro;
 
-    modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack};
+    modules = new SwerveModule[] { m_leftFront, m_rightFront, m_leftBack, m_rightBack };
 
-    m_poseEstimator =
-      new SwerveDrivePoseEstimator(
-          m_gyro.getRotation2d(),
-          new Pose2d(),
-          m_kinematics,
-          VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),
-          VecBuilder.fill(Units.degreesToRadians(1)),
-          VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)));
+    m_poseEstimator = new SwerveDrivePoseEstimator(
+        m_gyro.getRotation2d(),
+        new Pose2d(),
+        m_kinematics,
+        VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),
+        VecBuilder.fill(Units.degreesToRadians(1)),
+        VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)));
 
     m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
 
-    m_gyro.reset(); 
+    m_gyro.reset();
     SmartDashboard.putData("Field", m_field);
   }
-//https://github.com/ZachOrr/MK3-Swerve-Example
- /**
- * Method to drive the robot using joystick info.
- *
- * @param speeds[0] Speed of the robot in the x direction (forward).
- * @param speeds[1] Speed of the robot in the y direction (sideways).
- * @param rot Angular rate of the robot.
- * @param fieldRelative Whether the provided x and y speeds are relative to the field.
- */
-  public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative)
-  {
-    if (speedX == 0 && speedY == 0 && rot == 0) ignoreAngles = true;
-    else ignoreAngles = false;
-    Translation2d speed = new Translation2d(speedX, speedY);
+
+  // https://github.com/ZachOrr/MK3-Swerve-Example
+  /**
+   * Method to drive the robot using joystick info.
+   *
+   * @param speeds[0]     Speed of the robot in the x direction (forward).
+   * @param speeds[1]     Speed of the robot in the y direction (sideways).
+   * @param rot           Angular rate of the robot.
+   * @param fieldRelative Whether the provided x and y speeds are relative to the
+   *                      field.
+   */
+  public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative) {
+    if (speedX == 0 && speedY == 0 && rot == 0)
+      ignoreAngles = true;
+    else
+      ignoreAngles = false;
+    Translation2d speed = new Translation2d(-speedX, speedY);
     double mag = speed.getNorm();
     speed = speed.times(mag * speedAdjust);
 
     double xSpeedMetersPerSecond = -speed.getX();
     double ySpeedMetersPerSecond = speed.getY();
-    SwerveModuleState[] states =
-        m_kinematics.toSwerveModuleStates(
-            fieldRelative
-              ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
-              : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
+    SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
+        fieldRelative
+            ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
+                rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
+            : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
+                rot * SwerveDriveConstants.ROTATION_SPEED));
     setModuleStates(states);
   }
 
-  public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative)
-  {
+  public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative) {
     ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
-    Translation2d speed = new Translation2d(leftX, leftY);
+    Translation2d speed = new Translation2d(-leftX, leftY);
     speed = speed.times(speed.getNorm() * speedAdjust);
     if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND)
       rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1));
@@ -124,20 +135,22 @@ public class SwerveDrive extends SubsystemBase {
     double xSpeedMetersPerSecond = -speed.getX();
     double ySpeedMetersPerSecond = speed.getY();
     chassisSpeeds = fieldRelative
-      ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
-      : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED);
-    SwerveModuleState[] states =
-        m_kinematics.toSwerveModuleStates(
-            chassisSpeeds);
+        ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
+            rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
+        : new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED);
+    SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(
+        chassisSpeeds);
     setModuleStates(states);
   }
 
   /**
    * Set each module of the swerve drive to the corresponding desired state.
+   * 
    * @param desiredStates Array of module states to set.
    */
   public void setModuleStates(SwerveModuleState[] desiredStates) {
-    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
+    SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates,
+        Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
     for (int i = 0; i < desiredStates.length; i++) {
       SwerveModule module = modules[i];
       SwerveModuleState state = desiredStates[i];
@@ -165,13 +178,16 @@ public class SwerveDrive extends SubsystemBase {
     SmartDashboard.putNumber("Odometry: θ", getOdometry().getRotation().getDegrees());
 
     // chassis speeds
-    // TODO: find the actual max velocity in m/s of the robot in fast mode to have accurate chassis speeds 
+    // TODO: find the actual max velocity in m/s of the robot in fast mode to have
+    // accurate chassis speeds
     SmartDashboard.putNumber("Chassis Vel: X", chassisSpeeds.vxMetersPerSecond);
     SmartDashboard.putNumber("Chassis Vel: Y", chassisSpeeds.vyMetersPerSecond);
     SmartDashboard.putNumber("Chassis Vel: ω", chassisSpeeds.omegaRadiansPerSecond);
   }
+
   /**
    * Gets the distance between two given poses.
+   * 
    * @param p1 The first pose.
    * @param p2 The second pose.
    * @return Absolute distance between p1 and p2.
@@ -184,13 +200,14 @@ public class SwerveDrive extends SubsystemBase {
    * Returns a scalar from your distance to the hub to your target distance.
    * 
    * @param target_dist The target distance.
-   * @return A scalar that multiplies your distance from the hub to get your target distance.
+   * @return A scalar that multiplies your distance from the hub to get your
+   *         target distance.
    */
   public Pose2d poseGivenDist(double target_dist) {
     Pose2d p1 = m_poseEstimator.getEstimatedPosition();
     Pose2d p2 = SwerveDriveConstants.HUB_POSE;
 
-    double scalar = target_dist/distBtwPoses(p1, p2);
+    double scalar = target_dist / distBtwPoses(p1, p2);
     Pose2d new_pose = new Pose2d(p1.getX() * scalar, p1.getY() * scalar, p1.getRotation());
 
     return new_pose;
@@ -198,6 +215,7 @@ public class SwerveDrive extends SubsystemBase {
 
   /**
    * Gets the current pose of the robot.
+   * 
    * @return Robot's current pose.
    */
   public Pose2d getOdometry() {
@@ -207,6 +225,7 @@ public class SwerveDrive extends SubsystemBase {
 
   /**
    * Gets the current gyro using regression formula.
+   * 
    * @return Rotation2d object holding current gyro in radians
    */
   public Rotation2d getRegGyro() {
@@ -221,8 +240,9 @@ public class SwerveDrive extends SubsystemBase {
     m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
   }
 
-  /** Updates the field relative position of the robot.  
-  */
+  /**
+   * Updates the field relative position of the robot.
+   */
   public void updateOdometry() {
     m_poseEstimator.update( getRegGyro(),
                             modules[0].getState(), 
@@ -243,7 +263,7 @@ public class SwerveDrive extends SubsystemBase {
   /**
    * Resets pigeon.
    */
-  public void resetGyro(){
+  public void resetGyro() {
     m_gyro.reset();
     rotTarget = new Rotation2d(0);
   }
@@ -260,13 +280,13 @@ public class SwerveDrive extends SubsystemBase {
 
   /**
    * Switches speed modes.
+   * 
    * @param shift True if fast mode, false if slow mode.
    */
-  public void highSpeed(boolean shift){
-    if (shift){
+  public void highSpeed(boolean shift) {
+    if (shift) {
       speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST;
-    }
-    else{
+    } else {
       speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
     }
   }

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

@@ -30,7 +30,7 @@ public class SwerveModule extends SubsystemBase {
   private static double kEncoderTicksPerRotation = 4096;
   private SwerveModuleState state;
   private double canCoderFeedbackCoefficient;
-  
+
   public long m_currentTime;
   public long m_lastTime;
   public double m_deltaTime;
@@ -47,21 +47,22 @@ public class SwerveModule extends SubsystemBase {
 
     TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration();
 
-    angleTalonFXConfiguration.slot0.kP = m_swerveGains.m_kP;
-    angleTalonFXConfiguration.slot0.kI = m_swerveGains.m_kI;
-    angleTalonFXConfiguration.slot0.kD = m_swerveGains.m_kD;
+    angleTalonFXConfiguration.slot0.kP = m_swerveGains.kP;
+    angleTalonFXConfiguration.slot0.kI = m_swerveGains.kI;
+    angleTalonFXConfiguration.slot0.kD = m_swerveGains.kD;
 
     // Use the CANCoder as the remote sensor for the primary TalonFX PID
     angleTalonFXConfiguration.remoteFilter0.remoteSensorDeviceID = canCoder.getDeviceID();
     angleTalonFXConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
     angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
     angleMotor.configAllSettings(angleTalonFXConfiguration);
-
+    // angleMotor.setInverted(true);
     // TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
     // driveTalonFXConfiguration.slot0.kP = 0.05;
     // driveTalonFXConfiguration.slot0.kI = 0.0;
     // driveTalonFXConfiguration.slot0.kD = 0.0;
-    // driveTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.IntegratedSensor;
+    // driveTalonFXConfiguration.primaryPID.selectedFeedbackSensor =
+    // FeedbackDevice.IntegratedSensor;
     driveMotor.configFactoryDefault();
     driveMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30);
     driveMotor.configNominalOutputForward(0, 30);
@@ -69,15 +70,17 @@ public class SwerveModule extends SubsystemBase {
     driveMotor.configPeakOutputForward(1, 30);
     driveMotor.configPeakOutputReverse(-1, 30);
     driveMotor.configAllowableClosedloopError(0, 0, 30);
-    driveMotor.config_kP(0, 0.5, 30);
+    // driveMotor.setInverted(true);
+    driveMotor.config_kP(0, 0, 30);
     driveMotor.config_kI(0, 0, 30);
     driveMotor.config_kD(0, 0, 30);
     // maybe try a feedforward value?
-    
+
     // driveMotor.configAllSettings(driveTalonFXConfiguration);
 
     CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
     canCoderConfiguration.magnetOffsetDegrees = offset;
+    canCoderConfiguration.sensorDirection = true;
     canCoder.configAllSettings(canCoderConfiguration);
 
     m_currentTime = System.currentTimeMillis();
@@ -87,40 +90,50 @@ public class SwerveModule extends SubsystemBase {
   }
 
   private Rotation2d getAngle() {
-    // Note: This assumes the CANCoders are setup with the default feedback coefficient
+    // Note: This assumes the CANCoders are setup with the default feedback
+    // coefficient
     // and the sensor value reports degrees.
     return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
   }
 
   /**
    * Set the speed + rotation of the swerve module from a SwerveModuleState object
-   * @param desiredState - A SwerveModuleState representing the desired new state of the module
+   * 
+   * @param desiredState - A SwerveModuleState representing the desired new state
+   *                     of the module
    */
   public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
     Rotation2d currentRotation = getAngle();
-    // SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
+    // SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(),
+    // currentRotation.getDegrees());
     state = SwerveModuleState.optimize(desiredState, currentRotation);
 
-    // Find the difference between our current rotational position + our new rotational position
+    // Find the difference between our current rotational position + our new
+    // rotational position
     Rotation2d rotationDelta = state.angle.minus(currentRotation);
 
-    // Find the new absolute position of the module based on the difference in rotation
+    // Find the new absolute position of the module based on the difference in
+    // rotation
     double deltaTicks = (rotationDelta.getDegrees() / 360.) * kEncoderTicksPerRotation;
     // Convert the CANCoder from it's position reading back to ticks
     double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient;
     double desiredTicks = currentTicks + deltaTicks;
 
-    if (!ignoreAngle){
+    if (!ignoreAngle) {
       angleMotor.set(TalonFXControlMode.Position, desiredTicks);
     }
 
+    // Please work
     double ftPerSec = Units.metersToFeet(state.speedMetersPerSecond);
     double normFtPerSec = ftPerSec / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC;
     // double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69;
-    
-    // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
+
+    // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection +
+    // (Units.metersToInches(state.speedMetersPerSecond) *
+    // SwerveDriveConstants.TICKS_PER_INCH) / 10);
     driveMotor.set(normFtPerSec);// - angleMotor.get());
-    // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio between axis = 1/1.75    Ratio of wheel is 5.14/1    ratio of steer is 12.8/1
+    // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio
+    // between axis = 1/1.75 Ratio of wheel is 5.14/1 ratio of steer is 12.8/1
 
     // m_currentTime = System.currentTimeMillis();
     // m_deltaTime = (double) (m_currentTime - m_lastTime);
@@ -129,10 +142,14 @@ public class SwerveModule extends SubsystemBase {
     // m_currentPos = driveMotor.getSelectedSensorPosition();
 
     // double m_desiredCorrectionVel = 3.2 * angleMotor.getSelectedSensorVelocity();
-    // double m_desiredCorrectionPos = (m_deltaTime * m_desiredCorrectionVel) % 2048;
-    // double m_lastPos = (driveMotor.getSelectedSensorPosition() % 2048) - (m_deltaTime * driveMotor.getSelectedSensorVelocity());
-    // double m_actualDesiredPos = m_deltaTime * ((Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
-    
+    // double m_desiredCorrectionPos = (m_deltaTime * m_desiredCorrectionVel) %
+    // 2048;
+    // double m_lastPos = (driveMotor.getSelectedSensorPosition() % 2048) -
+    // (m_deltaTime * driveMotor.getSelectedSensorVelocity());
+    // double m_actualDesiredPos = m_deltaTime *
+    // ((Units.metersToInches(state.speedMetersPerSecond) *
+    // SwerveDriveConstants.TICKS_PER_INCH) / 10);
+
     // System.out.println("Current Pos: " + driveMotor.getSelectedSensorPosition());
     // System.out.println("Desired Correction Pos: " + m_desiredCorrectionPos);
     // System.out.println("Last Pos: " + m_lastPos);
@@ -150,7 +167,8 @@ public class SwerveModule extends SubsystemBase {
    */
   public SwerveModuleState getState() {
     // return state;
-    return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
+    return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK
+        * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
   }
 
   /**
@@ -160,11 +178,18 @@ public class SwerveModule extends SubsystemBase {
     driveMotor.set(0);
     angleMotor.set(0);
   }
+
   @Override
-  public void periodic(){
+  public void periodic() {
     Rotation2d currentRotation = getAngle();
     SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
-    SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(), ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360);
+    SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(),
+        ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360);
+  }
+
+  public void reset() {
+    canCoder.setPositionToAbsolute();
+    // canCoder.configSensorInitializationStrategy(initializationStrategy)
   }
 
 }

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

@@ -63,12 +63,12 @@ public class Turret extends SubsystemBase {
 
     m_boomBoomRotateMotor.setInverted(false);
 
-    m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP);
-    m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI);
-    m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD);
-    m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF);
-    m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone);
-    m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput);
+    m_boomBoomRotatePIDController.setP(m_shooterTGains.kP);
+    m_boomBoomRotatePIDController.setI(m_shooterTGains.kI);
+    m_boomBoomRotatePIDController.setD(m_shooterTGains.kD);
+    m_boomBoomRotatePIDController.setFF(m_shooterTGains.kF);
+    m_boomBoomRotatePIDController.setIZone(m_shooterTGains.kIzone);
+    m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.kPeakOutput);
   }
 
   @Override