work

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

@@ -13,7 +13,6 @@ import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO
 import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2
 import com.team4099.robot2023.subsystems.elevator.Elevator
 import com.team4099.robot2023.subsystems.elevator.ElevatorIO
-import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO
 import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim
 import com.team4099.robot2023.subsystems.feeder.Feeder
 import com.team4099.robot2023.subsystems.feeder.FeederIONeo
@@ -70,7 +69,7 @@ object RobotContainer {
       limelight = LimelightVision(object : LimelightVisionIO {})
       intake = Intake(IntakeIONEO)
       feeder = Feeder(FeederIONeo)
-      elevator = Elevator(object: ElevatorIO {})
+      elevator = Elevator(object : ElevatorIO {})
       flywheel = Flywheel(FlywheelIOTalon)
       wrist = Wrist(object : WristIO {})
     } else {
@@ -153,16 +152,16 @@ object RobotContainer {
   fun mapTeleopControls() {
 
     ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 0.degrees))
-    //ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand())
-//    ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand())
-//    ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand())
-//    ControlBoard.ampScore.whileTrue(superstructure.scoreAmpCommand())
-//    ControlBoard.scoreSpeakerLow.whileTrue(superstructure.scoreSpeakerLowCommand())
-//    ControlBoard.scoreSpeakerMid.whileTrue(superstructure.scoreSpeakerMidCommand())
-//    ControlBoard.scoreSpeakerHigh.whileTrue(superstructure.scoreSpeakerHighCommand())
-//    ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand())
-//    ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand())
-//    ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand())
+    // ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand())
+    //    ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand())
+    //    ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand())
+    //    ControlBoard.ampScore.whileTrue(superstructure.scoreAmpCommand())
+    //    ControlBoard.scoreSpeakerLow.whileTrue(superstructure.scoreSpeakerLowCommand())
+    //    ControlBoard.scoreSpeakerMid.whileTrue(superstructure.scoreSpeakerMidCommand())
+    //    ControlBoard.scoreSpeakerHigh.whileTrue(superstructure.scoreSpeakerHighCommand())
+    //    ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand())
+    //    ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand())
+    //    ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand())
     ControlBoard.runGroundIntake.whileTrue(ExamplePathAuto(drivetrain))
 
     /*

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
@@ -56,11 +56,11 @@ object AutonomousSelector {
     get() = secondaryWaitInAuto.getDouble(0.0).seconds
 
   fun getCommand(drivetrain: Drivetrain): Command {
-    val mode = autonomousModeChooser.get()
+    val mode = AutonomousMode.TEST_AUTO_PATH
     //    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/auto/mode/ExamplePathAuto.kt

@@ -2,16 +2,15 @@ package com.team4099.robot2023.auto.mode
 
 import com.team4099.robot2023.auto.PathStore
 import com.team4099.robot2023.commands.drivetrain.FollowPathPlannerPathCommand
-import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
-import org.team4099.lib.geometry.Pose2d
 
 class ExamplePathAuto(val drivetrain: Drivetrain) : SequentialCommandGroup() {
   init {
     addRequirements(drivetrain)
     addCommands(
-      ResetPoseCommand(drivetrain, Pose2d(PathStore.examplePath.previewStartingHolonomicPose)),
+      //      ResetPoseCommand(drivetrain,
+      // Pose2d(PathStore.examplePath.previewStartingHolonomicPose)),
       FollowPathPlannerPathCommand(drivetrain, PathStore.examplePath)
     )
   }

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

@@ -1,10 +1,12 @@
 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.robot2023.config.constants.DrivetrainConstants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import com.team4099.robot2023.subsystems.superstructure.Request
+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
@@ -38,8 +40,11 @@ import org.team4099.lib.units.derived.perMeterSeconds
 import org.team4099.lib.units.derived.radians
 import org.team4099.lib.units.perSecond
 
-class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPlannerPath) :
-  Command() {
+class FollowPathPlannerPathCommand(
+  val drivetrain: Drivetrain,
+  val path: PathPlannerPath,
+  val resetPose: Boolean = true
+) : Command() {
   private val translationToleranceAtEnd = 1.inches
   private val thetaToleranceAtEnd = 2.5.degrees
 
@@ -50,6 +55,11 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla
 
   private var pathFollowRequest = Request.DrivetrainRequest.ClosedLoop(ChassisSpeeds(0.0, 0.0, 0.0))
   private var lastSampledPose = Pose2d()
+
+  private lateinit var currentSpeeds: ChassisSpeeds
+  private lateinit var poseRotation: Rotation2d
+  private lateinit var generatedTrajectory: PathPlannerTrajectory
+
   private val atReference: Boolean
     get() =
       (
@@ -140,6 +150,12 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla
 
   override fun initialize() {
     trajStartTime = Clock.fpgaTime
+
+    drivetrain.odometryPose = Pose2d(path.previewStartingHolonomicPose)
+
+    currentSpeeds = drivetrain.targetedChassisSpeeds
+    poseRotation = drivetrain.odometryPose.rotation.inRotation2ds
+    generatedTrajectory = path.getTrajectory(currentSpeeds, poseRotation)
   }
 
   override fun execute() {
@@ -175,10 +191,6 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla
         )
     }
 
-    val currentSpeeds = drivetrain.targetedChassisSpeeds
-    val poseRotation = drivetrain.odometryPose.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)
 
@@ -198,7 +210,6 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla
     )
     Logger.recordOutput("Pathfollow/currentTheta", drivetrain.odometryPose.rotation.inDegrees)
 
-
     val targetedChassisSpeeds =
       swerveDriveController.calculateRobotRelativeSpeeds(
         drivetrain.odometryPose, stateFromTrajectory
@@ -219,7 +230,7 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla
   //                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/commands/drivetrain/ResetPoseCommand.kt

@@ -21,6 +21,7 @@ class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : Command()
   }
 
   override fun isFinished(): Boolean {
+    Logger.recordOutput("ActiveCommands/ResetPoseCommand", false)
     return true
   }
 }

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

@@ -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.0001.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/swervemodule/SwerveModuleIOTalon.kt

@@ -132,7 +132,7 @@ class SwerveModuleIOTalon(
       driveSensor.integralVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KI)
     driveConfiguration.Slot0.kD =
       driveSensor.derivativeVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KD)
-    driveConfiguration.Slot0.kV = 0.02
+    driveConfiguration.Slot0.kV = 0.1267939375649165 / 15 // kv too high rn
     //      driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF)
     driveConfiguration.CurrentLimits.SupplyCurrentLimit =
       DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes

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

@@ -1,5 +1,6 @@
 package com.team4099.robot2023.util
 
+import com.team4099.lib.hal.Clock
 import com.team4099.lib.vision.TimestampedVisionUpdate
 import edu.wpi.first.math.Matrix
 import edu.wpi.first.math.Nat
@@ -32,6 +33,7 @@ class PoseEstimator(stateStdDevs: Matrix<N3?, N1?>) {
 
   /** Resets the odometry to a known pose. */
   fun resetPose(pose: Pose2d) {
+    Logger.recordOutput("Odometry/resetPoseTime", Clock.fpgaTime.inSeconds)
     basePose = pose
     updates.clear()
     update()