change to odo/field frames

simgui-ds.json

@@ -1,9 +1,4 @@
 {
-  "Keyboard 0 Settings": {
-    "window": {
-      "visible": true
-    }
-  },
   "System Joysticks": {
     "window": {
       "visible": false

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 = 267
-const val GIT_SHA = "8a0440af3f7d960133b2b2ff5047c68c4f771c09"
-const val GIT_DATE = "2024-02-20T12:14:36Z"
+const val GIT_REVISION = 270
+const val GIT_SHA = "441e0228f8376921c8ade0367010a84d394f3994"
+const val GIT_DATE = "2024-02-20T15:57:31Z"
 const val GIT_BRANCH = "targeting-command-with-drivetrain"
-const val BUILD_DATE = "2024-02-20T12:54:28Z"
-const val BUILD_UNIX_TIME = 1708451668558L
+const val BUILD_DATE = "2024-02-20T18:35:21Z"
+const val BUILD_UNIX_TIME = 1708472121142L
 const val DIRTY = 1

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

@@ -33,6 +33,7 @@ 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
@@ -94,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() {
@@ -164,7 +166,7 @@ object RobotContainer {
     ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand())
     ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand())
     ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand())
-    ControlBoard.wristTestDown.whileTrue(TargetPoseCommand(drivetrain, FieldConstants.centerSpeakerOpening))
+    ControlBoard.wristTestDown.whileTrue(TargetPoseCommand(drivetrain, FrameCoordinate.FieldCoordinate(FieldConstants.centerSpeakerOpening)))
 
     /*
     TUNING COMMANDS

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

@@ -1,11 +1,16 @@
 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 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
@@ -32,12 +37,12 @@ import org.team4099.lib.units.perSecond
 import kotlin.math.PI
 import kotlin.math.atan2
 
-class TargetPoseCommand(val drivetrain: Drivetrain, targetPose: Pose2d) : Command() {
+class TargetPoseCommand(val drivetrain: Drivetrain, targetCoordinate: FrameCoordinate) : Command() {
     private val thetaPID: ProfiledPIDController<Radian, Velocity<Radian>>
     private val fieldVelocitySupplier = { drivetrain.fieldVelocity.x to drivetrain.fieldVelocity.y }
-    val targetPose = if (FMSData.allianceColor == DriverStation.Alliance.Red) {
-        Pose2d(FieldConstants.fieldLength - targetPose.x, targetPose.y, targetPose.rotation)
-    } else targetPose
+    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(
@@ -65,12 +70,11 @@ class TargetPoseCommand(val drivetrain: Drivetrain, targetPose: Pose2d) : Comman
     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)
 
-        Logger.recordOutput("Odometry/targetedPose", doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians))
-
         if (RobotBase.isReal()) {
             thetakP.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KP)
             thetakI.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KI)
@@ -92,15 +96,24 @@ class TargetPoseCommand(val drivetrain: Drivetrain, targetPose: Pose2d) : Comman
     }
 
     override fun initialize() {
-        thetaPID.reset(drivetrain.odometryPose.rotation)
+        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.odometryPose
+        val currentPose = drivetrain.odomTRobot
         val currentFieldVelocity = fieldVelocitySupplier()
-        val relativeToRobotPose = targetPose.relativeTo(currentPose)
+        val relativeToRobotPose = odometryCoordinateToTarget.toPose2d().relativeTo(currentPose)
+
         desiredAngle = currentPose.rotation + atan2(relativeToRobotPose.y.inMeters, relativeToRobotPose.x.inMeters).radians
 
         val thetaFeedback = thetaPID.calculate(currentPose.rotation, desiredAngle)
@@ -112,20 +125,20 @@ class TargetPoseCommand(val drivetrain: Drivetrain, targetPose: Pose2d) : Comman
                 fieldOriented = true
             )
 
-        Logger.recordOutput("AutoLevel/CurrentYawDegrees", drivetrain.odometryPose.rotation.inDegrees)
+        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.odometryPose.rotation - desiredAngle).absoluteValue <
+        return (drivetrain.odomTRobot.rotation - desiredAngle).absoluteValue <
                 DrivetrainConstants.PID.AUTO_THETA_ALLOWED_ERROR
     }
 
     override fun end(interrupted: Boolean) {
         drivetrain.currentRequest =
             Request.DrivetrainRequest.OpenLoop(
-                0.0.radians.perSecond, Pair(drivetrain.fieldVelocity.x, drivetrain.fieldVelocity.y)
+                0.0.radians.perSecond, fieldVelocitySupplier()
             )
     }
 }

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
+    }
+}