progress without path planner

src/main/deploy/pathplanner/paths/Example Path.path

@@ -64,8 +64,8 @@
   "constraintZones": [],
   "eventMarkers": [],
   "globalConstraints": {
-    "maxVelocity": 1.0,
-    "maxAcceleration": 2.0,
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
     "maxAngularVelocity": 270.0,
     "maxAngularAcceleration": 600.0
   },

src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt

@@ -1,6 +1,6 @@
 package com.team4099.robot2023.auto
 
-import com.team4099.robot2023.auto.mode.ExamplePathAuto
+import com.team4099.robot2023.auto.mode.TestAutoPath
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import edu.wpi.first.networktables.GenericEntry
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets
@@ -61,7 +61,7 @@ object AutonomousSelector {
     //    println("${waitTime().inSeconds} wait command")
     when (mode) {
       AutonomousMode.TEST_AUTO_PATH ->
-        return WaitCommand(waitTime.inSeconds).andThen(ExamplePathAuto(drivetrain))
+        return WaitCommand(waitTime.inSeconds).andThen(TestAutoPath(drivetrain))
       else -> println("ERROR: unexpected auto mode: $mode")
     }
     return InstantCommand()

src/main/kotlin/com/team4099/robot2023/auto/mode/TestAutoPath.kt

@@ -19,17 +19,17 @@ class TestAutoPath(val drivetrain: Drivetrain) : SequentialCommandGroup() {
         {
           listOf(
             Waypoint(
-              Translation2d(0.0.meters, 0.0.meters).translation2d,
+              Translation2d(5.0.meters, 2.0.meters).translation2d,
               null,
               0.0.degrees.inRotation2ds
             ),
             Waypoint(
-              Translation2d(2.0.meters, 0.0.meters).translation2d,
+              Translation2d(7.0.meters, 2.0.meters).translation2d,
               null,
               90.0.degrees.inRotation2ds
             ),
             Waypoint(
-              Translation2d(0.0.meters, 1.0.meters).translation2d,
+              Translation2d(7.0.meters, 3.0.meters).translation2d,
               null,
               0.0.degrees.inRotation2ds
             ),

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

@@ -1,12 +1,17 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.lib.logging.LoggedTunableValue
+import com.team4099.lib.logging.toDoubleArray
+import com.team4099.lib.math.asPose2d
+import com.team4099.lib.math.asTransform2d
 import com.team4099.lib.trajectory.CustomHolonomicDriveController
 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.FrameType
 import com.team4099.robot2023.util.Velocity2d
 import edu.wpi.first.math.kinematics.ChassisSpeeds
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics
@@ -19,6 +24,8 @@ 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.geometry.Transform2d
+import org.team4099.lib.geometry.Translation2d
 import org.team4099.lib.hal.Clock
 import org.team4099.lib.kinematics.ChassisAccels
 import org.team4099.lib.units.Velocity
@@ -65,6 +72,8 @@ class DrivePathCommand(
   val endPathOnceAtReference: Boolean = true,
   val leaveOutYAdjustment: Boolean = false,
   val endVelocity: Velocity2d = Velocity2d(),
+  var stateFrame: FrameType = FrameType.ODOMETRY,
+  var pathFrame: FrameType = FrameType.ODOMETRY,
 ) : Command() {
   private val xPID: PIDController<Meter, Velocity<Meter>>
   private val yPID: PIDController<Meter, Velocity<Meter>>
@@ -128,6 +137,13 @@ class DrivePathCommand(
       )
     )
 
+  private var pathFollowRequest = Request.DrivetrainRequest.ClosedLoop(ChassisSpeeds(0.0, 0.0, 0.0))
+  private var lastSampledPose = Pose2d()
+  private lateinit var pathTransform: Transform2d
+
+  private var drivePoseSupplier: () -> Pose2d
+  private var odoTField: Transform2d = Transform2d(Translation2d(), 0.0.degrees)
+
   private fun generate(
     waypoints: List<Waypoint>,
     constraints: List<TrajectoryConstraint> = listOf()
@@ -173,6 +189,17 @@ class DrivePathCommand(
 
     thetaPID.enableContinuousInput(-PI.radians, PI.radians)
 
+    when (stateFrame) {
+      FrameType.ODOMETRY -> drivePoseSupplier = { drivetrain.odomTRobot }
+      FrameType.FIELD -> {
+        drivePoseSupplier = { drivetrain.fieldTRobot }
+        // if we're already in field frame we do not want to shift by `odoTField` again
+        pathFrame = FrameType.FIELD
+      }
+    }
+
+    drivePoseSupplier = { drivetrain.odomTRobot }
+
     swerveDriveController =
       CustomHolonomicDriveController(
         xPID.wpiPidController, yPID.wpiPidController, thetaPID.wpiPidController
@@ -182,6 +209,13 @@ class DrivePathCommand(
   }
 
   override fun initialize() {
+    odoTField = drivetrain.odomTField
+    pathTransform =
+      Transform2d(
+        Translation2d(waypoints.get()[0].translation),
+        waypoints.get()[0].driveRotation?.radians?.radians ?: 0.0.degrees
+      )
+
     // trajectory generation!
     generate(waypoints.get())
 
@@ -213,6 +247,36 @@ class DrivePathCommand(
     var desiredRotation =
       trajectoryGenerator.holonomicRotationSequence.sample(trajCurTime.inSeconds)
 
+    val targetHolonomicPose =
+      Pose2d(
+        desiredState.poseMeters.x.meters,
+        desiredState.poseMeters.y.meters,
+        desiredRotation.position.radians.radians
+      )
+
+    val robotPoseInSelectedFrame: Pose2d = drivePoseSupplier()
+    if (pathFrame == stateFrame) {
+      // odoTField x fieldTRobot
+      lastSampledPose = (targetHolonomicPose - pathTransform.asPose2d()).asPose2d()
+    } else {
+      when (pathFrame) {
+        FrameType.ODOMETRY ->
+          lastSampledPose = odoTField.asPose2d().transformBy(targetHolonomicPose.asTransform2d())
+        FrameType.FIELD ->
+          lastSampledPose =
+            odoTField.inverse().asPose2d().transformBy(targetHolonomicPose.asTransform2d())
+      }
+    }
+
+    val pathFrameTRobotPose = (robotPoseInSelectedFrame + pathTransform)
+    Logger.recordOutput(
+      "Pathfollow/fieldTRobotVisualized", pathFrameTRobotPose.toDoubleArray().toDoubleArray()
+    )
+    Logger.recordOutput(
+      "Pathfollow/fieldTRobotTargetVisualized",
+      targetHolonomicPose.toDoubleArray().toDoubleArray()
+    )
+
     if (flipForAlliances) {
       desiredState = AllianceFlipUtil.apply(desiredState)
       desiredRotation = AllianceFlipUtil.apply(desiredRotation)
@@ -226,7 +290,7 @@ class DrivePathCommand(
         desiredState.curvatureRadPerMeter.radians.sin
 
     var nextDriveState =
-      swerveDriveController.calculate(drivetrain.odomTRobot.pose2d, desiredState, desiredRotation)
+      swerveDriveController.calculate(pathFrameTRobotPose.pose2d, desiredState, desiredRotation)
 
     if (leaveOutYAdjustment) {
       nextDriveState =

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

@@ -2,6 +2,7 @@ package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 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 com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
@@ -22,10 +23,12 @@ class TeleopDriveCommand(
   override fun initialize() {}
 
   override fun execute() {
-    val speed = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode)
-    val rotation = driver.rotationSpeedClampedSupplier(turn, slowMode)
-    drivetrain.currentRequest = DrivetrainRequest.OpenLoop(rotation, speed)
-    Logger.recordOutput("ActiveCommands/TeleopDriveCommand", true)
+    if (DriverStation.isTeleop()) {
+      val speed = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode)
+      val rotation = driver.rotationSpeedClampedSupplier(turn, slowMode)
+      drivetrain.currentRequest = DrivetrainRequest.OpenLoop(rotation, speed)
+      Logger.recordOutput("ActiveCommands/TeleopDriveCommand", true)
+    }
   }
   override fun isFinished(): Boolean {
     return false

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

@@ -0,0 +1,25 @@
+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.math.kinematics.ChassisSpeeds
+import edu.wpi.first.wpilibj2.command.Command
+import org.littletonrobotics.junction.Logger
+import org.team4099.lib.units.derived.inRotation2ds
+
+class TestDriveCommand(val drivetrain: Drivetrain) : Command() {
+
+  override fun execute() {
+    drivetrain.currentRequest =
+      Request.DrivetrainRequest.ClosedLoop(
+        ChassisSpeeds.fromFieldRelativeSpeeds(
+          4.0, 0.0, 0.0, drivetrain.odomTRobot.rotation.inRotation2ds
+        )
+      )
+    Logger.recordOutput("ActiveCommands/TestDriveCommand", true)
+  }
+
+  override fun isFinished(): Boolean {
+    return false
+  }
+}

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

@@ -58,8 +58,8 @@ object DrivetrainConstants {
   val SLOW_AUTO_VEL = 2.meters.perSecond
   val SLOW_AUTO_ACCEL = 2.0.meters.perSecond.perSecond
 
-  val MAX_AUTO_VEL = 1.meters.perSecond // 4
-  val MAX_AUTO_ACCEL = 2.meters.perSecond.perSecond // 3
+  val MAX_AUTO_VEL = 3.meters.perSecond // 4
+  val MAX_AUTO_ACCEL = 3.meters.perSecond.perSecond // 3
 
   val MAX_AUTO_BRAKE_VEL = 0.5.meters.perSecond // 4
   val MAX_AUTO_BRAKE_ACCEL = 0.5.meters.perSecond.perSecond // 3