bruh

TitaniumTigers4829 · Oct 28, 2024 · 9dc17b5 · 9dc17b5
1 parent 92bd17f
commit 9dc17b5
Show file tree

Hide file tree

Showing 2 changed files with 81 additions and 14 deletions.
diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/PhysicalModule.java
@@ -202,13 +202,13 @@ public double getDriveVoltage() {
 
   @Override
   public void setDesiredState(SwerveModuleState desiredState) {
-    double turnRadians = getTurnRotations();
+    double turnRotations = getTurnRotations();
     // Optimize the reference state to avoid spinning further than 90 degrees
     SwerveModuleState setpoint =
         new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
 
-    setpoint.optimize(Rotation2d.fromRotations(turnRadians));
-    setpoint.cosineScale(Rotation2d.fromRotations(turnRadians));
+    setpoint.optimize(Rotation2d.fromRotations(turnRotations));
+    setpoint.cosineScale(Rotation2d.fromRotations(turnRotations));
 
     if (Math.abs(setpoint.speedMetersPerSecond) < 0.01) {
       driveMotor.set(0);
@@ -252,4 +252,22 @@ public void stopModule() {
   public double getDrivePosition() {
     return driveMotor.getPosition().refresh().getValueAsDouble();
   }
+
+  @Override
+  public String getCANBus() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'getCANBus'");
+  }
+
+  @Override
+  public void setDriveSpeed(double speedPercent) {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'setDriveSpeed'");
+  }
+
+  @Override
+  public void setTurnSpeed(double powerPercent) {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'setTurnSpeed'");
+  }
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java b/src/main/java/frc/robot/subsystems/swerve/moduleIO/SimulatedModule.java
@@ -3,10 +3,14 @@
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.math.util.Units;
 import frc.robot.extras.simulation.OdometryTimestampsSim;
 import frc.robot.extras.simulation.mechanismSim.swervePhysicsSim.SwerveModuleSimulation;
+import frc.robot.subsystems.swerve.SwerveConstants.ModuleConstants;
+
 import java.util.Arrays;
 
 /** Wrapper class around {@link SwerveModuleSimulation} that implements ModuleIO */
@@ -84,36 +88,81 @@ public double getDrivePosition() {
   }
 
   public void setDesiredState(SwerveModuleState desiredState) {
-    Rotation2d turnRotations = getTurnRotations();
+    double turnRotations = getTurnRotations();
     // Optimize the reference state to avoid spinning further than 90 degrees
-    SwerveModuleState optimizedDesiredState =
-        SwerveModuleState.optimize(desiredState, turnRotations);
+    SwerveModuleState setpoint =
+        new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
+
+    setpoint.optimize(Rotation2d.fromRotations(turnRotations));
+    setpoint.cosineScale(Rotation2d.fromRotations(turnRotations));
 
-    if (Math.abs(optimizedDesiredState.speedMetersPerSecond) < 0.01) {
+    if (Math.abs(setpoint.speedMetersPerSecond) < 0.01) {
       moduleSimulation.requestDriveVoltageOut(0);
       moduleSimulation.requestTurnVoltageOut(0);
       return;
     }
 
     // Converts meters per second to rotations per second
     double desiredDriveRPS =
-        optimizedDesiredState.speedMetersPerSecond
+        setpoint.speedMetersPerSecond
             * ModuleConstants.DRIVE_GEAR_RATIO
             / ModuleConstants.WHEEL_CIRCUMFERENCE_METERS;
 
     moduleSimulation.requestDriveVoltageOut(
         drivePID.calculate(
                 Units.radiansToRotations(moduleSimulation.getDriveWheelFinalSpeedRadPerSec()),
-                desiredDriveRPS)
-            + driveFF.calculate(desiredDriveRPS));
+                desiredDriveRPS));
+            // + driveFF.calculate(desiredDriveRPS));
     moduleSimulation.requestTurnVoltageOut(
         turnPID.calculate(
                 moduleSimulation.getTurnAbsolutePosition().getRotations(),
-                desiredState.angle.getRotations())
-            + turnFF.calculate(turnPID.getSetpoint().velocity));
+                desiredState.angle.getRotations()));
+            // + turnFF.calculate(turnPID.getSetpoint().velocity));
+  }
+
+  public double getTurnRotations() {
+    return moduleSimulation.getTurnAbsolutePosition().getRotations();
+  }
+
+  @Override
+  public String getCANBus() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'getCANBus'");
+  }
+
+  @Override
+  public void setDriveSpeed(double speedPercent) {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'setDriveSpeed'");
+  }
+
+  @Override
+  public void setTurnSpeed(double powerPercent) {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'setTurnSpeed'");
+  }
+
+  @Override
+  public void setDriveBrake(boolean enable) {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'setDriveBrake'");
   }
 
-  public Rotation2d getTurnRotations() {
-    return Rotation2d.fromRotations(moduleSimulation.getTurnAbsolutePosition());
+  @Override
+  public void setTurnBrake(boolean enable) {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'setTurnBrake'");
+  }
+
+  @Override
+  public void stopModule() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'stopModule'");
+  }
+
+  @Override
+  public double getDriveVoltage() {
+    // TODO Auto-generated method stub
+    throw new UnsupportedOperationException("Unimplemented method 'getDriveVoltage'");
   }
 }