fix multiple fuck ups

TitaniumTigers4829 · Sep 11, 2024 · a36a2d2 · a36a2d2
1 parent 1eea666
commit a36a2d2
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 13 deletions.
diff --git a/src/main/java/frc/robot/subsystems/swerve/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/DriveSubsystem.java
@@ -22,9 +22,7 @@
 import frc.robot.Constants.TrajectoryConstants;
 import frc.robot.Constants.VisionConstants;
 import frc.robot.extras.SmarterDashboardRegistry;
-import frc.robot.extras.swerve.ModuleLimits;
-import frc.robot.extras.swerve.SwerveSetpoint;
-import frc.robot.extras.swerve.SwerveSetpointGenerator;
+import frc.robot.subsystems.swerve.SwerveSetpointGenerator;
 import java.util.Optional;
 
 public class DriveSubsystem extends SubsystemBase {
@@ -53,7 +51,7 @@ public class DriveSubsystem extends SubsystemBase {
   private final AHRS gyro;
   private final SwerveDrivePoseEstimator odometry;
 
-  private SwerveSetpointGenerator SwerveSetpointProcessor;
+  private SwerveSetpointGenerator swerveSetpointGenerator;
 
   private Optional<DriverStation.Alliance> alliance;
 
@@ -116,7 +114,7 @@ public DriveSubsystem() {
             // resetOdometry() to set it in autonomous init
             stateStandardDeviations,
             visionMeasurementStandardDeviations);
-//comit plz
+
     alliance = DriverStation.getAlliance();
 
     setpointGenerator = new SwerveSetpointGenerator();

diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSetpointGenerator.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSetpointGenerator.java
@@ -11,6 +11,8 @@
 import java.util.Optional;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.ModuleConstants;
+import frc.robot.extras.utils.EqualsUtil;
+import frc.robot.extras.utils.GeomUtil;
 
 /**
  * "Inspired" by FRC team 254.
@@ -210,7 +212,7 @@ public SwerveSetpoint generateSetpoint(ChassisSpeeds desiredState) {
         if (disabled) {
             return new SwerveSetpoint(
                     desiredState,
-                    kinematics.toSwerveModuleStates(desiredState));
+                    swerveDriveKinematics.toSwerveModuleStates(desiredState));
         }
             return generateSetpointInner(desiredState, ConstValues.PERIODIC_TIME);
     }
@@ -221,12 +223,12 @@ private SwerveSetpoint generateSetpointInner(
     ) {
         final int moduleCount = moduleLocations.length;
 
-        SwerveModuleState[] desiredModuleState = kinematics.toSwerveModuleStates(desiredState);
+        SwerveModuleState[] desiredModuleState = swerveDriveKinematics.toSwerveModuleStates(desiredState);
 
         // Make sure desiredState respects velocity limits.
-        if (limits.maxDriveVelocity() > 0.0) {
-            SwerveDriveKinematics.desaturateWheelSpeeds(desiredModuleState, limits.maxDriveVelocity());
-            desiredState = kinematics.toChassisSpeeds(desiredModuleState);
+        if (moduleLimits.maxDriveVelocity() > 0.0) {
+            swerveDriveKinematics.desaturateWheelSpeeds(desiredModuleState, moduleLimits.maxDriveVelocity());
+            desiredState = swerveDriveKinematics.toChassisSpeeds(desiredModuleState);
         }
 
         // Special case: desiredState is a complete stop. In this case, module angle is
@@ -314,7 +316,7 @@ private SwerveSetpoint generateSetpointInner(
         // We remember the
         // minimum across all modules, since
         // that is the active constraint.
-        final double max_theta_step = dt * limits.maxSteeringVelocity();
+        final double max_theta_step = dt * moduleLimits.maxSteeringVelocity();
 
         for (int i = 0; i < moduleCount; ++i) {
             if (!need_to_steer) {
@@ -384,7 +386,7 @@ private SwerveSetpoint generateSetpointInner(
         }
 
         // Enforce drive wheel acceleration limits.
-        final double max_vel_step = dt * limits.maxDriveAcceleration();
+        final double max_vel_step = dt * moduleLimits.maxDriveAcceleration();
         for (int i = 0; i < moduleCount; ++i) {
             if (min_s == 0.0) {
                 // No need to carry on.
@@ -414,7 +416,7 @@ private SwerveSetpoint generateSetpointInner(
                 prevSetpoint.chassisSpeeds().vxMetersPerSecond + min_s * dx,
                 prevSetpoint.chassisSpeeds().vyMetersPerSecond + min_s * dy,
                 prevSetpoint.chassisSpeeds().omegaRadiansPerSecond + min_s * dtheta);
-        var retStates = kinematics.toSwerveModuleStates(retSpeeds);
+        var retStates = swerveDriveKinematics.toSwerveModuleStates(retSpeeds);
         for (int i = 0; i < moduleCount; ++i) {
             final var maybeOverride = overrideSteering.get(i);
             if (maybeOverride.isPresent()) {