working frame irl

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

@@ -35,7 +35,6 @@ import com.team4099.robot2023.util.driver.Ryan
 import edu.wpi.first.wpilibj.RobotBase
 import org.team4099.lib.smoothDeadband
 import org.team4099.lib.units.derived.Angle
-import org.team4099.lib.units.derived.degrees
 import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 
 object RobotContainer {
@@ -98,8 +97,8 @@ object RobotContainer {
     drivetrain.defaultCommand =
       TeleopDriveCommand(
         driver = Ryan(),
-        { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
-        { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
+        { -ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
+        { -ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
         { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) },
         { ControlBoard.slowMode },
         drivetrain
@@ -150,7 +149,7 @@ object RobotContainer {
 
   fun mapTeleopControls() {
 
-    ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 180.degrees))
+    ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain))
     ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand())
     ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand())
     ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand())

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

@@ -1,6 +1,6 @@
 package com.team4099.robot2023.auto
 
-import com.team4099.robot2023.auto.mode.TestAutoPath
+import com.team4099.robot2023.auto.mode.ExamplePathAuto
 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(TestAutoPath(drivetrain))
+        return WaitCommand(waitTime.inSeconds).andThen(ExamplePathAuto(drivetrain))
       else -> println("ERROR: unexpected auto mode: $mode")
     }
     return InstantCommand()

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

@@ -1,13 +1,15 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.pathplanner.lib.path.PathPlannerPath
+import com.pathplanner.lib.path.PathPlannerTrajectory
 import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.lib.math.asPose2d
 import com.team4099.lib.math.asTransform2d
 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.FrameType
+import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.math.kinematics.ChassisSpeeds
 import edu.wpi.first.wpilibj2.command.Command
 import org.littletonrobotics.junction.Logger
@@ -45,8 +47,8 @@ import org.team4099.lib.units.perSecond
 class FollowPathPlannerPathCommand(
   val drivetrain: Drivetrain,
   val path: PathPlannerPath,
-  pathFrame: FrameType = FrameType.ODOMETRY,
-  var stateFrame: FrameType = FrameType.FIELD,
+  var stateFrame: FrameType = FrameType.ODOMETRY,
+  var pathFrame: FrameType = FrameType.FIELD,
 ) : Command() {
   private val translationToleranceAtEnd = 1.inches
   private val thetaToleranceAtEnd = 2.5.degrees
@@ -133,12 +135,12 @@ class FollowPathPlannerPathCommand(
   init {
     addRequirements(drivetrain)
 
-    when (pathFrame) {
+    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
-        stateFrame = FrameType.FIELD
+        pathFrame = FrameType.FIELD
       }
     }
 
@@ -159,9 +161,17 @@ class FollowPathPlannerPathCommand(
       )
   }
 
+  private lateinit var currentSpeeds: ChassisSpeeds
+  private lateinit var poseRotation: Rotation2d
+  private lateinit var generatedTrajectory: PathPlannerTrajectory
+
   override fun initialize() {
     trajStartTime = Clock.fpgaTime
     odoTField = drivetrain.odomTField
+
+    currentSpeeds = drivetrain.targetedChassisSpeeds
+    poseRotation = drivePoseSupplier().rotation.inRotation2ds
+    generatedTrajectory = path.getTrajectory(currentSpeeds, poseRotation)
   }
 
   override fun execute() {
@@ -197,10 +207,6 @@ class FollowPathPlannerPathCommand(
         )
     }
 
-    val currentSpeeds = drivetrain.targetedChassisSpeeds
-    val poseRotation = drivePoseSupplier().rotation.inRotation2ds
-    val generatedTrajectory = path.getTrajectory(currentSpeeds, poseRotation)
-
     // Sampling the trajectory for a state that we're trying to target
     val stateFromTrajectory = generatedTrajectory.sample(trajCurTime.inSeconds)
 
@@ -213,29 +219,32 @@ class FollowPathPlannerPathCommand(
         lastSampledPose.rotation.inRadians
       )
     )
-    val stateFrameRobotPose: Pose2d
-    when (stateFrame) {
-      FrameType.FIELD -> {
-        lastSampledPose =
-          odoTField
-            .asPose2d()
-            .transformBy(
-              Pose2d(stateFromTrajectory.targetHolonomicPose).asTransform2d().inverse()
-            )
-        stateFrameRobotPose =
-          odoTField.inverse().asPose2d().transformBy(drivePoseSupplier().asTransform2d())
-      }
-      FrameType.ODOMETRY -> {
-        lastSampledPose = Pose2d(stateFromTrajectory.targetHolonomicPose)
-        stateFrameRobotPose = drivePoseSupplier()
+    val robotPoseInSelectedFrame: Pose2d = drivePoseSupplier()
+    if (pathFrame == stateFrame) {
+      lastSampledPose = Pose2d(stateFromTrajectory.targetHolonomicPose)
+    } else {
+      when (pathFrame) {
+        FrameType.ODOMETRY ->
+          lastSampledPose =
+            odoTField
+              .asPose2d()
+              .transformBy(Pose2d(stateFromTrajectory.targetHolonomicPose).asTransform2d())
+        FrameType.FIELD ->
+          lastSampledPose =
+            odoTField
+              .inverse()
+              .asPose2d()
+              .transformBy(Pose2d(stateFromTrajectory.targetHolonomicPose).asTransform2d())
       }
     }
 
     Logger.recordOutput("Pathfollow/thetaSetpointDegrees", lastSampledPose.rotation.inDegrees)
     Logger.recordOutput("Pathfollow/currentThetaDegrees", drivePoseSupplier().rotation.inDegrees)
 
     val targetedChassisSpeeds =
-      swerveDriveController.calculateRobotRelativeSpeeds(stateFrameRobotPose, stateFromTrajectory)
+      swerveDriveController.calculateRobotRelativeSpeeds(
+        robotPoseInSelectedFrame, stateFromTrajectory
+      )
 
     // Set closed loop request
     pathFollowRequest.chassisSpeeds = targetedChassisSpeeds.chassisSpeedsWPILIB
@@ -252,7 +261,7 @@ class FollowPathPlannerPathCommand(
   //                trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds
   //    }
   override fun isFinished(): Boolean {
-    return atReference
+    return atReference && trajCurTime.inSeconds > generatedTrajectory.totalTimeSeconds
   }
 
   override fun end(interrupted: Boolean) {

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 = 3.meters.perSecond // 4
-  val MAX_AUTO_ACCEL = 3.meters.perSecond.perSecond // 3
+  val MAX_AUTO_VEL = 1.meters.perSecond // 4
+  val MAX_AUTO_ACCEL = 1.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
@@ -151,7 +151,7 @@ object DrivetrainConstants {
 
     val STEERING_KFF = 0.0.volts / 1.0.radians.perSecond // 0.0375
 
-    val DRIVE_KP = 2.6829.volts / 1.meters.perSecond
+    val DRIVE_KP = 0.0.volts / 1.meters.perSecond
     val DRIVE_KI = 0.0.volts / (1.meters.perSecond * 1.seconds)
     val DRIVE_KD = 0.0.volts / 1.meters.perSecond.perSecond
 

src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt

@@ -2,6 +2,7 @@ package com.team4099.robot2023.subsystems.drivetrain.drive
 
 import com.team4099.lib.hal.Clock
 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.vision.TimestampedVisionUpdate
@@ -299,7 +300,9 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
         .pose3d
     )
 
-    Logger.recordOutput("FieldFrameEstimator/odomTField", odomTField.transform2d)
+    Logger.recordOutput(
+      "FieldFrameEstimator/odomTField", odomTField.toDoubleArray().toDoubleArray()
+    )
 
     Logger.recordOutput(
       "Odometry/targetPose",
@@ -577,6 +580,10 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
       Pose2d(odomTRobot.x, odomTRobot.y, toAngle).pose2d
     )
 
+    fieldFrameEstimator.resetFieldFrameFilter(
+      Transform2d(odomTField.translation, gyroInputs.gyroYaw)
+    )
+
     if (!(gyroInputs.gyroConnected)) {
       gyroYawOffset = toAngle - rawGyroAngle
     }

src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt

@@ -10,6 +10,7 @@ import com.ctre.phoenix6.controls.DutyCycleOut
 import com.ctre.phoenix6.controls.PositionDutyCycle
 import com.ctre.phoenix6.controls.VelocityDutyCycle
 import com.ctre.phoenix6.hardware.TalonFX
+import com.ctre.phoenix6.signals.InvertedValue
 import com.ctre.phoenix6.signals.NeutralModeValue
 import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.config.constants.DrivetrainConstants
@@ -104,9 +105,7 @@ class SwerveModuleIOTalon(
     steeringConfiguration.CurrentLimits.SupplyCurrentLimit =
       DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT.inAmperes
     steeringConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true
-    steeringConfiguration.ClosedLoopGeneral.ContinuousWrap = true
-    steeringConfiguration.Feedback.SensorToMechanismRatio =
-      1 / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO
+
     steeringConfiguration.MotorOutput.NeutralMode =
       NeutralModeValue.Brake // change back to coast maybe?
     steeringFalcon.inverted = true
@@ -118,7 +117,7 @@ class SwerveModuleIOTalon(
       driveSensor.integralVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KI)
     driveConfiguration.Slot0.kD =
       driveSensor.derivativeVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KD)
-    driveConfiguration.Slot0.kV = 0.05425
+    driveConfiguration.Slot0.kV = 0.12679 / 15
     //      driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF)
     driveConfiguration.CurrentLimits.SupplyCurrentLimit =
       DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes
@@ -132,6 +131,7 @@ class SwerveModuleIOTalon(
     driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = false // TODO tune
 
     driveConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake
+    driveConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive
     driveFalcon.configurator.apply(driveConfiguration)
 
     MotorChecker.add(
@@ -477,6 +477,7 @@ class SwerveModuleIOTalon(
     } else {
       motorOutputConfig.NeutralMode = NeutralModeValue.Coast
     }
+    motorOutputConfig.Inverted = InvertedValue.Clockwise_Positive
     driveFalcon.configurator.apply(motorOutputConfig)
   }