fixing sim

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 = 25
-const val GIT_SHA = "b4edbcbc86bce4f445e9dcbf3dc9d1361b02aca3"
-const val GIT_DATE = "2024-01-15T19:28:22Z"
+const val GIT_REVISION = 26
+const val GIT_SHA = "73f4f3ef5bab17de6b7bbc919ece786ceb7b52ea"
+const val GIT_DATE = "2024-01-15T19:49:07Z"
 const val GIT_BRANCH = "intake"
-const val BUILD_DATE = "2024-01-15T19:47:46Z"
-const val BUILD_UNIX_TIME = 1705366066090L
+const val BUILD_DATE = "2024-01-15T20:41:51Z"
+const val BUILD_UNIX_TIME = 1705369311220L
 const val DIRTY = 1

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

@@ -6,16 +6,14 @@ import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand
 import com.team4099.robot2023.config.ControlBoard
 import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
-import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal
+import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIO
 import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim
 import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO
-import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2
 import com.team4099.robot2023.subsystems.intake.Intake
 import com.team4099.robot2023.subsystems.intake.IntakeIONEO
 import com.team4099.robot2023.subsystems.intake.IntakeIOSim
 import com.team4099.robot2023.subsystems.limelight.LimelightVision
 import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO
-import com.team4099.robot2023.subsystems.superstructure.Request
 import com.team4099.robot2023.subsystems.vision.Vision
 import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar
 import com.team4099.robot2023.util.driver.Ryan
@@ -34,7 +32,7 @@ object RobotContainer {
     if (RobotBase.isReal()) {
       // Real Hardware Implementations
       // drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {}
-      drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal)
+      drivetrain = Drivetrain(object : GyroIO {}, object : DrivetrainIO {})
       intake = Intake(IntakeIONEO)
       vision =
         Vision(
@@ -92,7 +90,7 @@ object RobotContainer {
   }
 
   fun zeroSensors() {
-    drivetrain.currentRequest = Request.DrivetrainRequest.ZeroSensors()
+    drivetrain.zeroSensors()
   }
 
   fun zeroAngle(toAngle: Angle) {

src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt

@@ -2,14 +2,14 @@ package com.team4099.robot2023.commands
 
 import edu.wpi.first.wpilibj.Timer
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
-import edu.wpi.first.wpilibj2.command.CommandBase
+import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.Subsystem
 import org.littletonrobotics.junction.Logger
 import java.util.function.BiConsumer
 import java.util.function.Consumer
 import java.util.function.Supplier
 
-class SysIdCommand : CommandBase {
+class SysIdCommand : Command {
   private val isDriveTrain: Boolean
   private lateinit var driveTrainSetter: BiConsumer<Double, Double>
   private lateinit var mechanismSetter: Consumer<Double>

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

@@ -1,20 +1,18 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
-import com.team4099.robot2023.subsystems.superstructure.Request
-import edu.wpi.first.wpilibj2.command.CommandBase
+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
 
-class DriveBrakeModeCommand(val drivetrain: Drivetrain) : CommandBase() {
+class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() {
   init {
     addRequirements(drivetrain)
   }
 
   override fun execute() {
-    drivetrain.currentRequest =
-      Request.DrivetrainRequest.OpenLoop(
+    drivetrain.setOpenLoop(
         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
 import com.team4099.robot2023.util.AllianceFlipUtil
 import com.team4099.robot2023.util.Velocity2d
 import edu.wpi.first.math.kinematics.ChassisSpeeds
@@ -15,44 +14,16 @@ import edu.wpi.first.math.trajectory.TrajectoryParameterizer.TrajectoryGeneratio
 import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint
 import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint
 import edu.wpi.first.wpilibj.DriverStation
-import edu.wpi.first.wpilibj2.command.CommandBase
+import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.controller.PIDController
 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 org.team4099.lib.units.Velocity
-import org.team4099.lib.units.base.Meter
-import org.team4099.lib.units.base.inMeters
-import org.team4099.lib.units.base.inSeconds
-import org.team4099.lib.units.base.inches
-import org.team4099.lib.units.base.meters
-import org.team4099.lib.units.base.seconds
-import org.team4099.lib.units.derived.Radian
-import org.team4099.lib.units.derived.cos
-import org.team4099.lib.units.derived.degrees
-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.inMetersPerSecondPerMeter
-import org.team4099.lib.units.derived.inMetersPerSecondPerMeterSeconds
-import org.team4099.lib.units.derived.inMetersPerSecondPerMetersPerSecond
-import org.team4099.lib.units.derived.inRadians
-import org.team4099.lib.units.derived.metersPerSecondPerMetersPerSecond
-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.perMeter
-import org.team4099.lib.units.derived.perMeterSeconds
-import org.team4099.lib.units.derived.radians
-import org.team4099.lib.units.derived.sin
-import org.team4099.lib.units.inDegreesPerSecond
-import org.team4099.lib.units.inMetersPerSecond
-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 org.team4099.lib.units.*
+import org.team4099.lib.units.base.*
+import org.team4099.lib.units.derived.*
 import java.util.function.Supplier
 import kotlin.math.PI
 
@@ -65,7 +36,7 @@ class DrivePathCommand(
   val endPathOnceAtReference: Boolean = true,
   val leaveOutYAdjustment: Boolean = false,
   val endVelocity: Velocity2d = Velocity2d(),
-) : CommandBase() {
+) : Command() {
   private val xPID: PIDController<Meter, Velocity<Meter>>
   private val yPID: PIDController<Meter, Velocity<Meter>>
 
@@ -244,8 +215,7 @@ class DrivePathCommand(
       Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position)
     )
 
-    drivetrain.currentRequest =
-      Request.DrivetrainRequest.ClosedLoop(
+    drivetrain.setClosedLoop(
         nextDriveState,
         ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB
       )
@@ -310,16 +280,14 @@ class DrivePathCommand(
     Logger.recordOutput("ActiveCommands/DrivePathCommand", false)
     if (interrupted) {
       // Stop where we are if interrupted
-      drivetrain.currentRequest =
-        Request.DrivetrainRequest.OpenLoop(
+      drivetrain.setOpenLoop(
           0.degrees.perSecond, Pair(0.meters.perSecond, 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 =
-        Request.DrivetrainRequest.OpenLoop(
+      drivetrain.setOpenLoop(
           0.degrees.perSecond, Pair(0.meters.perSecond, 0.meters.perSecond)
         )
     }

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

@@ -3,30 +3,19 @@ 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
 import edu.wpi.first.wpilibj.RobotBase
-import edu.wpi.first.wpilibj2.command.CommandBase
+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.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.derived.*
 import org.team4099.lib.units.inDegreesPerSecond
 import org.team4099.lib.units.perSecond
 import kotlin.math.PI
 
-class GoToAngle(val drivetrain: Drivetrain) : CommandBase() {
+class GoToAngle(val drivetrain: Drivetrain) : Command() {
   private val thetaPID: ProfiledPIDController<Radian, Velocity<Radian>>
 
   val thetakP =
@@ -92,8 +81,7 @@ class GoToAngle(val drivetrain: Drivetrain) : CommandBase() {
 
     val thetaFeedback = thetaPID.calculate(drivetrain.odometryPose.rotation, desiredAngle)
 
-    drivetrain.currentRequest =
-      Request.DrivetrainRequest.OpenLoop(
+    drivetrain.setOpenLoop(
         thetaFeedback, Pair(0.0.meters.perSecond, 0.0.meters.perSecond), fieldOriented = true
       )
 
@@ -109,8 +97,7 @@ class GoToAngle(val drivetrain: Drivetrain) : CommandBase() {
   }
 
   override fun end(interrupted: Boolean) {
-    drivetrain.currentRequest =
-      Request.DrivetrainRequest.OpenLoop(
+    drivetrain.setOpenLoop(
         0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
       )
   }

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

@@ -4,26 +4,17 @@ import com.team4099.lib.hal.Clock
 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
-import edu.wpi.first.wpilibj2.command.CommandBase
+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.Fraction
-import org.team4099.lib.units.Value
-import org.team4099.lib.units.Velocity
+import org.team4099.lib.units.*
 import org.team4099.lib.units.base.Meter
 import org.team4099.lib.units.base.Second
 import org.team4099.lib.units.base.meters
-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.radians
-import org.team4099.lib.units.inMetersPerSecond
-import org.team4099.lib.units.perSecond
-
-class GyroAutoLevel(val drivetrain: Drivetrain) : CommandBase() {
+import org.team4099.lib.units.derived.*
+
+class GyroAutoLevel(val drivetrain: Drivetrain) : Command() {
   private val gyroPID: ProfiledPIDController<Radian, Velocity<Meter>>
 
   var alignmentAngle = drivetrain.odometryPose.rotation
@@ -100,8 +91,7 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : CommandBase() {
   override fun execute() {
     Logger.recordOutput("ActiveCommands/AutoLevelCommand", true)
 
-    drivetrain.currentRequest =
-      Request.DrivetrainRequest.OpenLoop(
+    drivetrain.setOpenLoop(
         0.0.radians.perSecond, gyroFeedback, fieldOriented = true
       )
 
@@ -129,8 +119,7 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : CommandBase() {
   }
 
   override fun end(interrupted: Boolean) {
-    drivetrain.currentRequest =
-      Request.DrivetrainRequest.OpenLoop(
+    drivetrain.setOpenLoop(
         0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond)
       )
   }

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

@@ -1,20 +1,18 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
-import com.team4099.robot2023.subsystems.superstructure.Request
-import edu.wpi.first.wpilibj2.command.CommandBase
+import edu.wpi.first.wpilibj2.command.Command
 import org.team4099.lib.units.base.feet
 import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.perSecond
 
-class OpenLoopReverseCommand(val drivetrain: Drivetrain) : CommandBase() {
+class OpenLoopReverseCommand(val drivetrain: Drivetrain) : Command() {
   init {
     addRequirements(drivetrain)
   }
 
   override fun execute() {
-    drivetrain.currentRequest =
-      Request.DrivetrainRequest.OpenLoop(
+    drivetrain.setOpenLoop(
         0.degrees.perSecond,
         Pair(-5.0.feet.perSecond, 0.0.feet.perSecond),
         fieldOriented = false

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

@@ -1,13 +1,13 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
-import edu.wpi.first.wpilibj2.command.CommandBase
+import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
 
 class ResetGyroPitchCommand(val drivetrain: Drivetrain, val toAngle: Angle = 0.0.degrees) :
-  CommandBase() {
+  Command() {
   init {
     addRequirements(drivetrain)
   }

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

@@ -1,13 +1,13 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
-import edu.wpi.first.wpilibj2.command.CommandBase
+import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
 
 class ResetGyroYawCommand(val drivetrain: Drivetrain, val toAngle: Angle = 0.0.degrees) :
-  CommandBase() {
+  Command() {
   init {
     addRequirements(drivetrain)
   }

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

@@ -2,11 +2,11 @@ package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import com.team4099.robot2023.util.AllianceFlipUtil
-import edu.wpi.first.wpilibj2.command.CommandBase
+import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.geometry.Pose2d
 
-class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : CommandBase() {
+class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : Command() {
   init {
     addRequirements(drivetrain)
   }

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

@@ -1,10 +1,10 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
-import edu.wpi.first.wpilibj2.command.CommandBase
+import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
 
-class ResetZeroCommand(val drivetrain: Drivetrain) : CommandBase() {
+class ResetZeroCommand(val drivetrain: Drivetrain) : Command() {
   init {
     addRequirements(drivetrain)
   }

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

@@ -2,12 +2,12 @@ package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import edu.wpi.first.math.kinematics.SwerveModuleState
-import edu.wpi.first.wpilibj2.command.CommandBase
+import edu.wpi.first.wpilibj2.command.Command
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.inRotation2ds
 
 class SwerveModuleTuningCommand(val drivetrain: Drivetrain, val steeringPosition: () -> Angle) :
-  CommandBase() {
+  Command() {
   init {
     addRequirements(drivetrain)
   }