Saraansh optimizations + further encapsulation

simgui-ds.json

@@ -106,6 +106,9 @@
     },
     {
       "guid": "Keyboard1"
+    },
+    {
+      "guid": "03000000ac0500004f02000011010000"
     }
   ]
 }

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

@@ -78,9 +78,13 @@ class Drivetrain(private val gyroIO: GyroIO, val swerveModules: List<SwerveModul
 
   private var isFieldOriented = true
 
-  private var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0)
+  private var targetedChassisSpeeds =
+    ChassisSpeeds(0.0.meters.perSecond, 0.0.meters.perSecond, 0.0.radians.perSecond)
+      .chassisSpeedsWPILIB
 
-  private var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0)
+  private var targetedChassisAccels =
+    ChassisSpeeds(0.0.meters.perSecond, 0.0.meters.perSecond, 0.0.radians.perSecond)
+      .chassisSpeedsWPILIB
 
   private var isInAutonomous = false
 
@@ -484,6 +488,7 @@ class Drivetrain(private val gyroIO: GyroIO, val swerveModules: List<SwerveModul
     chassisAccels: edu.wpi.first.math.kinematics.ChassisSpeeds =
       edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0)
   ) {
+    // Allows parameter chassisSpeeds to have its value reassigned
     var chassisSpeeds = chassisSpeeds
 
     // If theres skew consider slowing down

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

@@ -39,7 +39,7 @@ import kotlin.math.abs
 import kotlin.math.cos
 import kotlin.math.withSign
 
-class SwerveModule(val io: SwerveModuleIO) {
+class SwerveModule(private val io: SwerveModuleIO) {
 
   val inputs = SwerveModuleIO.SwerveModuleIOInputs()
 
@@ -56,8 +56,6 @@ class SwerveModule(val io: SwerveModuleIO) {
 
   private var lastDrivePos = 0.0.meters
 
-  private var shouldInvert = false
-
   private val steerkP =
     LoggedTunableValue(
       "Drivetrain/modulesteerkP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree })
@@ -150,12 +148,6 @@ class SwerveModule(val io: SwerveModuleIO) {
     CustomLogger.recordDebugOutput("${io.label}/steeringSetpoint", steeringSetpoint.inDegrees)
     CustomLogger.recordDebugOutput("${io.label}/lastDrivePos", lastDrivePos.inMeters)
 
-    posDeltas.add(
-      SwerveModulePosition(
-        (inputs.drivePosition - lastDrivePos).inMeters, inputs.steerPosition.inRotation2ds
-      )
-    )
-
     lastDrivePos = inputs.drivePosition
 
     modulePosition.distanceMeters = inputs.drivePosition.inMeters
@@ -183,7 +175,7 @@ class SwerveModule(val io: SwerveModuleIO) {
   fun setOpenLoop(steering: Angle, speed: LinearVelocity, optimize: Boolean = true) {
     var steeringDifference =
       (steering - inputs.steerPosition).inRadians.IEEErem(2 * Math.PI).radians
-    shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize
+    val shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize
     if (shouldInvert) {
       steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians
     }

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

@@ -8,6 +8,7 @@ import com.team4099.robot2023.subsystems.falconspin.MotorCollection
 import com.team4099.robot2023.subsystems.falconspin.SimulatedMotor
 import edu.wpi.first.math.MathUtil
 import edu.wpi.first.math.system.plant.DCMotor
+import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj.simulation.BatterySim
 import edu.wpi.first.wpilibj.simulation.FlywheelSim
 import edu.wpi.first.wpilibj.simulation.RoboRioSim
@@ -273,11 +274,11 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO {
   }
 
   override fun setDriveBrakeMode(brake: Boolean) {
-    println("Can't set brake mode in simulation")
+    DriverStation.reportError("Can't set brake mode in simulation", true)
   }
 
   override fun configSteerMotionMagic(maxVel: AngularVelocity, maxAccel: AngularAcceleration) {
-    println("Can't configure motion magic in simulation")
+    DriverStation.reportError("Can't configure motion magic in simulation", true)
   }
 
   override fun runCharacterization(input: ElectricalPotential) {

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

@@ -224,9 +224,7 @@ class SwerveModuleIOTalon(
         DrivetrainConstants.WHEEL_DIAMETER.inMeters *
         DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO
     )
-    Logger.recordOutput("$label/drivePositionUnits", driveSensor.position.inMeters)
     inputs.drivePosition = driveSensor.position
-    inputs.steerPosition = steeringSensor.position
     Logger.recordOutput("$label/rawSteeringValue", steeringFalcon.position.value)
     Logger.recordOutput("$label/rawSteeringValue", steeringFalcon.position.value)
 

src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt

@@ -64,8 +64,7 @@ sealed interface Request {
 
     class ClosedLoop(
       var chassisSpeeds: ChassisSpeeds,
-      val chassisAccels: ChassisSpeeds =
-        edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0)
+      val chassisAccels: ChassisSpeeds = ChassisSpeeds(0.0, 0.0, 0.0)
     ) : DrivetrainRequest
 
     class ZeroSensors(val isInAutonomous: Boolean = false) : DrivetrainRequest