Targeting command

simgui.json

@@ -1,16 +1,4 @@
 {
-  "HALProvider": {
-    "Other Devices": {
-      "window": {
-        "visible": false
-      }
-    },
-    "Timing": {
-      "window": {
-        "visible": false
-      }
-    }
-  },
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
@@ -19,18 +7,9 @@
     }
   },
   "NetworkTables": {
-    "Persistent Values": {
-      "open": false
-    },
     "transitory": {
-      "AdvantageKit": {
-        "open": true
-      },
       "SmartDashboard": {
         "TunableNumbers": {
-          "Flywheel": {
-            "open": true
-          },
           "Pathfollow": {
             "open": true
           },

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

@@ -0,0 +1,15 @@
+package com.team4099.robot2023
+
+/**
+ * Automatically generated file containing build version information.
+ */
+const val MAVEN_GROUP = ""
+const val MAVEN_NAME = "Crescendo-2024"
+const val VERSION = "unspecified"
+const val GIT_REVISION = 272
+const val GIT_SHA = "17d8c89f309a6f0cd516ad5b977d44793755d524"
+const val GIT_DATE = "2024-02-20T19:22:23Z"
+const val GIT_BRANCH = "targeting-command-with-drivetrain"
+const val BUILD_DATE = "2024-02-20T19:30:24Z"
+const val BUILD_UNIX_TIME = 1708475424897L
+const val DIRTY = 1

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

@@ -2,9 +2,11 @@ package com.team4099.robot2023
 
 import com.team4099.robot2023.auto.AutonomousSelector
 import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand
+import com.team4099.robot2023.commands.drivetrain.TargetPoseCommand
 import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand
 import com.team4099.robot2023.config.ControlBoard
 import com.team4099.robot2023.config.constants.Constants
+import com.team4099.robot2023.config.constants.FieldConstants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal
 import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim
@@ -31,9 +33,12 @@ import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar
 import com.team4099.robot2023.subsystems.wrist.Wrist
 import com.team4099.robot2023.subsystems.wrist.WristIO
 import com.team4099.robot2023.subsystems.wrist.WristIOSim
+import com.team4099.robot2023.util.FrameCoordinate
 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 com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 
@@ -90,6 +95,7 @@ object RobotContainer {
     superstructure = Superstructure(intake, feeder, elevator, wrist, flywheel)
     vision.setDataInterfaces({ drivetrain.fieldTRobot }, { drivetrain.addVisionData(it) })
     limelight.poseSupplier = { drivetrain.odomTRobot }
+    FrameCoordinate.robotInOdometryFrameSupplier = { drivetrain.odomTRobot }
   }
 
   fun mapDefaultCommands() {
@@ -160,6 +166,17 @@ object RobotContainer {
     ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand())
     ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand())
     ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand())
+    ControlBoard.wristTestDown.whileTrue(
+      TargetPoseCommand(
+        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,
+        FrameCoordinate.FieldCoordinate(FieldConstants.centerSpeakerOpening)
+      )
+    )
 
     /*
     TUNING COMMANDS

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

@@ -0,0 +1,156 @@
+package com.team4099.robot2023.commands.drivetrain
+
+import com.team4099.lib.logging.LoggedTunableValue
+import com.team4099.lib.math.asPose2d
+import com.team4099.robot2023.config.constants.DrivetrainConstants
+import com.team4099.robot2023.config.constants.FieldConstants
+import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
+import com.team4099.robot2023.subsystems.superstructure.Request
+import com.team4099.robot2023.util.FMSData
+import com.team4099.robot2023.util.FrameCoordinate
+import com.team4099.robot2023.util.FrameType
+import com.team4099.robot2023.util.driver.DriverProfile
+import edu.wpi.first.math.Matrix
+import edu.wpi.first.math.kinematics.Odometry
+import edu.wpi.first.wpilibj.DriverStation
+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.geometry.Pose2d
+import org.team4099.lib.units.Velocity
+import org.team4099.lib.units.base.inMeters
+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.inRadians
+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 kotlin.math.atan2
+
+class TargetPoseCommand(
+    val driver: DriverProfile,
+    val driveX: () -> Double,
+    val driveY: () -> Double,
+    val turn: () -> Double,
+    val slowMode: () -> Boolean,
+    val drivetrain: Drivetrain,
+    targetCoordinate: FrameCoordinate
+) : Command() {
+    private val thetaPID: ProfiledPIDController<Radian, Velocity<Radian>>
+    private val fieldVelocitySupplier = { drivetrain.fieldVelocity.x to drivetrain.fieldVelocity.y }
+    val targetCoordinate = if (FMSData.allianceColor == DriverStation.Alliance.Red && targetCoordinate is FrameCoordinate.FieldCoordinate) {
+        FrameCoordinate.FieldCoordinate(FieldConstants.fieldLength - targetCoordinate.x, targetCoordinate.y)
+    } else targetCoordinate
+
+    val thetakP =
+        LoggedTunableValue(
+            "Pathfollow/thetakP",
+            Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree })
+        )
+    val thetakI =
+        LoggedTunableValue(
+            "Pathfollow/thetakI",
+            Pair(
+                { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds }
+            )
+        )
+    val thetakD =
+        LoggedTunableValue(
+            "Pathfollow/thetakD",
+            Pair(
+                { it.inDegreesPerSecondPerDegreePerSecond },
+                { it.degrees.perSecond.perDegreePerSecond }
+            )
+        )
+
+    val thetaMaxVel =
+        LoggedTunableValue("Pathfollow/thetaMaxVel", DrivetrainConstants.PID.MAX_AUTO_ANGULAR_VEL)
+    val thetaMaxAccel =
+        LoggedTunableValue("Pathfollow/thetaMaxAccel", DrivetrainConstants.PID.MAX_AUTO_ANGULAR_ACCEL)
+    var desiredAngle: Angle = 0.0.degrees
+    lateinit var odometryCoordinateToTarget: FrameCoordinate.OdometryCoordinate
+
+    init {
+        addRequirements(drivetrain)
+
+        if (RobotBase.isReal()) {
+            thetakP.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KP)
+            thetakI.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KI)
+            thetakD.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KD)
+        } else {
+            thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP)
+            thetakI.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI)
+            thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD)
+        }
+
+        thetaPID =
+            ProfiledPIDController(
+                thetakP.get(),
+                thetakI.get(),
+                thetakD.get(),
+                TrapezoidProfile.Constraints(thetaMaxVel.get(), thetaMaxAccel.get())
+            )
+        thetaPID.enableContinuousInput(-PI.radians, PI.radians)
+    }
+
+    override fun initialize() {
+        thetaPID.reset(drivetrain.odomTRobot.rotation)
+
+        odometryCoordinateToTarget = if (targetCoordinate is FrameCoordinate.FieldCoordinate) {
+            FrameCoordinate.OdometryCoordinate(
+                drivetrain.odomTField.asPose2d().transformBy(targetCoordinate.toTransform2d())
+            )
+        } else {
+            targetCoordinate as FrameCoordinate.OdometryCoordinate // Always true, kotlin type casting sucks sometimes
+        }
+    }
+
+    override fun execute() {
+        Logger.recordOutput("ActiveCommands/TargetPoseCommand", true)
+
+        val currentPose = drivetrain.odomTRobot
+        val currentFieldVelocity = fieldVelocitySupplier()
+        val relativeToRobotPose = odometryCoordinateToTarget.toPose2d().relativeTo(currentPose)
+
+        desiredAngle = currentPose.rotation + atan2(relativeToRobotPose.y.inMeters, relativeToRobotPose.x.inMeters).radians
+
+        val thetaFeedback = thetaPID.calculate(currentPose.rotation, desiredAngle)
+
+        drivetrain.currentRequest =
+            Request.DrivetrainRequest.OpenLoop(
+                thetaFeedback,
+                driver.driveSpeedClampedSupplier(driveX, driveY, slowMode),
+                fieldOriented = true
+            )
+
+        Logger.recordOutput("AutoLevel/CurrentYawDegrees", drivetrain.odomTRobot.rotation.inDegrees)
+        Logger.recordOutput("AutoLevel/DesiredYawDegrees", desiredAngle.inDegrees)
+        Logger.recordOutput("AutoLevel/thetaFeedbackDPS", thetaFeedback.inDegreesPerSecond)
+    }
+
+    override fun isFinished(): Boolean {
+        return (drivetrain.odomTRobot.rotation - desiredAngle).absoluteValue <
+                DrivetrainConstants.PID.AUTO_THETA_ALLOWED_ERROR
+    }
+
+    override fun end(interrupted: Boolean) {
+        drivetrain.currentRequest =
+            Request.DrivetrainRequest.OpenLoop(
+                driver.rotationSpeedClampedSupplier(turn, slowMode),
+                driver.driveSpeedClampedSupplier(driveX, driveY, slowMode),
+                fieldOriented = true
+            )
+        Logger.recordOutput("ActiveCommands/TargetPoseCommand", false)
+    }
+}

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

@@ -5,6 +5,10 @@ import com.team4099.robot2023.util.driver.DriverProfile
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
+import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.LinearVelocity
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.perSecond
 import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 
 class TeleopDriveCommand(

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

@@ -1,7 +1,19 @@
 package com.team4099.robot2023.config.constants
 
+import org.team4099.lib.geometry.Pose2d
 import org.team4099.lib.units.base.feet
+import org.team4099.lib.units.base.inches
+import org.team4099.lib.units.base.seconds
+import org.team4099.lib.units.derived.degrees
 
 object FieldConstants {
   val fieldLength = 54.feet
+
+  val bottomRightSpeaker = Pose2d(0.0.inches, 238.815.inches, 0.degrees)
+  val bottomLeftSpeaker = Pose2d(0.0.inches, 197.765.inches, 0.degrees)
+  val topRightSpeaker = Pose2d(18.055.inches, 238.815.inches, 0.degrees)
+  val topLeftSpeaker = Pose2d(18.055.inches, 197.765.inches, 0.degrees)
+
+  // Center of the speaker opening for the blue alliance
+  val centerSpeakerOpening = bottomLeftSpeaker.interpolate(topRightSpeaker, 0.5.seconds)
 }

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

@@ -33,7 +33,7 @@ sealed interface Request {
 
   sealed interface DrivetrainRequest : Request {
     class OpenLoop(
-      val angularVelocity: AngularVelocity,
+      var angularVelocity: AngularVelocity,
       val driveVector: Pair<LinearVelocity, LinearVelocity>,
       val fieldOriented: Boolean = true
     ) : DrivetrainRequest

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

@@ -0,0 +1,30 @@
+package com.team4099.robot2023.util
+
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.geometry.Transform2d
+import org.team4099.lib.geometry.Translation2d
+import org.team4099.lib.units.base.Length
+import org.team4099.lib.units.derived.degrees
+
+abstract class FrameCoordinate(val x: Length, val y: Length) {
+
+    fun toTransform2d(): Transform2d {
+        return Transform2d(Translation2d(x, y), 0.degrees)
+    }
+
+    fun toPose2d(): Pose2d {
+        return Pose2d(x, y, 0.degrees)
+    }
+
+    class OdometryCoordinate(x: Length, y: Length) : FrameCoordinate(x, y) {
+        constructor (pose2d: Pose2d) : this(pose2d.translation.x, pose2d.translation.y)
+    }
+
+    class FieldCoordinate(x: Length, y: Length) : FrameCoordinate(x, y) {
+        constructor (pose2d: Pose2d) : this(pose2d.translation.x, pose2d.translation.y)
+    }
+
+    companion object {
+        lateinit var robotInOdometryFrameSupplier: () -> Pose2d
+    }
+}