am i dumb?

TitaniumTigers4829 · Oct 28, 2024 · 4c386b8 · 4c386b8
1 parent e10337d
commit 4c386b8
Show file tree

Hide file tree

Showing 3 changed files with 54 additions and 62 deletions.
diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/ModuleInterface.java
@@ -14,7 +14,7 @@ class ModuleInputs {
     public double steerMotorAppliedVolts = 0.0;
     public double steerMotorCurrentAmps = 0.0;
 
-    public double[] odometryDriveWheelRevolutions = new double[] {};
+    public double[] odometryDriveWheelRevolutions = new double[];;
 
     public double driveVelocity = 0.0;
     public double driveAppliedVolts = 0.0;
@@ -24,25 +24,26 @@ class ModuleInputs {
     public double turnVelocityRadPerSec = 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[] odometryTimestamps = new double[];;
+    public double[] odometryDrivePositionsRad = new double[];;
+    // public Rotation2d[] odometryTurnPositions = new Rotation2d[];;
     public double driveWheelFinalVelocityPerSec = 0.0;
     public double turnMotorAppliedVolts = 0.0;
     public double turnMotorCurrentAmps = 0.0;
     public double turnPosition = 0.0;
   }
 
   /** Updates the inputs */
-  default void updateInputs(ModuleInputs inputs) {}
+  void updateInputs(ModuleInputs inputs);
 
-  default String getCANBus() {
-    return "";
-  }
-
-  default void calibrate() {}
+  String getCANBus();
 
-  default void setDesiredState(SwerveModuleState desiredState) {}
+  /**
+   * Sets the desired state for the module and sends calculated output from controller to the motor.
+   *
+   * @param desiredState Desired state with speed and angle.
+   */
+  void setDesiredState(SwerveModuleState desiredState);
 
   double getDriveVelocity();
 
@@ -55,34 +56,30 @@ default void setDesiredState(SwerveModuleState desiredState) {}
    *
    * @param speedPercent from -1 to 1, where 1 is the forward direction of the wheel
    */
-  default void setDriveSpeed(double speedPercent) {}
+  void setDriveSpeed(double speedPercent);
 
   /**
    * Run the turn motor at the specified percent power.
    *
    * @param powerPercent from -1 to 1, where 1 is counter-clockwise
    */
-  default void setTurnSpeed(double powerPercent) {}
+  void setTurnSpeed(double powerPercent);
 
   /** Enable or disable brake mode on the drive motor. */
-  default void setDriveBrake(boolean enable) {}
+  void setDriveBrake(boolean enable);
 
   /** Enable or disable brake mode on the turn motor. */
-  default void setTurnBrake(boolean enable) {}
+  void setTurnBrake(boolean enable);
 
-  default void setDriveVoltage(double voltage) {}
+  void setDriveVoltage(double voltage);
 
-  default void setTurnVoltage(double voltage) {}
+  void setTurnVoltage(double voltage);
 
-  default void stopModule() {}
+  void stopModule();
 
-  default double getTurnRotations() {
-    return 0.0;
-  }
+  double getTurnRotations();
 
-  default double getDriveVoltage() {
-    return 0.0;
-  }
+  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
@@ -200,11 +200,6 @@ public double getDriveVoltage() {
     return driveMotorAppliedVoltage.refresh().getValueAsDouble();
   }
 
-  /**
-   * Sets the desired state for the module and sends calculated output from controller to the motor.
-   *
-   * @param desiredState Desired state with speed and angle.
-   */
   @Override
   public void setDesiredState(SwerveModuleState desiredState) {
     double turnRadians = getTurnRotations();

diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java
@@ -83,37 +83,37 @@ public double getDrivePosition() {
     throw new UnsupportedOperationException("Unimplemented method 'getDrivePosition'");
   }
 
-  // public void setDesiredState(SwerveModuleState desiredState) {
-  //   Rotation2d turnRotations = getTurnRotations();
-  //   // Optimize the reference state to avoid spinning further than 90 degrees
-  //   SwerveModuleState optimizedDesiredState =
-  //       SwerveModuleState.optimize(desiredState, new Rotation2d(turnRotations));
-
-  //   if (Math.abs(optimizedDesiredState.speedMetersPerSecond) < 0.01) {
-  //     moduleSimulation.requestDriveVoltageOut(0);
-  //     moduleSimulation.requestTurnVoltageOut(0);
-  //     return;
-  //   }
-
-  //   // Converts meters per second to rotations per second
-  //   double desiredDriveRPS =
-  //       optimizedDesiredState.speedMetersPerSecond
-  //           * ModuleConstants.DRIVE_GEAR_RATIO
-  //           / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS;
-
-  //   moduleSimulation.requestDriveVoltageOut(
-  //       drivePID.calculate(
-  //               Units.radiansToRotations(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()),
-  //               desiredDriveRPS)
-  //           + driveFF.calculate(desiredDriveRPS));
-  //   moduleSimulation.requestTurnVoltageOut(
-  //       turnPID.calculate(
-  //               moduleSimulation.getTurnAbsolutePosition().getRotations(),
-  //               desiredState.angle.getRotations())
-  //           + turnFF.calculate(turnPID.getSetpoint().velocity));
-  // }
-
-  // public Rotation2d getTurnRotations() {
-  //   return new Ro(moduleSimulation.getTurnAbsolutePosition()).getRadians();
-  // }
+  public void setDesiredState(SwerveModuleState desiredState) {
+    Rotation2d turnRotations = getTurnRotations();
+    // Optimize the reference state to avoid spinning further than 90 degrees
+    SwerveModuleState optimizedDesiredState =
+        SwerveModuleState.optimize(desiredState, turnRotations);
+
+    if (Math.abs(optimizedDesiredState.speedMetersPerSecond) < 0.01) {
+      moduleSimulation.requestDriveVoltageOut(0);
+      moduleSimulation.requestTurnVoltageOut(0);
+      return;
+    }
+
+    // Converts meters per second to rotations per second
+    double desiredDriveRPS =
+        optimizedDesiredState.speedMetersPerSecond
+            * ModuleConstants.DRIVE_GEAR_RATIO
+            / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS;
+
+    moduleSimulation.requestDriveVoltageOut(
+        drivePID.calculate(
+                Units.radiansToRotations(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()),
+                desiredDriveRPS)
+            + driveFF.calculate(desiredDriveRPS));
+    moduleSimulation.requestTurnVoltageOut(
+        turnPID.calculate(
+                moduleSimulation.getTurnAbsolutePosition().getRotations(),
+                desiredState.angle.getRotations())
+            + turnFF.calculate(turnPID.getSetpoint().velocity));
+  }
+
+  public Rotation2d getTurnRotations() {
+    return Rotation2d.fromRotations(moduleSimulation.getTurnAbsolutePosition());
+  }
 }