Lock rotation while intaking from double sub

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

@@ -27,7 +27,6 @@ import org.littletonrobotics.junction.LogFileUtil
 import org.littletonrobotics.junction.LoggedRobot
 import org.littletonrobotics.junction.Logger
 import org.littletonrobotics.junction.inputs.LoggedPowerDistribution
-import org.littletonrobotics.junction.networktables.NT4Publisher
 import org.littletonrobotics.junction.wpilog.WPILOGReader
 import org.littletonrobotics.junction.wpilog.WPILOGWriter
 import org.team4099.lib.units.base.inMilliseconds

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

@@ -2,6 +2,7 @@ package com.team4099.robot2023
 
 import com.team4099.robot2023.auto.AutonomousSelector
 import com.team4099.robot2023.commands.AutoScoreCommand
+import com.team4099.robot2023.commands.DoubleSubIntakeCommand
 import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand
 import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand
 import com.team4099.robot2023.config.ControlBoard
@@ -47,6 +48,13 @@ object RobotContainer {
   val rumbleState: Boolean
     get() = superstructure.rumbleState
 
+  val driveX: () -> Double = {
+    ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND)
+  }
+  val driveY = { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }
+  val turn = { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }
+  val slowMode = { ControlBoard.slowMode }
+
   init {
     if (RobotBase.isReal()) {
       // Real Hardware Implementations
@@ -99,14 +107,7 @@ object RobotContainer {
 
   fun mapDefaultCommands() {
     drivetrain.defaultCommand =
-      TeleopDriveCommand(
-        driver = Ryan(),
-        { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
-        { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
-        { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
-        { ControlBoard.slowMode },
-        drivetrain
-      )
+      TeleopDriveCommand(driver = Ryan(), driveX, driveY, turn, slowMode, drivetrain)
   }
 
   fun requestSuperstructureIdle() {
@@ -190,7 +191,11 @@ object RobotContainer {
     ControlBoard.scoreOuttake.whileTrue(superstructure.score())
     ControlBoard.singleSubstationIntake.whileTrue(superstructure.singleSubConeCommand())
     ControlBoard.groundIntakeCube.whileTrue(superstructure.groundIntakeCubeCommand())
-    ControlBoard.doubleSubstationIntake.whileTrue(superstructure.doubleSubConeCommand())
+    ControlBoard.doubleSubstationIntake.whileTrue(
+      DoubleSubIntakeCommand(
+        drivetrain, superstructure, driveX, driveY, slowMode, driver = Ryan()
+      )
+    )
     ControlBoard.prepScore.whileTrue(
       superstructure.prepScoreCommand(
         {

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

@@ -0,0 +1,71 @@
+package com.team4099.robot2023.commands
+
+import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand
+import com.team4099.robot2023.config.constants.DrivetrainConstants
+import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import com.team4099.robot2023.subsystems.superstructure.Superstructure
+import com.team4099.robot2023.util.FMSData
+import com.team4099.robot2023.util.driver.DriverProfile
+import edu.wpi.first.wpilibj2.command.Commands.runOnce
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
+import edu.wpi.first.wpilibj2.command.WaitUntilCommand
+import org.team4099.lib.controller.ProfiledPIDController
+import org.team4099.lib.controller.TrapezoidProfile
+import org.team4099.lib.units.derived.Angle
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.radians
+import org.team4099.lib.units.perSecond
+import java.lang.Math.PI
+
+class DoubleSubIntakeCommand(
+  val drivetrain: Drivetrain,
+  val superstructure: Superstructure,
+  val driveX: () -> Double,
+  val driveY: () -> Double,
+  val slowMode: () -> Boolean,
+  val driver: DriverProfile
+) : SequentialCommandGroup() {
+  var driveRotation = { drivetrain.odometryPose.rotation }
+  var targetAngle: Angle = 0.0.degrees
+
+  val thetaProfiledPidController =
+    ProfiledPIDController(
+      DrivetrainConstants.PID.DOUBLE_SUB_AUTO_ALIGN_KP,
+      DrivetrainConstants.PID.DOUBLE_SUB_AUTO_ALIGN_KI,
+      DrivetrainConstants.PID.DOUBLE_SUB_AUTO_ALIGN_KD,
+      TrapezoidProfile.Constraints(
+        DrivetrainConstants.TURN_SETPOINT_MAX, 40.degrees.perSecond.perSecond
+      )
+    )
+
+  init {
+    thetaProfiledPidController.enableContinuousInput(-PI.radians, PI.radians)
+
+    val setupCommand =
+      runOnce(
+        { targetAngle = if (FMSData.isBlue) 0.0.degrees else 180.degrees },
+        drivetrain,
+        superstructure
+      )
+
+    addCommands(
+      setupCommand,
+      ParallelCommandGroup(
+        TeleopDriveCommand(
+          driver,
+          driveX,
+          driveY,
+          {
+            thetaProfiledPidController.calculate(driveRotation.invoke()) /
+              DrivetrainConstants.TURN_SETPOINT_MAX
+          },
+          slowMode,
+          drivetrain
+        ),
+        WaitUntilCommand { (driveRotation.invoke() - targetAngle).absoluteValue <= 5.degrees }
+          .andThen(superstructure.doubleSubConeCommand()),
+      )
+    )
+  }
+}

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

@@ -1,9 +1,11 @@
 package com.team4099.robot2023.commands.drivetrain
 
+import com.team4099.robot2023.config.constants.DrivetrainConstants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import com.team4099.robot2023.util.driver.DriverProfile
 import edu.wpi.first.wpilibj2.command.CommandBase
 import org.littletonrobotics.junction.Logger
+import org.team4099.lib.units.inDegreesPerSecond
 
 class TeleopDriveCommand(
   val driver: DriverProfile,
@@ -25,6 +27,12 @@ class TeleopDriveCommand(
     val rotation = driver.rotationSpeedClampedSupplier(turn, slowMode)
     drivetrain.setOpenLoop(rotation, speed)
     Logger.getInstance().recordOutput("ActiveCommands/TeleopDriveCommand", true)
+
+    Logger.getInstance()
+      .recordOutput(
+        "DoubleSubIntake/commandedTurnSpeed",
+        turn.invoke() * DrivetrainConstants.TURN_SETPOINT_MAX.inDegreesPerSecond
+      )
   }
   override fun isFinished(): Boolean {
     return false

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

@@ -1,6 +1,8 @@
 package com.team4099.robot2023.config.constants
 
 import edu.wpi.first.wpilibj.RobotBase
+import org.team4099.lib.units.Fraction
+import org.team4099.lib.units.Value
 import org.team4099.lib.units.Velocity
 import org.team4099.lib.units.base.Meter
 import org.team4099.lib.units.base.amps
@@ -13,6 +15,8 @@ import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.DerivativeGain
 import org.team4099.lib.units.derived.IntegralGain
 import org.team4099.lib.units.derived.ProportionalGain
+import org.team4099.lib.units.derived.RADIANS_PER_DEGREES
+import org.team4099.lib.units.derived.Radian
 import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.meterSquared
 import org.team4099.lib.units.derived.metersPerSecondPerMetersPerSecond
@@ -89,6 +93,13 @@ object DrivetrainConstants {
   val STEERING_WHEEL_INERTIA = 0.004096955.kilo.grams.meterSquared
 
   object PID {
+    val DOUBLE_SUB_AUTO_ALIGN_KP: ProportionalGain<Radian, Velocity<Radian>> =
+      0.0.degrees.perSecond / 5.0.degrees
+    val DOUBLE_SUB_AUTO_ALIGN_KI: IntegralGain<Radian, Velocity<Radian>> =
+      0.degrees.perSecond / (0.0.degrees * 1.0.seconds)
+    val DOUBLE_SUB_AUTO_ALIGN_KD: DerivativeGain<Radian, Velocity<Radian>> =
+      (0.degrees.perSecond / (0.0.degrees.perSecond)).degreesPerSecondPerDegreesPerSecond
+
     val AUTO_POS_KP: ProportionalGain<Meter, Velocity<Meter>>
       get() {
         if (RobotBase.isReal()) {
@@ -168,3 +179,6 @@ object DrivetrainConstants {
     val SIM_STEERING_KD = 0.0.volts.perDegreePerSecond
   }
 }
+
+inline val Double.degreesPerSecondPerDegreesPerSecond
+  get() = Value<Fraction<Velocity<Radian>, Velocity<Radian>>>(this * RADIANS_PER_DEGREES)

src/main/kotlin/com/team4099/robot2023/util/NTSafePublisher.kt

@@ -14,8 +14,10 @@ import org.littletonrobotics.junction.LogTable.LoggableType
 class NTSafePublisher : LogDataReceiver {
   private val rootTable: NetworkTable = NetworkTableInstance.getDefault().getTable("/AdvantageKit")
   private var lastTable = LogTable(0)
-  private val timestampPublisher: IntegerPublisher = rootTable.getIntegerTopic(LogDataReceiver.timestampKey.substring(1))
-    .publish(PubSubOption.sendAll(true))
+  private val timestampPublisher: IntegerPublisher =
+    rootTable
+      .getIntegerTopic(LogDataReceiver.timestampKey.substring(1))
+      .publish(PubSubOption.sendAll(true))
   private val publishers: MutableMap<String, GenericPublisher?> = HashMap()
 
   override fun putTable(table: LogTable) {
@@ -40,40 +42,29 @@ class NTSafePublisher : LogDataReceiver {
         val key = field.key.substring(1)
         var publisher = publishers[key]
         if (publisher == null) {
-          publisher = rootTable.getTopic(key).genericPublish(
-            newValue.type.nT4Type,
-            PubSubOption.sendAll(true)
-          )
+          publisher =
+            rootTable
+              .getTopic(key)
+              .genericPublish(newValue.type.nT4Type, PubSubOption.sendAll(true))
           publishers[key] = publisher
         }
 
         when (newValue.type) {
           LoggableType.Raw -> publisher!!.setRaw(newValue.raw, table.timestamp)
           LoggableType.Boolean -> publisher!!.setBoolean(newValue.boolean, table.timestamp)
-          LoggableType.BooleanArray -> publisher!!.setBooleanArray(
-            newValue.booleanArray,
-            table.timestamp
-          )
-
+          LoggableType.BooleanArray ->
+            publisher!!.setBooleanArray(newValue.booleanArray, table.timestamp)
           LoggableType.Integer -> publisher!!.setInteger(newValue.integer, table.timestamp)
-          LoggableType.IntegerArray -> publisher!!.setIntegerArray(
-            newValue.integerArray,
-            table.timestamp
-          )
-
+          LoggableType.IntegerArray ->
+            publisher!!.setIntegerArray(newValue.integerArray, table.timestamp)
           LoggableType.Float -> publisher!!.setFloat(newValue.float, table.timestamp)
           LoggableType.FloatArray -> publisher!!.setFloatArray(newValue.floatArray, table.timestamp)
           LoggableType.Double -> publisher!!.setDouble(newValue.double, table.timestamp)
-          LoggableType.DoubleArray -> publisher!!.setDoubleArray(
-            newValue.doubleArray,
-            table.timestamp
-          )
-
+          LoggableType.DoubleArray ->
+            publisher!!.setDoubleArray(newValue.doubleArray, table.timestamp)
           LoggableType.String -> publisher!!.setString(newValue.string, table.timestamp)
-          LoggableType.StringArray -> publisher!!.setStringArray(
-            newValue.stringArray,
-            table.timestamp
-          )
+          LoggableType.StringArray ->
+            publisher!!.setStringArray(newValue.stringArray, table.timestamp)
         }
       }