Create NewSewerveModule.kt

Add a new updated swervemodule file

src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/NewSewerveModule.kt

@@ -0,0 +1,188 @@
+// Copyright (c) 2024 FRC 4099
+
+package com.team4099.robot2023.subsystems.drivetrain.swervemodule
+
+import com.team4099.lib.logging.LoggedTunableValue
+import com.team4099.robot2023.config.constants.DrivetrainConstants
+import edu.wpi.first.math.kinematics.SwerveModulePosition
+import edu.wpi.first.wpilibj.RobotBase
+import org.team4099.lib.units.LinearAcceleration
+import org.team4099.lib.units.LinearVelocity
+import org.team4099.lib.units.base.feet
+import org.team4099.lib.units.base.inMeters
+import org.team4099.lib.units.base.meters
+import org.team4099.lib.units.base.seconds
+import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inRotation2ds
+import org.team4099.lib.units.derived.inVoltsPerDegree
+import org.team4099.lib.units.derived.inVoltsPerDegreePerSecond
+import org.team4099.lib.units.derived.inVoltsPerDegreeSeconds
+import org.team4099.lib.units.derived.inVoltsPerMeters
+import org.team4099.lib.units.derived.inVoltsPerMetersPerSecond
+import org.team4099.lib.units.derived.inVoltsPerMetersPerSecondPerSecond
+import org.team4099.lib.units.derived.perDegree
+import org.team4099.lib.units.derived.perDegreePerSecond
+import org.team4099.lib.units.derived.perDegreeSeconds
+import org.team4099.lib.units.derived.perMeterPerSecond
+import org.team4099.lib.units.derived.perMeterPerSecondPerSecond
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.perSecond
+
+class NewSewerveModule (val io: SwerveModuleIO) {
+    inputs = SwerveModuleIO.SwerveModuleIOInputs()
+
+    var modulePosition = SwerveModulePosition()
+
+    var positionDeltas = mutableListOf<SwerveModulePosition>()
+
+    private var speedSetPoint: LinearVelocity = 0.feet.perSecond
+    private var accelerationSetPoint: LinearAcceleration = 0.feet.perSecond.perSecond
+
+    private var steeringSetPoint: Angle = 0.degrees
+
+    private var lastDrivePosition = 0.meters
+
+
+    private val driveKV =
+        LoggedTunableValue(
+            "Drivetrain/kV",
+            DrivetrainConstants.PID.DRIVE_KV,
+            Pair({ it.inVoltsPerMetersPerSecond }, { it.volts.perMeterPerSecond })
+        )
+    private val driveKA =
+        LoggedTunableValue(
+            "Drivetrain/kA",
+            DrivetrainConstants.PID.DRIVE_KA,
+            Pair({ it.inVoltsPerMetersPerSecondPerSecond }, { it.volts.perMeterPerSecondPerSecond })
+        )
+
+    private val steeringkP =
+        LoggedTunableValue(
+            "Drivetrain/moduleSteeringkP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree })
+        )
+    private val steeringkI =
+        LoggedTunableValue(
+            "Drivetrain/moduleSteeringkI",
+            Pair({ it.inVoltsPerDegreeSeconds }, { it.volts.perDegreeSeconds })
+        )
+    private val steeringkD =
+        LoggedTunableValue(
+            "Drivetrain/moduleSteeringkD",
+            Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond })
+        )
+
+    private val steeringMaxVel =
+        LoggedTunableValue(
+            "Drivetrain/moduleSteeringMaxVelRadPerSec", DrivetrainConstants.STEERING_VEL_MAX
+        )
+    private val steeringMaxAccel =
+        LoggedTunableValue(
+            "Drivetrain/moduleSteeringMaxAccelRadPerSecSq", DrivetrainConstants.STEERING_ACCEL_MAX
+        )
+
+    private val drivekP =
+        LoggedTunableValue(
+            "Drivetrain/moduleDrivekP",
+            Pair({ it.inVoltsPerMetersPerSecond }, { it.volts.perMeterPerSecond })
+        )
+
+    private val drivekI =
+        LoggedTunableValue(
+            "Drivetrain/moduleDrivekI",
+            Pair({ it.inVoltsPerMeters }, { it.volts / (1.meters.perSecond * 1.seconds) })
+        )
+
+    private val drivekD =
+        LoggedTunableValue(
+            "Drivetrain/moduleDrivekD",
+            Pair({ it.inVoltsPerMetersPerSecondPerSecond }, { it.volts.perMeterPerSecondPerSecond })
+        )
+
+    init {
+        if (RobotBase.isReal()) {
+            steeringkP.initDefault(DrivetrainConstants.PID.STEERING_KP)
+            steeringkI.initDefault(DrivetrainConstants.PID.STEERING_KI)
+            steeringkD.initDefault(DrivetrainConstants.PID.STEERING_KD)
+
+            drivekP.initDefault(DrivetrainConstants.PID.DRIVE_KP)
+            drivekI.initDefault(DrivetrainConstants.PID.DRIVE_KI)
+            drivekD.initDefault(DrivetrainConstants.PID.DRIVE_KD)
+        } else {
+            steeringkP.initDefault(DrivetrainConstants.PID.SIM_STEERING_KP)
+            steeringkI.initDefault(DrivetrainConstants.PID.SIM_STEERING_KI)
+            steeringkD.initDefault(DrivetrainConstants.PID.SIM_STEERING_KD)
+
+            drivekP.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KP)
+            drivekI.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KI)
+            drivekD.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KD)
+        }
+
+        driveKV.initDefault(DrivetrainConstants.PID.DRIVE_KV)
+        driveKA.initDefault(DrivetrainConstants.PID.DRIVE_KA)
+    }
+
+    fun updateInputs() {
+        io.updateInputs(inputs)
+    }
+
+    fun periodic(){
+        positionDeltas.add(
+            SwerveModulePosition(
+                (inputs.drivePosition - lastDrivePosition).inMeters,
+                inputs.steeringPosition.inRotation2ds
+            )
+        )
+        lastDrivePosition = inputs.drivePosition
+
+        // Updating SwerveModulePosition every loop cycle
+        modulePosition.distanceMeters = inputs.drivePosition.inMeters
+        modulePosition.angle = inputs.steeringPosition.inRotation2ds
+
+        if (steeringkP.hasChanged() || steeringkI.hasChanged() || steeringkD.hasChanged()) {
+            io.configureSteeringPID(steeringkP.get(), steeringkI.get(), steeringkD.get())
+        }
+
+        if (steeringMaxVel.hasChanged() || steeringMaxAccel.hasChanged()) {
+            io.configureSteeringMotionMagic(steeringMaxVel.get(), steeringMaxAccel.get())
+        }
+
+        if (drivekP.hasChanged() ||
+            drivekI.hasChanged() ||
+            drivekD.hasChanged() ||
+            driveKV.hasChanged() ||
+            driveKA.hasChanged()
+        ) {
+            io.configureDrivePID(
+                drivekP.get(), drivekI.get(), drivekD.get(), driveKV.get(), driveKA.get()
+            )
+        }
+
+
+    }
+    fun resetModuleZero() {
+        io.resetModuleZero()
+    }
+
+    /** Zeros the steering motor */
+    fun zeroSteering(isInAutonomous: Boolean = false) {
+        io.zeroSteering(isInAutonomous)
+    }
+
+    /** Zeros the drive motor */
+    fun zeroDrive() {
+        io.zeroDrive()
+    }
+
+    fun setDriveBrakeMode(brake: Boolean) {
+        io.setDriveBrakeMode(brake)
+    }
+
+    fun setSteeringBrakeMode(brake: Boolean) {
+        io.setSteeringBrakeMode(brake)
+    }
+
+    fun runCharacterization(input: ElectricalPotential) {
+        io.runCharacterization(input)
+    }
+}