From 7201cc71b3b113f87f71a34a3f2a1d038b06e745 Mon Sep 17 00:00:00 2001
From: 00magikarp <94652654+00magikarp@users.noreply.github.com>
Date: Tue, 18 Jun 2024 23:40:45 -0400
Subject: [PATCH] add logging to SwerveModule.kt

---
 .../drivetrain/swervemodule/SwerveModule.kt   | 75 +++++++++++--------
 1 file changed, 45 insertions(+), 30 deletions(-)

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 32f16e1e..fd4b9a65 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
@@ -1,36 +1,19 @@
 package com.team4099.robot2023.subsystems.drivetrain.swervemodule
 
-
 import com.sun.org.apache.xalan.internal.lib.ExsltMath.abs
 import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.robot2023.config.constants.DrivetrainConstants
+import com.team4099.robot2023.util.DebugLogger
 import edu.wpi.first.math.kinematics.SwerveModulePosition
 import edu.wpi.first.math.kinematics.SwerveModuleState
 import edu.wpi.first.wpilibj.RobotBase.isReal
-import org.team4099.lib.units.LinearAcceleration
-import org.team4099.lib.units.LinearVelocity
+import org.littletonrobotics.junction.Logger
+import org.team4099.lib.units.*
+import org.team4099.lib.units.base.inCelsius
 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.Angle
-import org.team4099.lib.units.derived.angle
-import org.team4099.lib.units.derived.degrees
-import org.team4099.lib.units.derived.inRadians
-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
-
+import org.team4099.lib.units.derived.*
 
 class SwerveModule(val io: SwerveModuleIO) {
 
@@ -124,26 +107,58 @@ class SwerveModule(val io: SwerveModuleIO) {
     io.updateInputs(inputs)
   }
   fun periodic() {
-    posDeltas.add (SwerveModulePosition ((inputs.drivePosition - lastDrivePos).inMeters, inputs.steerPosition.inRotation2ds))
+    Logger.processInputs(io.label, inputs)
+    DebugLogger.recordDebugOutput("${io.label}/lastDrivePos", lastDrivePos.inMeters)
+
+    DebugLogger.recordDebugOutput("${io.label}/velocitySetpoint", velocitySetpoint.inMetersPerSecond)
+    DebugLogger.recordDebugOutput("${io.label}/accelerationSetpoint", accelerationSetpoint.inMetersPerSecondPerSecond)
+    DebugLogger.recordDebugOutput("${io.label}/steeringSetpoint", steeringSetpoint.inDegrees)
+    DebugLogger.recordDebugOutput("${io.label}/lastDrivePos", lastDrivePos.inMeters)
+
+    Logger.recordOutput("${io.label}/driveAppliedVoltage", inputs.driveAppliedVoltage.inVolts)
+    Logger.recordOutput("${io.label}/swerveAppliedVoltage", inputs.swerveAppliedVoltage.inVolts)
+
+    Logger.recordOutput("${io.label}/drivePosition", inputs.drivePosition.inMeters)
+    Logger.recordOutput("${io.label}/steerPosition", inputs.steerPosition.inRadians)
+    Logger.recordOutput("${io.label}/driveVelocity", inputs.driveVelocity.inMetersPerSecond)
+    Logger.recordOutput("${io.label}/steerVelocity", inputs.steerVelocity.inRadiansPerSecond)
+    Logger.recordOutput("${io.label}/driveTemp", inputs.driveTemp.inCelsius)
+    Logger.recordOutput("${io.label}/steerTemp", inputs.steerTemp.inCelsius)
+    DebugLogger.recordDebugOutput("${io.label}/drift", inputs.drift.inMeters)
+
+    posDeltas.add(
+      SwerveModulePosition(
+        (inputs.drivePosition - lastDrivePos).inMeters, inputs.steerPosition.inRotation2ds
+      )
+    )
 
     lastDrivePos = inputs.drivePosition
 
     modulePosition.distanceMeters = inputs.drivePosition.inMeters
     modulePosition.angle = inputs.steerPosition.inRotation2ds
 
-    if (steerkD.hasChanged()|| steerkP.hasChanged()|| steerkI.hasChanged()){
+    if (steerkD.hasChanged() || steerkP.hasChanged() || steerkI.hasChanged()) {
       io.configSteerPID(steerkP.get(), steerkI.get(), steerkD.get())
     }
 
-    if (drivekD.hasChanged()|| drivekP.hasChanged()|| drivekI.hasChanged()||drivekA.hasChanged()||drivekV.hasChanged()){
+    if (drivekD.hasChanged() ||
+      drivekP.hasChanged() ||
+      drivekI.hasChanged() ||
+      drivekA.hasChanged() ||
+      drivekV.hasChanged()
+    ) {
       io.configDrivePID(drivekP.get(), drivekI.get(), drivekD.get(), drivekV.get(), drivekA.get())
     }
   }
-  fun OpenLoop(desiredState:SwerveModuleState, optimize: Boolean){
-    if(optimize){
-      var optimizedState = SwerveModuleState.optimize(desiredState, inputs.steerPosition.inRotation2ds)
-      io.setOpenLoop(optimizedState.angle.angle, optimizedState.speedMetersPerSecond.meters.perSecond * Math.cos(abs((optimizedState.angle.angle - inputs.steerPosition).inRadians)))
-
+  fun openLoop(desiredState: SwerveModuleState, optimize: Boolean) {
+    if (optimize) {
+      var optimizedState =
+        SwerveModuleState.optimize(desiredState, inputs.steerPosition.inRotation2ds)
+      io.setOpenLoop(
+        optimizedState.angle.angle,
+        optimizedState.speedMetersPerSecond.meters.perSecond *
+          Math.cos(abs((optimizedState.angle.angle - inputs.steerPosition).inRadians))
+      )
     }
   }
 }