fixed some programming issues

src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt

@@ -12,50 +12,61 @@ import org.team4099.lib.units.perSecond
 import com.ctre.phoenix6.controls.PositionVoltage as PositionVoltagePhoenix6
 
 class PositionVoltage(
-    private var position: Angle, // Assuming an AngularPosition type exists similar to AngularVelocity
-    private var enableFOC: Boolean = true,
-    private var feedforward: ElectricalPotential = 0.0.volts,
-    private var slot: Int = 0,
-    private var overrideBrakeDurNeutral: Boolean = false,
-    private var limitForwardMotion: Boolean = false,
-    private var limitReverseMotion: Boolean = false,
-    var velocity: AngularVelocity = 0.0.degrees.perSecond,
+  private var position:
+    Angle, // Assuming an AngularPosition type exists similar to AngularVelocity
+  private var enableFOC: Boolean = true,
+  private var feedforward: ElectricalPotential = 0.0.volts,
+  private var slot: Int = 0,
+  private var overrideBrakeDurNeutral: Boolean = false,
+  private var limitForwardMotion: Boolean = false,
+  private var limitReverseMotion: Boolean = false,
+  var velocity: AngularVelocity = 0.0.degrees.perSecond,
 ) {
 
-    val positionVoltagePhoenix6 = PositionVoltagePhoenix6(position.inRotations, velocity.inRotationsPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion)
-
-    fun setPosition(new_position: Angle) {
-        position = new_position
-        positionVoltagePhoenix6.Position = new_position.inRotations
-    }
-
-    fun setEnableFOC(new_enableFOC: Boolean) {
-        enableFOC = new_enableFOC
-        positionVoltagePhoenix6.EnableFOC = new_enableFOC
-    }
-
-    fun setFeedforward(new_feedforward: ElectricalPotential) {
-        feedforward = new_feedforward
-        positionVoltagePhoenix6.FeedForward = new_feedforward.inVolts
-    }
-
-    fun setSlot(new_slot: Int) {
-        slot = new_slot
-        positionVoltagePhoenix6.Slot = new_slot
-    }
-
-    fun setOverrideBrakeDurNeutral(new_override: Boolean) {
-        overrideBrakeDurNeutral = new_override
-        positionVoltagePhoenix6.OverrideBrakeDurNeutral = new_override
-    }
-
-    fun setLimitForwardMotion(new_limitForward: Boolean) {
-        limitForwardMotion = new_limitForward
-        positionVoltagePhoenix6.LimitForwardMotion = new_limitForward
-    }
-
-    fun setLimitReverseMotion(new_limitReverse: Boolean) {
-        limitReverseMotion = new_limitReverse
-        positionVoltagePhoenix6.LimitReverseMotion = new_limitReverse
-    }
-}
+  val positionVoltagePhoenix6 =
+    PositionVoltagePhoenix6(
+      position.inRotations,
+      velocity.inRotationsPerSecond,
+      enableFOC,
+      feedforward.inVolts,
+      slot,
+      overrideBrakeDurNeutral,
+      limitForwardMotion,
+      limitReverseMotion
+    )
+
+  fun setPosition(new_position: Angle) {
+    position = new_position
+    positionVoltagePhoenix6.Position = new_position.inRotations
+  }
+
+  fun setEnableFOC(new_enableFOC: Boolean) {
+    enableFOC = new_enableFOC
+    positionVoltagePhoenix6.EnableFOC = new_enableFOC
+  }
+
+  fun setFeedforward(new_feedforward: ElectricalPotential) {
+    feedforward = new_feedforward
+    positionVoltagePhoenix6.FeedForward = new_feedforward.inVolts
+  }
+
+  fun setSlot(new_slot: Int) {
+    slot = new_slot
+    positionVoltagePhoenix6.Slot = new_slot
+  }
+
+  fun setOverrideBrakeDurNeutral(new_override: Boolean) {
+    overrideBrakeDurNeutral = new_override
+    positionVoltagePhoenix6.OverrideBrakeDurNeutral = new_override
+  }
+
+  fun setLimitForwardMotion(new_limitForward: Boolean) {
+    limitForwardMotion = new_limitForward
+    positionVoltagePhoenix6.LimitForwardMotion = new_limitForward
+  }
+
+  fun setLimitReverseMotion(new_limitReverse: Boolean) {
+    limitReverseMotion = new_limitReverse
+    positionVoltagePhoenix6.LimitReverseMotion = new_limitReverse
+  }
+}

src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt

@@ -1,6 +1,5 @@
 package com.team4099.lib.phoenix6
 
-import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6
 import org.team4099.lib.units.AngularAcceleration
 import org.team4099.lib.units.AngularVelocity
 import org.team4099.lib.units.derived.ElectricalPotential
@@ -10,55 +9,68 @@ import org.team4099.lib.units.derived.volts
 import org.team4099.lib.units.inRotationsPerSecond
 import org.team4099.lib.units.inRotationsPerSecondPerSecond
 import org.team4099.lib.units.perSecond
+import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6
 
-class VelocityVoltage(private var velocity: AngularVelocity,
-                           private var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond,
-                           private var enableFOC:Boolean = true,
-                           private var feedforward: ElectricalPotential = 0.0.volts,
-                           private var slot:Int = 0,
-                           private var overrideBrakeDurNeutral: Boolean = false,
-                           private var limitForwardMotion: Boolean = false,
-                           private var limitReverseMotion: Boolean = false){
+class VelocityVoltage(
+  private var velocity: AngularVelocity,
+  private var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond,
+  private var enableFOC: Boolean = true,
+  private var feedforward: ElectricalPotential = 0.0.volts,
+  private var slot: Int = 0,
+  private var overrideBrakeDurNeutral: Boolean = false,
+  private var limitForwardMotion: Boolean = false,
+  private var limitReverseMotion: Boolean = false
+) {
 
-    val velocityVoltagePhoenix6 = VelocityVoltagePhoenix6(velocity.inRotationsPerSecond, acceleration.inRotationsPerSecondPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion)
+  val velocityVoltagePhoenix6 =
+    VelocityVoltagePhoenix6(
+      velocity.inRotationsPerSecond,
+      acceleration.inRotationsPerSecondPerSecond,
+      enableFOC,
+      feedforward.inVolts,
+      slot,
+      overrideBrakeDurNeutral,
+      limitForwardMotion,
+      limitReverseMotion
+    )
 
-    fun setVelocity(new_velocity: AngularVelocity) {
-        velocity = new_velocity
-        velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond
-    }
+  fun setVelocity(new_velocity: AngularVelocity) {
+    velocity = new_velocity
+    velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond
+  }
 
-    fun setAcceleration(new_accel: AngularAcceleration) {
-        acceleration = new_accel
-        velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond
-    }
+  fun setAcceleration(new_accel: AngularAcceleration) {
+    acceleration = new_accel
+    velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond
+  }
 
-    fun setEnableFOC(new_enableFOC: Boolean) {
-        enableFOC = new_enableFOC
-        velocityVoltagePhoenix6.EnableFOC = new_enableFOC
-    }
+  fun setEnableFOC(new_enableFOC: Boolean) {
+    enableFOC = new_enableFOC
+    velocityVoltagePhoenix6.EnableFOC = new_enableFOC
+  }
 
-    fun setFeedforward(new_feedforward: ElectricalPotential) {
-        feedforward = new_feedforward
-        velocityVoltagePhoenix6.FeedForward = new_feedforward.inVolts
-    }
+  fun setFeedforward(new_feedforward: ElectricalPotential) {
+    feedforward = new_feedforward
+    velocityVoltagePhoenix6.FeedForward = new_feedforward.inVolts
+  }
 
-    fun setSlot(new_slot: Int) {
-        slot = new_slot
-        velocityVoltagePhoenix6.Slot = new_slot
-    }
+  fun setSlot(new_slot: Int) {
+    slot = new_slot
+    velocityVoltagePhoenix6.Slot = new_slot
+  }
 
-    fun setOverrideBrakeDurNeutral(new_override: Boolean) {
-        overrideBrakeDurNeutral = new_override
-        velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override
-    }
+  fun setOverrideBrakeDurNeutral(new_override: Boolean) {
+    overrideBrakeDurNeutral = new_override
+    velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override
+  }
 
-    fun setLimitForwardMotion(new_limitForward: Boolean) {
-        limitForwardMotion = new_limitForward
-        velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward
-    }
+  fun setLimitForwardMotion(new_limitForward: Boolean) {
+    limitForwardMotion = new_limitForward
+    velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward
+  }
 
-    fun setLimitReverseMotion(new_limitReverse: Boolean) {
-        limitReverseMotion = new_limitReverse
-        velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse
-    }
-}
+  fun setLimitReverseMotion(new_limitReverse: Boolean) {
+    limitReverseMotion = new_limitReverse
+    velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse
+  }
+}

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

@@ -14,16 +14,14 @@ import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO
 import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim
 import com.team4099.robot2023.subsystems.limelight.LimelightVision
 import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO
-import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import com.team4099.robot2023.subsystems.vision.Vision
 import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar
 import com.team4099.robot2023.util.driver.Ryan
 import edu.wpi.first.wpilibj.RobotBase
-import org.team4099.lib.geometry.Pose2d
 import org.team4099.lib.smoothDeadband
-import org.team4099.lib.units.base.feet
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 
 object RobotContainer {
   private val drivetrain: Drivetrain
@@ -140,7 +138,6 @@ object RobotContainer {
     ControlBoard.elevatorOpenLoopRetract.whileTrue(elevator.testElevatorOpenLoopRetractCommand())
     ControlBoard.elevatorClosedLoopHigh.whileTrue(elevator.testElevatorClosedLoopExtendCommand())
     ControlBoard.elevatorClosedLoopLow.whileTrue(elevator.testElevatorClosedLoopExtendCommand())
-
   }
 
   fun mapTestControls() {}

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt

@@ -1,19 +1,20 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
-import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import edu.wpi.first.wpilibj2.command.Command
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.radians
 import org.team4099.lib.units.perSecond
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 
 class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() {
   init {
     addRequirements(drivetrain)
   }
 
   override fun execute() {
-    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
+    drivetrain.currentRequest =
+      DrivetrainRequest.OpenLoop(
         0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
       )
     drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) }

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt

@@ -6,7 +6,6 @@ import com.team4099.lib.trajectory.CustomTrajectoryGenerator
 import com.team4099.lib.trajectory.Waypoint
 import com.team4099.robot2023.config.constants.DrivetrainConstants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
-import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import com.team4099.robot2023.util.AllianceFlipUtil
 import com.team4099.robot2023.util.Velocity2d
 import edu.wpi.first.math.kinematics.ChassisSpeeds
@@ -22,8 +21,6 @@ import org.team4099.lib.geometry.Pose2d
 import org.team4099.lib.geometry.Pose2dWPILIB
 import org.team4099.lib.hal.Clock
 import org.team4099.lib.kinematics.ChassisAccels
-import java.util.function.Supplier
-import kotlin.math.PI
 import org.team4099.lib.units.Velocity
 import org.team4099.lib.units.base.Meter
 import org.team4099.lib.units.base.inMeters
@@ -55,6 +52,9 @@ import org.team4099.lib.units.inMetersPerSecondPerSecond
 import org.team4099.lib.units.inRadiansPerSecond
 import org.team4099.lib.units.inRadiansPerSecondPerSecond
 import org.team4099.lib.units.perSecond
+import java.util.function.Supplier
+import kotlin.math.PI
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 
 class DrivePathCommand(
   val drivetrain: Drivetrain,
@@ -244,7 +244,8 @@ class DrivePathCommand(
       Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position)
     )
 
-    drivetrain.currentRequest = DrivetrainRequest.ClosedLoop(
+    drivetrain.currentRequest =
+      DrivetrainRequest.ClosedLoop(
         nextDriveState,
         ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB
       )
@@ -309,16 +310,18 @@ class DrivePathCommand(
     Logger.recordOutput("ActiveCommands/DrivePathCommand", false)
     if (interrupted) {
       // Stop where we are if interrupted
-      drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
-        0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
-      )
+      drivetrain.currentRequest =
+        DrivetrainRequest.OpenLoop(
+          0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
+        )
     } else {
       // Execute one last time to end up in the final state of the trajectory
       // Since we weren't interrupted, we know curTime > endTime
       execute()
-      drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
-        0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
-      )
+      drivetrain.currentRequest =
+        DrivetrainRequest.OpenLoop(
+          0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
+        )
     }
   }
 }

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt

@@ -3,18 +3,28 @@ package com.team4099.robot2023.commands.drivetrain
 import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.robot2023.config.constants.DrivetrainConstants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
-import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.controller.ProfiledPIDController
 import org.team4099.lib.controller.TrapezoidProfile
 import org.team4099.lib.units.Velocity
 import org.team4099.lib.units.base.meters
-import org.team4099.lib.units.derived.*
+import org.team4099.lib.units.derived.Angle
+import org.team4099.lib.units.derived.Radian
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inDegrees
+import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree
+import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond
+import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds
+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.radians
 import org.team4099.lib.units.inDegreesPerSecond
 import org.team4099.lib.units.perSecond
 import kotlin.math.PI
+import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 
 class GoToAngle(val drivetrain: Drivetrain) : Command() {
   private val thetaPID: ProfiledPIDController<Radian, Velocity<Radian>>
@@ -82,7 +92,8 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() {
 
     val thetaFeedback = thetaPID.calculate(drivetrain.odometryPose.rotation, desiredAngle)
 
-    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
+    drivetrain.currentRequest =
+      DrivetrainRequest.OpenLoop(
         thetaFeedback, Pair(0.0.meters.perSecond, 0.0.meters.perSecond), fieldOriented = true
       )
 
@@ -98,8 +109,9 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() {
   }
 
   override fun end(interrupted: Boolean) {
-    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(
-      0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
-    )
+    drivetrain.currentRequest =
+      DrivetrainRequest.OpenLoop(
+        0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
+      )
   }
 }