added crappy dynamic profiler command that spits out a .wpilog file.

src/main/kotlin/frc/team449/control/holonomic/SwerveDrive.kt

@@ -138,15 +138,19 @@ open class SwerveDrive(
   }
 
   /** @return An array of [SwerveModulePosition] for each module, containing distance and angle. */
-  private fun getPositions(): Array<SwerveModulePosition> {
+  fun getPositions(): Array<SwerveModulePosition> {
     return Array(modules.size) { i -> modules[i].position }
   }
 
   /** @return An array of [SwerveModuleState] for each module, containing speed and angle. */
-  private fun getStates(): Array<SwerveModuleState> {
+  fun getStates(): Array<SwerveModuleState> {
     return Array(modules.size) { i -> modules[i].state }
   }
 
+  fun getVoltages(): Array<Double> {
+    return Array(modules.size) { i -> modules[i].lastDrivingVoltage() }
+  }
+
   private fun setRobotPose() {
     this.field.robotPose = this.pose
     this.field.getObject("FL").pose = this.pose.plus(Transform2d(Translation2d(SwerveConstants.WHEELBASE / 2, SwerveConstants.TRACKWIDTH / 2), this.getPositions()[0].angle))

src/main/kotlin/frc/team449/control/holonomic/sysid/SwerveDynamicProfiler.kt

@@ -0,0 +1,62 @@
+package frc.team449.control.holonomic.sysid
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds
+import edu.wpi.first.util.datalog.DoubleLogEntry
+import edu.wpi.first.wpilibj.DataLogManager
+import edu.wpi.first.wpilibj.Timer
+import edu.wpi.first.wpilibj2.command.Command
+import frc.team449.control.holonomic.SwerveDrive
+
+/**
+ * Before using this set kS, kA, kP, kI, kD = 0 and kV = 1
+ */
+class SwerveDynamicProfiler(
+  private val drive: SwerveDrive
+) : Command() {
+  val timer = Timer()
+
+  val log = DataLogManager.getLog()
+  val time = DoubleLogEntry(log, "time")
+  val frontLeftVol = DoubleLogEntry(log, "front left voltage")
+  val frontLeftVel = DoubleLogEntry(log, "front left velocity")
+  val frontLeftPos = DoubleLogEntry(log, "front left position")
+  val frontRightVol = DoubleLogEntry(log, "front right voltage")
+  val frontRightVel = DoubleLogEntry(log, "front right velocity")
+  val frontRightPos = DoubleLogEntry(log, "front right position")
+  val backRightVol = DoubleLogEntry(log, "back right voltage")
+  val backRightVel = DoubleLogEntry(log, "back right velocity")
+  val backRightPos = DoubleLogEntry(log, "back right position")
+  val backLeftVol = DoubleLogEntry(log, "back left voltage")
+  val backLeftVel = DoubleLogEntry(log, "back left velocity")
+  val backLeftPos = DoubleLogEntry(log, "back left position")
+
+  init {
+    addRequirements(drive)
+    timer.restart()
+
+    DataLogManager.start()
+  }
+
+  override fun execute() {
+    val v_goal = 2.0 * timer.get() / 5
+    drive.set(ChassisSpeeds(v_goal, 0.0, 0.0))
+    val positions = drive.getPositions()
+    val velocities = drive.getStates()
+    val voltages = drive.getVoltages()
+    if (v_goal > 0) {
+      time.append(timer.get())
+      frontLeftVol.append(voltages[0])
+      frontRightVol.append(voltages[1])
+      backRightVol.append(voltages[2])
+      backLeftVol.append(voltages[3])
+      frontLeftPos.append(positions[0].distanceMeters)
+      frontRightPos.append(positions[1].distanceMeters)
+      backRightPos.append(positions[2].distanceMeters)
+      backLeftPos.append(positions[3].distanceMeters)
+      frontLeftVel.append(velocities[0].speedMetersPerSecond)
+      frontRightVel.append(velocities[1].speedMetersPerSecond)
+      backRightVel.append(velocities[2].speedMetersPerSecond)
+      backLeftVel.append(velocities[3].speedMetersPerSecond)
+    }
+  }
+}

src/main/kotlin/frc/team449/robot2023/auto/routines/RoutineChooser.kt

@@ -34,7 +34,7 @@ class RoutineChooser(private val robot: Robot) : SendableChooser<String>() {
     )
 
     this.addOption(
-      "One Piece and Drop",
+      "One Piece and Drop, WIP, Don't use.",
       if (isRed) {
         "RedOnePieceDrop"
       } else {

src/main/kotlin/frc/team449/robot2023/constants/RobotConstants.kt

@@ -25,18 +25,17 @@ object RobotConstants {
   const val MAX_LINEAR_SPEED = SwerveConstants.MAX_ATTAINABLE_MK4I_SPEED // m/s
   const val MAX_ROT_SPEED = PI // rad/s
   val MAX_ACCEL = 4 * DCMotor(
-      MotorConstants.NOMINAL_VOLTAGE,
-      MotorConstants.STALL_TORQUE,
-      MotorConstants.STALL_CURRENT,
-      MotorConstants.FREE_CURRENT,
-      MotorConstants.FREE_SPEED,
-      1
-    ).getTorque(90.0)/
+    MotorConstants.NOMINAL_VOLTAGE,
+    MotorConstants.STALL_TORQUE,
+    MotorConstants.STALL_CURRENT,
+    MotorConstants.FREE_CURRENT,
+    MotorConstants.FREE_SPEED,
+    1
+  ).getTorque(90.0) /
     (Units.inchesToMeters(2.0) * ROBOT_WEIGHT * SwerveConstants.DRIVE_GEARING) // m/s/s
 
   val INITIAL_POSE = Pose2d(0.0, 0.0, Rotation2d.fromDegrees(0.0))
 
-
   init {
     println("Max Accel $MAX_ACCEL")
   }