removed updateOdometryInputs (might wanna reconsider idk), cleaned up…

… swerve code a bit. this boutta break the code
TitaniumTigers4829 · Oct 29, 2024 · 5586e6a · 5586e6a
1 parent 71a47d3
commit 5586e6a
Show file tree

Hide file tree

Showing 8 changed files with 55 additions and 125 deletions.
diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java
@@ -1,19 +1,17 @@
 package frc.robot;
 
-/**
- * Automatically generated file containing build version information.
- */
+/** Automatically generated file containing build version information. */
 public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
-  public static final String MAVEN_NAME = "4829-BaseRobotCode-2025-1";
+  public static final String MAVEN_NAME = "4829-BaseRobotCode-1";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 25;
-  public static final String GIT_SHA = "3b5012c7524895e7c511a707a351c6a1f9225018";
-  public static final String GIT_DATE = "2024-10-28 14:52:32 EDT";
+  public static final int GIT_REVISION = 32;
+  public static final String GIT_SHA = "71a47d311fd066ba3b76be8d09eff3e03b893cd8";
+  public static final String GIT_DATE = "2024-10-28 22:21:12 EDT";
   public static final String GIT_BRANCH = "docs";
-  public static final String BUILD_DATE = "2024-10-28 14:53:10 EDT";
-  public static final long BUILD_UNIX_TIME = 1730141590401L;
+  public static final String BUILD_DATE = "2024-10-29 16:56:45 EDT";
+  public static final long BUILD_UNIX_TIME = 1730235405242L;
   public static final int DIRTY = 0;
 
-  private BuildConstants(){}
+  private BuildConstants() {}
 }
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -22,8 +22,8 @@ public static enum Mode {
   }
 
   /**
-   * This is where we place constants related to hardware on a robot that aren't
-   * specific to any singular subsystem.
+   * This is where we place constants related to hardware on a robot that aren't specific to any
+   * singular subsystem.
    */
   public static final class HardwareConstants {
     public static final double TIMEOUT_S = 0.02;
@@ -34,21 +34,20 @@ public static final class HardwareConstants {
     public static final String RIO_CAN_BUS_STRING = "rio";
 
     /**
-     * For some reason, falcons normally have a deadband threshold of 4%. This is
-     * incredibly high! It makes it very hard to do precise movements, so with this
-     * constant we set the threshold to the lowest possible value.
+     * For some reason, falcons normally have a deadband threshold of 4%. This is incredibly high!
+     * It makes it very hard to do precise movements, so with this constant we set the threshold to
+     * the lowest possible value.
      */
     public static final double MIN_FALCON_DEADBAND = 0.001;
-
   }
 
   /**
-   * This is where constants used to describe the game's field go. This will have the
-   * dimensions of the field, but also the coordinates of obstacles, game pieces, or other
-   * places of interest.
+   * This is where constants used to describe the game's field go. This will have the dimensions of
+   * the field, but also the coordinates of obstacles, game pieces, or other places of interest.
    */
   public static final class FieldConstants {
-    // TODO: Now that I think about it, I'm pretty sure these measurements stay the same every year, so consider setting them in the base code
+    // TODO: Now that I think about it, I'm pretty sure these measurements stay the same every year,
+    // so consider setting them in the base code
     public static final double FIELD_LENGTH_METERS = Units.inchesToMeters(0 - 9);
     public static final double FIELD_WIDTH_METERS = Units.inchesToMeters(0 - 9);
   }

diff --git a/src/main/java/frc/robot/commands/drive/DriveCommandBase.java b/src/main/java/frc/robot/commands/drive/DriveCommandBase.java
@@ -23,6 +23,7 @@ public abstract class DriveCommandBase extends Command {
 
   /**
    * An abstract class that handles pose estimation while driving.
+   *
    * @param driveSubsystem The subsystem for the swerve drive
    * @param vision The subsystem for vision measurements
    */
@@ -56,7 +57,8 @@ public void calculatePoseFromLimelight(int limelightNumber) {
       // Depending on how many april tags we see, we change our confidence as more april tags
       // results in a much more accurate pose estimate
       // TODO: check if this is necessary anymore with MT2, also we might want to set the limelight
-      //  so it only uses 1 april tag, if they set up the field wrong (they can set april tags +-1 inch I believe)
+      //  so it only uses 1 april tag, if they set up the field wrong (they can set april tags +-1
+      // inch I believe)
       //  using multiple *could* really mess things up.
       if (vision.getNumberOfAprilTags(limelightNumber) == 1) {
         double[] standardDeviations =

diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java
@@ -41,29 +41,19 @@ public void updateOdometryInputs() {
   }
 
   @Override
-  public void periodic() {
-    updateOdometryPositions();
-  }
+  public void periodic() {}
 
   public void setVoltage(Voltage volts) {
     io.setDriveVoltage(volts);
     io.setTurnVoltage(Volts.zero());
   }
 
   public double getDriveVoltage() {
-    return io.getDriveVoltage();
+    return inputs.driveAppliedVolts;
   }
 
   public double getCharacterizationVelocity() {
-    return io.getDriveVelocity();
-  }
-
-  private void updateOdometryPositions() {
-    odometryPositions = new SwerveModulePosition[inputs.odometryDriveWheelRevolutions.length];
-    for (int i = 0; i < odometryPositions.length; i++) {
-      odometryPositions[i] =
-          new SwerveModulePosition(getPosition().distanceMeters, getPosition().angle);
-    }
+    return inputs.driveVelocity;
   }
 
   /** Runs the module with the specified setpoint state. Returns the optimized state. */
@@ -76,8 +66,8 @@ public Rotation2d getTurnRotation() {
     return inputs.turnAbsolutePosition;
   }
 
-  public double getSteerVelocityRadPerSec() {
-    return inputs.turnVelocityRadPerSec;
+  public double getTurnVelocity() {
+    return inputs.turnVelocity;
   }
 
   /** Returns the current drive position of the module in meters. */

diff --git a/src/main/java/frc/robot/subsystems/swerve/gyroIO/GyroInterface.java b/src/main/java/frc/robot/subsystems/swerve/gyroIO/GyroInterface.java
@@ -18,6 +18,7 @@ public static class GyroInputs {
 
   /**
    * Updates the inputs created in GyroInputs
+   *
    * @param inputs inputs to update
    */
   void updateInputs(GyroInputs inputs);

diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java
@@ -11,22 +11,18 @@ class ModuleInputs {
 
     public boolean isConnected = false;
 
-    public double[] odometryDriveWheelRevolutions = new double[] {};
-
     public double driveVelocity = 0.0;
     public double driveAppliedVolts = 0.0;
     public double driveCurrentAmps = 0.0;
     public double drivePosition = 0.0;
 
     public Rotation2d turnAbsolutePosition = new Rotation2d();
-    public double turnVelocityRadPerSec = 0.0;
+    public double turnVelocity = 0.0;
     public double turnAppliedVolts = 0.0;
     public double turnCurrentAmps = 0.0;
 
     public double[] odometryTimestamps = new double[] {};
-    public double[] odometryDrivePositionsRad = new double[] {};
-    public Rotation2d[] odometryTurnPositions = new Rotation2d[] {};
-
+
     public double turnPosition = 0.0;
   }
 
@@ -44,20 +40,11 @@ class ModuleInputs {
    */
   void setDesiredState(SwerveModuleState desiredState);
 
-  // TODO: all of these methods need to be commented
-  double getDriveVelocity();
-
-  double getTurnAbsolutePosition();
-
   void setDriveVoltage(Voltage voltage);
 
   void setTurnVoltage(Voltage voltage);
 
   void stopModule();
 
   double getTurnRotations();
-
-  double getDriveVoltage();
-
-  double getDrivePosition();
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java
@@ -16,7 +16,6 @@
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
@@ -117,8 +116,7 @@ public PhysicalModule(ModuleConfig moduleConfig) {
           driveMotorCurrent,
           turnEncoderVelocity,
           turnMotorAppliedVolts,
-          turnMotorCurrent,
-          turnMotor.getPosition()
+          turnMotorCurrent
         };
 
     driveMotor.setPosition(0.0);
@@ -134,44 +132,39 @@ public void updateInputs(ModuleInputs inputs) {
     inputs.isConnected = BaseStatusSignal.isAllGood(periodicallyRefreshedSignals);
 
     inputs.driveVelocity = driveVelocity.getValueAsDouble();
-    inputs.odometryDriveWheelRevolutions =
-        drivePosition.stream()
-            .mapToDouble(value -> value.in(Rotations) / ModuleConstants.DRIVE_GEAR_RATIO)
-            .toArray();
-    drivePosition.clear();
-    if (inputs.odometryDriveWheelRevolutions.length > 0)
-      inputs.drivePosition =
-          inputs.odometryDriveWheelRevolutions[inputs.odometryDriveWheelRevolutions.length - 1];
-
-
-    // Handle odometry yaw positions
+
+    // Handle drive positions
+    if (!drivePosition.isEmpty()) {
+      Angle driveRelativePosition = Rotations.zero();
+      for (Angle angle : drivePosition) {
+        driveRelativePosition = angle;
+      }
+      inputs.drivePosition = driveRelativePosition.in(Rotations);
+      drivePosition.clear();
+    }
+
+    // Handle turn absolute positions
     if (!turnEncoderAbsolutePosition.isEmpty()) {
-      Rotation2d odometryYawPositions = new Rotation2d();
+      Rotation2d turnPosition = new Rotation2d();
       for (Angle angle : turnEncoderAbsolutePosition) {
-        odometryYawPositions = Rotation2d.fromRotations(angle.in(Rotations));
+        turnPosition = Rotation2d.fromRotations(angle.in(Rotations));
       }
-      inputs.turnAbsolutePosition = odometryYawPositions;
+      inputs.turnAbsolutePosition = turnPosition;
       turnEncoderAbsolutePosition.clear();
     }
 
-    if (inputs.odometryTurnPositions.length > 0)
-      inputs.turnPosition = inputs.odometryTurnPositions[inputs.odometryTurnPositions.length - 1].getRotations();
+    inputs.turnPosition = turnMotor.getPosition().getValueAsDouble();
 
     inputs.driveAppliedVolts = driveMotorAppliedVoltage.getValueAsDouble();
     inputs.driveCurrentAmps = driveMotorCurrent.getValueAsDouble();
 
     inputs.turnPosition = turnMotor.getPosition().getValueAsDouble();
 
-    inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnEncoderVelocity.getValueAsDouble());
+    inputs.turnVelocity = turnEncoderVelocity.getValueAsDouble();
     inputs.turnAppliedVolts = turnMotorAppliedVolts.getValueAsDouble();
     inputs.turnCurrentAmps = turnMotorCurrent.getValueAsDouble();
   }
 
-  @Override
-  public double getDriveVelocity() {
-    return driveVelocity.refresh().getValueAsDouble();
-  }
-
   @Override
   public void setDriveVoltage(Voltage volts) {
     driveMotor.setControl(voltageOut.withOutput(volts));
@@ -182,16 +175,6 @@ public void setTurnVoltage(Voltage volts) {
     turnMotor.setControl(voltageOut.withOutput(volts));
   }
 
-  @Override
-  public double getTurnAbsolutePosition() {
-    return turnEncoder.getAbsolutePosition().refresh().getValueAsDouble();
-  }
-
-  @Override
-  public double getDriveVoltage() {
-    return driveMotorAppliedVoltage.refresh().getValueAsDouble();
-  }
-
   @Override
   public void setDesiredState(SwerveModuleState desiredState) {
     double turnRotations = getTurnRotations();
@@ -230,9 +213,4 @@ public void stopModule() {
     driveMotor.stopMotor();
     turnMotor.stopMotor();
   }
-
-  @Override
-  public double getDrivePosition() {
-    return driveMotor.getPosition().refresh().getValueAsDouble();
-  }
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java
@@ -13,7 +13,6 @@
 import frc.robot.extras.simulation.OdometryTimestampsSim;
 import frc.robot.extras.simulation.mechanismSim.swerve.SwerveModuleSimulation;
 import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants;
-import java.util.Arrays;
 
 /** Wrapper class around {@link SwerveModuleSimulation} */
 public class SimulatedModule implements ModuleInterface {
@@ -32,54 +31,35 @@ public SimulatedModule(SwerveModuleSimulation moduleSimulation) {
   @Override
   public void updateInputs(ModuleInputs inputs) {
     inputs.drivePosition =
-        Units.radiansToRotations(moduleSimulation.getDriveEncoderFinalPositionRad());
+        Radians.of(moduleSimulation.getDriveEncoderFinalPositionRad()).in(Rotations);
     inputs.driveVelocity =
-        Units.radiansToRotations(moduleSimulation.getDriveWheelFinalSpeedRadPerSec());
+        RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec())
+            .in(RotationsPerSecond);
     inputs.driveAppliedVolts = moduleSimulation.getDriveMotorAppliedVolts();
     inputs.driveCurrentAmps = Math.abs(moduleSimulation.getDriveMotorSupplyCurrentAmps());
-    
+
     // inputs.turnAbsolutePosition = moduleSimulation.getTurnAbsolutePosition();
-    inputs.turnPosition = moduleSimulation.getTurnRelativeEncoderPositionRad();
-    inputs.turnVelocityRadPerSec = moduleSimulation.getTurnRelativeEncoderSpeedRadPerSec();
+    inputs.turnPosition =
+        Radians.of(moduleSimulation.getTurnRelativeEncoderPositionRad()).in(Rotations);
+    inputs.turnVelocity =
+        RadiansPerSecond.of(moduleSimulation.getTurnRelativeEncoderSpeedRadPerSec())
+            .in(RotationsPerSecond);
     inputs.turnAppliedVolts = moduleSimulation.getTurnMotorAppliedVolts();
     inputs.turnCurrentAmps = Math.abs(moduleSimulation.getTurnMotorSupplyCurrentAmps());
 
     inputs.odometryTimestamps = OdometryTimestampsSim.getTimestamps();
-    inputs.odometryDrivePositionsRad = moduleSimulation.getCachedDriveWheelFinalPositionsRad();
-    inputs.odometryTurnPositions = moduleSimulation.getCachedTurnAbsolutePositions();
-
-    inputs.odometryDriveWheelRevolutions =
-        Arrays.stream(moduleSimulation.getCachedDriveWheelFinalPositionsRad())
-            .map(Units::radiansToRotations)
-            .toArray();
   }
 
   @Override
   public void setDriveVoltage(Voltage volts) {
     moduleSimulation.requestDriveVoltageOut(volts);
   }
 
-  @Override
-  public double getDriveVelocity() {
-    return RadiansPerSecond.of(moduleSimulation.getDriveWheelFinalSpeedRadPerSec())
-        .in(RotationsPerSecond);
-  }
-
-  @Override
-  public double getTurnAbsolutePosition() {
-    return moduleSimulation.getTurnAbsolutePosition().getRotations();
-  }
-
   @Override
   public void setTurnVoltage(Voltage volts) {
     moduleSimulation.requestTurnVoltageOut(volts);
   }
 
-  @Override
-  public double getDrivePosition() {
-    return Radians.of(moduleSimulation.getDriveEncoderFinalPositionRad()).in(Degrees);
-  }
-
   public void setDesiredState(SwerveModuleState desiredState) {
     double turnRotations = getTurnRotations();
     // Optimize the reference state to avoid spinning further than 90 degrees
@@ -124,9 +104,4 @@ public void stopModule() {
     moduleSimulation.requestDriveVoltageOut(Volts.zero());
     moduleSimulation.requestTurnVoltageOut(Volts.zero());
   }
-
-  @Override
-  public double getDriveVoltage() {
-    return moduleSimulation.getDriveMotorAppliedVolts();
-  }
 }
-Original file line number
+Diff line change
@@ Expand Up / @@ -18,6 +18,7 @@ public static class GyroInputs { @@
       /**
        * Updates the inputs created in GyroInputs
+       *
        * @param inputs inputs to update
        */
       void updateInputs(GyroInputs inputs);
@@ Expand Down @@