Separated feedforward from PID in SwerveModuleIO and added units to logs

team4099 · Jul 4, 2024 · 731e618 · 731e618
1 parent 3379c1e
commit 731e618
Show file tree

Hide file tree

Showing 10 changed files with 27 additions and 19 deletions.
diff --git a/.gradle/8.4/executionHistory/executionHistory.bin b/.gradle/8.4/executionHistory/executionHistory.bin
diff --git a/.gradle/8.4/executionHistory/executionHistory.lock b/.gradle/8.4/executionHistory/executionHistory.lock
diff --git a/.gradle/8.4/fileHashes/fileHashes.bin b/.gradle/8.4/fileHashes/fileHashes.bin
diff --git a/.gradle/8.4/fileHashes/fileHashes.lock b/.gradle/8.4/fileHashes/fileHashes.lock
diff --git a/.gradle/8.4/fileHashes/resourceHashesCache.bin b/.gradle/8.4/fileHashes/resourceHashesCache.bin
diff --git a/.gradle/buildOutputCleanup/buildOutputCleanup.lock b/.gradle/buildOutputCleanup/buildOutputCleanup.lock
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
@@ -163,9 +163,8 @@ class SwerveModule(private val io: SwerveModuleIO) {
       drivekA.hasChanged() ||
       drivekV.hasChanged()
     ) {
-      io.configureDrivePID(
-        drivekP.get(), drivekI.get(), drivekD.get(), drivekV.get(), drivekA.get()
-      )
+      io.configureDrivePID(drivekP.get(), drivekI.get(), drivekD.get())
+      io.configureDriveFeedForward(drivekV.get(), drivekA.get())
     }
     if (steerMaxVelo.hasChanged() || steerMaxAccel.hasChanged()) {
       io.configSteerMotionMagic(steerMaxVelo.get(), steerMaxAccel.get())

diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt
@@ -65,16 +65,16 @@ interface SwerveModuleIO {
     override fun toLog(table: LogTable?) {
       table?.put("driveAppliedVoltage", driveAppliedVoltage.inVolts)
       table?.put("swerveAppliedVoltage", steerAppliedVoltage.inVolts)
-      table?.put("statorCurrentDrive", statorCurrentDrive.inAmperes)
-      table?.put("supplyCurrentDrive", supplyCurrentDrive.inAmperes)
-      table?.put("statorCurrentSteer", statorCurrentSteer.inAmperes)
-      table?.put("supplyCurrentSteer", supplyCurrentSteer.inAmperes)
-      table?.put("drivePosition", drivePosition.inMeters)
-      table?.put("steerPosition", steerPosition.inRadians)
-      table?.put("steerTemp", steerTemp.inCelsius)
-      table?.put("driveTemp", driveTemp.inCelsius)
-      table?.put("driveVelocity", driveVelocity.inMetersPerSecond)
-      table?.put("steerVelocity", steerVelocity.inRadiansPerSecond)
+      table?.put("statorCurrentDriveInAmperes", statorCurrentDrive.inAmperes)
+      table?.put("supplyCurrentDriveInAmperes", supplyCurrentDrive.inAmperes)
+      table?.put("statorCurrentSteerInAmperes", statorCurrentSteer.inAmperes)
+      table?.put("supplyCurrentSteerInAmperes", supplyCurrentSteer.inAmperes)
+      table?.put("drivePositionInMeters", drivePosition.inMeters)
+      table?.put("steerPositionInRadians", steerPosition.inRadians)
+      table?.put("steerTempInCelsius", steerTemp.inCelsius)
+      table?.put("driveTempInCelsius", driveTemp.inCelsius)
+      table?.put("driveVelocityInMetersPerSecond", driveVelocity.inMetersPerSecond)
+      table?.put("steerVelocityInRadiansPerSecond", steerVelocity.inRadiansPerSecond)
       table?.put("potentiometerOutputRaw", potentiometerOutputRaw)
       table?.put("potentiometerOutputRadians", potentiometerOutputRadians.inRadians)
 
@@ -160,6 +160,9 @@ interface SwerveModuleIO {
     kP: ProportionalGain<Velocity<Meter>, Volt>,
     kI: IntegralGain<Velocity<Meter>, Volt>,
     kD: DerivativeGain<Velocity<Meter>, Volt>,
+  ) {}
+
+  fun configureDriveFeedForward(
     kV: Value<Fraction<Volt, Velocity<Meter>>>,
     kA: Value<Fraction<Volt, Velocity<Velocity<Meter>>>>
   ) {}

diff --git a/...ain/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt b/...ain/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt
@@ -17,10 +17,8 @@ import org.team4099.lib.controller.PIDController
 import org.team4099.lib.controller.SimpleMotorFeedforward
 import org.team4099.lib.units.AngularAcceleration
 import org.team4099.lib.units.AngularVelocity
-import org.team4099.lib.units.Fraction
 import org.team4099.lib.units.LinearAcceleration
 import org.team4099.lib.units.LinearVelocity
-import org.team4099.lib.units.Value
 import org.team4099.lib.units.Velocity
 import org.team4099.lib.units.base.Length
 import org.team4099.lib.units.base.Meter
@@ -259,8 +257,8 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO {
     kP: ProportionalGain<Velocity<Meter>, Volt>,
     kI: IntegralGain<Velocity<Meter>, Volt>,
     kD: DerivativeGain<Velocity<Meter>, Volt>,
-    kV: Value<Fraction<Volt, Velocity<Meter>>>,
-    kA: Value<Fraction<Volt, Velocity<Velocity<Meter>>>>
+    // kV: Value<Fraction<Volt, Velocity<Meter>>>,
+    // kA: Value<Fraction<Volt, Velocity<Velocity<Meter>>>>
   ) {
     driveFeedback.setPID(kP, kI, kD)
   }

diff --git a/...n/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt b/...n/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt
@@ -361,14 +361,22 @@ class SwerveModuleIOTalon(
     kP: ProportionalGain<Velocity<Meter>, Volt>,
     kI: IntegralGain<Velocity<Meter>, Volt>,
     kD: DerivativeGain<Velocity<Meter>, Volt>,
-    kV: Value<Fraction<Volt, Velocity<Meter>>>,
-    kA: Value<Fraction<Volt, Velocity<Velocity<Meter>>>>
   ) {
     val PIDConfig = Slot0Configs()
 
     PIDConfig.kP = driveSensor.proportionalVelocityGainToRawUnits(kP)
     PIDConfig.kI = driveSensor.integralVelocityGainToRawUnits(kI)
     PIDConfig.kD = driveSensor.derivativeVelocityGainToRawUnits(kD)
+
+    driveFalcon.configurator.apply(PIDConfig)
+  }
+
+  override fun configureDriveFeedForward(
+    kV: Value<Fraction<Volt, Velocity<Meter>>>,
+    kA: Value<Fraction<Volt, Velocity<Velocity<Meter>>>>
+  ) {
+    val PIDConfig = Slot0Configs()
+
     PIDConfig.kV = kV.inVoltsPerMetersPerSecond
     PIDConfig.kA = kA.inVoltsPerMetersPerSecondPerSecond