working dt maybe

simgui-ds.json

@@ -92,6 +92,12 @@
   "robotJoysticks": [
     {
       "guid": "Keyboard0"
+    },
+    {
+      "guid": "Keyboard1"
+    },
+    {
+      "guid": "Keyboard2"
     }
   ]
 }

src/main/kotlin/com/team4099/robot2023/BuildConstants.kt

@@ -6,10 +6,10 @@ package com.team4099.robot2023
 const val MAVEN_GROUP = ""
 const val MAVEN_NAME = "Crescendo-2024"
 const val VERSION = "unspecified"
-const val GIT_REVISION = 99
-const val GIT_SHA = "3676e5c0aa32041bab20dd30c8eb185a918d82dc"
-const val GIT_DATE = "2024-01-29T15:42:24Z"
+const val GIT_REVISION = 100
+const val GIT_SHA = "211385d92d6ac7f542123c55c842c94f23b08db0"
+const val GIT_DATE = "2024-01-29T16:34:41Z"
 const val GIT_BRANCH = "drivetrain"
-const val BUILD_DATE = "2024-01-29T16:27:27Z"
-const val BUILD_UNIX_TIME = 1706563647813L
+const val BUILD_DATE = "2024-01-31T16:13:12Z"
+const val BUILD_UNIX_TIME = 1706735592229L
 const val DIRTY = 1

src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt

@@ -159,14 +159,14 @@ object DrivetrainConstants {
     //    val DRIVE_KV = 2.2678.volts / 1.0.meters.perSecond
     //    val DRIVE_KA = 0.40499.volts / 1.0.meters.perSecond.perSecond
 
-    val SIM_DRIVE_KS = 0.116970.volts
+    val SIM_DRIVE_KS = 0.0.volts
     val SIM_DRIVE_KV = 2.7.volts / 1.0.meters.perSecond
 
     val SIM_DRIVE_KP = 1.5.volts / 1.meters.perSecond
     val SIM_DRIVE_KI = 0.0.volts / (1.meters.perSecond * 1.seconds)
     val SIM_DRIVE_KD = 0.0.volts / 1.meters.perSecond.perSecond
 
-    val SIM_STEERING_KP = 0.4.volts.perDegree
+    val SIM_STEERING_KP = 0.3.volts.perDegree
     val SIM_STEERING_KI = 0.0.volts.perDegreeSeconds
     val SIM_STEERING_KD = 0.0.volts.perDegreePerSecond
   }

src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt

@@ -41,10 +41,12 @@ import org.team4099.lib.units.base.inches
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inDegrees
 import org.team4099.lib.units.derived.inRadians
 import org.team4099.lib.units.derived.inRotation2ds
 import org.team4099.lib.units.derived.radians
 import org.team4099.lib.units.inMetersPerSecond
+import org.team4099.lib.units.inRadiansPerSecond
 import org.team4099.lib.units.perSecond
 import java.util.concurrent.locks.Lock
 import java.util.concurrent.locks.ReentrantLock
@@ -267,7 +269,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
     Logger.recordOutput("Drivetrain/setPointStates", SwerveModuleState.struct, *setPointStates.toTypedArray())
 
 
-    Logger.recordOutput(VisionConstants.POSE_TOPIC_NAME, Pose2dWPILIB.struct, odometryPose.pose2d)
+    Logger.recordOutput(VisionConstants.POSE_TOPIC_NAME, doubleArrayOf(odometryPose.x.inMeters, odometryPose.y.inMeters, odometryPose.rotation.inRadians))
     Logger.recordOutput(
       "Odometry/pose3d", Pose3dWPILIB.struct,
       Pose3d(
@@ -305,7 +307,8 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
       DrivetrainState.OPEN_LOOP -> {
         // Outputs
         setOpenLoop(velocityTarget, targetedDriveVector, fieldOrientation)
-
+        Logger.recordOutput("Drivetrain/TargetVelocityX", targetedDriveVector.first.inMetersPerSecond)
+        Logger.recordOutput("Drivetrain/TargetVelocityY", targetedDriveVector.second.inMetersPerSecond)
         // Transitions
         nextState = fromRequestToState(currentRequest)
       }

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

@@ -126,6 +126,8 @@ class SwerveModule(val io: SwerveModuleIO) {
     val deltaCount =
       Math.min(inputs.odometryDrivePositions.size, inputs.odometrySteeringPositions.size)
 
+    positionDeltas.clear()
+
     for (i in 0..deltaCount-1) {
       val newDrivePosition = inputs.odometryDrivePositions[i]
       val newSteeringAngle = inputs.odometrySteeringPositions[i]
@@ -137,6 +139,13 @@ class SwerveModule(val io: SwerveModuleIO) {
       lastDrivePosition = newDrivePosition
     }
 
+    if (positionDeltas.size > 0) {
+      Logger.recordOutput("Drivetrain/PositionDeltas", positionDeltas[0].distanceMeters)
+    }
+     else {
+       Logger.recordOutput("Drivetrain/PositionDeltas", -1337)
+    }
+
     // Updating SwerveModulePosition every loop cycle
     modulePosition.distanceMeters = inputs.drivePosition.inMeters
     modulePosition.angle = inputs.steeringPosition.inRotation2ds

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

@@ -13,6 +13,7 @@ import org.team4099.lib.units.base.amps
 import org.team4099.lib.units.base.celsius
 import org.team4099.lib.units.base.inAmperes
 import org.team4099.lib.units.base.inCelsius
+import org.team4099.lib.units.base.inDecameters
 import org.team4099.lib.units.base.inMeters
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.Angle
@@ -22,6 +23,7 @@ import org.team4099.lib.units.derived.ProportionalGain
 import org.team4099.lib.units.derived.Radian
 import org.team4099.lib.units.derived.Volt
 import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inDegrees
 import org.team4099.lib.units.derived.inRadians
 import org.team4099.lib.units.derived.inVolts
 import org.team4099.lib.units.derived.radians
@@ -73,6 +75,18 @@ interface SwerveModuleIO {
       table?.put("potentiometerOutputRaw", potentiometerOutputRaw)
       table?.put("potentiometerOutputRadians", potentiometerOutputRadians.inRadians)
       table?.put("driftPositionMeters", drift.inMeters)
+
+      if (odometryDrivePositions.size > 0) {
+        table?.put("odometryDrivePositionsMeters", odometryDrivePositions[0].inMeters)
+      } else {
+        table?.put("odometryDrivePositionsMeters", 0.0)
+      }
+      if (odometrySteeringPositions.size > 0) {
+        table?.put("odometrySteeringPositionsDegrees", odometrySteeringPositions[0].inDegrees)
+      } else {
+        table?.put("odometrySteeringPositionsDegrees", 0.0)
+      }
+
     }
 
     override fun fromLog(table: LogTable?) {

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

@@ -121,6 +121,9 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO {
     steeringFeedback.errorTolerance = DrivetrainConstants.ALLOWED_STEERING_ANGLE_ERROR
   }
 
+  var driveAppliedVolts = 0.0.volts
+  var turnAppliedVolts = 0.0.volts
+
   override fun updateInputs(inputs: SwerveModuleIO.SwerveModuleIOInputs) {
     driveMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds)
     steerMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds)
@@ -169,7 +172,7 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO {
     inputs.driveVelocity = driveVelocity
     inputs.steeringVelocity = steerMotorSim.angularVelocityRadPerSec.radians.perSecond
 
-    inputs.driveAppliedVoltage = (-1337).volts
+    inputs.driveAppliedVoltage = driveAppliedVolts
     inputs.driveSupplyCurrent = driveMotorSim.currentDrawAmps.amps
     inputs.driveStatorCurrent =
       (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator
@@ -178,7 +181,7 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO {
     inputs.driveTemp = (-1337).celsius
     inputs.steeringTemp = (-1337).celsius
 
-    inputs.steeringAppliedVoltage = (-1337).volts
+    inputs.steeringAppliedVoltage = turnAppliedVolts
     inputs.steeringSupplyCurrent = steerMotorSim.currentDrawAmps.amps
     inputs.steeringStatorCurrent =
       (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator
@@ -201,12 +204,12 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO {
 
   // helper functions to clamp all inputs and set sim motor voltages properly
   private fun setDriveVoltage(volts: ElectricalPotential) {
-    val driveAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts)
+    driveAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts)
     driveMotorSim.setInputVoltage(driveAppliedVolts.inVolts)
   }
 
   private fun setSteeringVoltage(volts: ElectricalPotential) {
-    val turnAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts)
+    turnAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts)
     steerMotorSim.setInputVoltage(turnAppliedVolts.inVolts)
   }