drive revamp finished

team4099 · Jun 25, 2024 · bc9334f · bc9334f
1 parent 301a332
commit bc9334f
Show file tree

Hide file tree

Showing 7 changed files with 10 additions and 9 deletions.
diff --git a/src/main/java/com/team4099/utils/threads/PhoenixOdometryThread.java b/src/main/java/com/team4099/utils/threads/PhoenixOdometryThread.java
@@ -66,13 +66,13 @@ public void run() {
             signalsLock.lock();
             try {
                 if (isCANFD && signals.length > 0) {
-                    com.ctre.phoenix6.BaseStatusSignal.waitForAll(2.0 / com.team4099.robot2023.config.constants.DrivetrainConstants.OMOMETRY_UPDATE_FREQUENCY, signals);
+                    com.ctre.phoenix6.BaseStatusSignal.waitForAll(2.0 / com.team4099.robot2023.config.constants.DrivetrainConstants.ODOMETRY_UPDATE_FREQUENCY, signals);
                 } else {
                     // "waitForAll" does not support blocking on multiple
                     // signals with a bus that is not CAN FD, regardless
                     // of Pro licensing. No reasoning for this behavior
                     // is provided by the documentation.
-                    Thread.sleep((long) (1000.0 / com.team4099.robot2023.config.constants.DrivetrainConstants.OMOMETRY_UPDATE_FREQUENCY));
+                    Thread.sleep((long) (1000.0 / com.team4099.robot2023.config.constants.DrivetrainConstants.ODOMETRY_UPDATE_FREQUENCY));
                     com.ctre.phoenix6.BaseStatusSignal.refreshAll(signals);
                 }
             } catch (InterruptedException e) {

diff --git a/src/main/java/com/team4099/utils/threads/SparkMaxOdometryThread.java b/src/main/java/com/team4099/utils/threads/SparkMaxOdometryThread.java
@@ -35,7 +35,7 @@ public static com.team4099.utils.threads.SparkMaxOdometryThread getInstance() {
     private SparkMaxOdometryThread() {
         notifier = new edu.wpi.first.wpilibj.Notifier(this::periodic);
         notifier.setName("SparkMaxOdometryThread");
-        notifier.startPeriodic(1.0 / com.team4099.robot2023.config.constants.DrivetrainConstants.OMOMETRY_UPDATE_FREQUENCY );
+        notifier.startPeriodic(1.0 / com.team4099.robot2023.config.constants.DrivetrainConstants.ODOMETRY_UPDATE_FREQUENCY);
     }
 
     public java.util.Queue<Double> registerSignal(java.util.function.DoubleSupplier signal) {

diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt
@@ -14,7 +14,7 @@ class SwerveModuleTuningCommand(val drivetrain: Drivetrain, val steeringPosition
 
   override fun execute() {
     for (module in drivetrain.swerveModules) {
-      module.closedLoop(
+      module.setPositionClosedLoop(
         SwerveModuleState(0.0, steeringPosition().inRotation2ds),
         SwerveModuleState(0.0, steeringPosition().inRotation2ds),
         false

diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt
@@ -35,7 +35,7 @@ object DrivetrainConstants {
 
   const val TELEOP_TURNING_SPEED_PERCENT = 0.6
 
-  const val OMOMETRY_UPDATE_FREQUENCY = 250.0
+  const val ODOMETRY_UPDATE_FREQUENCY = 250.0
 
   const val WHEEL_COUNT = 4
   val WHEEL_DIAMETER = (2.083 * 2).inches

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
@@ -494,6 +494,7 @@ class Drivetrain(private val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : Su
       edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0)
   ) {
     var chassisSpeeds = chassisSpeeds
+
     // If theres skew consider slowing down
     if (DrivetrainConstants.MINIMIZE_SKEW) {
       chassisSpeeds =
@@ -519,8 +520,8 @@ class Drivetrain(private val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : Su
     // Once we have all of our states obtained for both velocity and acceleration, apply these
     // states to each swerve module
     for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) {
-      swerveModules[moduleIndex].closedLoop(
-        velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex]
+      swerveModules[moduleIndex].setPositionClosedLoop(
+        accelSwerveModuleStates[moduleIndex], velSwerveModuleStates[moduleIndex]
       )
     }
   }

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt
@@ -216,7 +216,7 @@ class SwerveModule(val io: SwerveModuleIO) {
       )
     }
   }
-  fun closedLoop(
+  fun setPositionClosedLoop(
     desiredAccelState: SwerveModuleState,
     desiredVeloState: SwerveModuleState,
     optimize: Boolean = true

diff --git a/...ain/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt b/...ain/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt
@@ -230,8 +230,8 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO {
   ) {
     Logger.recordOutput("$label/desiredDriveSpeedMPS", speed.inMetersPerSecond)
     val feedforward = driveFeedForward.calculate(speed, acceleration)
-    setDriveVoltage(feedforward + driveFeedback.calculate(driveVelocity, speed))
 
+    setDriveVoltage(feedforward + driveFeedback.calculate(driveVelocity, speed))
     setSteeringSetpoint(steering)
   }