From bc9334fa094b2ffd572ee6455a7022d9a99c0a43 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Mon, 24 Jun 2024 20:01:29 -0400 Subject: [PATCH] drive revamp finished --- .../com/team4099/utils/threads/PhoenixOdometryThread.java | 4 ++-- .../com/team4099/utils/threads/SparkMaxOdometryThread.java | 2 +- .../commands/drivetrain/SwerveModuleTuningCommand.kt | 2 +- .../robot2023/config/constants/DrivetrainConstants.kt | 2 +- .../robot2023/subsystems/drivetrain/drive/Drivetrain.kt | 5 +++-- .../subsystems/drivetrain/swervemodule/SwerveModule.kt | 2 +- .../subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt | 2 +- 7 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/com/team4099/utils/threads/PhoenixOdometryThread.java b/src/main/java/com/team4099/utils/threads/PhoenixOdometryThread.java index 441e8a50..e4cf985b 100644 --- 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 index 2edc2304..666639a7 100644 --- 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 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 index 819c8175..6f0403e0 100644 --- 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 index e7acdf01..6056cc10 100644 --- 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 index 52dc786d..1bb813fe 100644 --- 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 index 67bd563a..4346c2f6 100644 --- 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/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt index e25a8e78..031cb932 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt +++ b/src/main/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) }