From 9ae44c73f48bc5fbe91abe3bf507dfe43951eebd Mon Sep 17 00:00:00 2001
From: Saraansh Wadkar <saraanshwadkar@gmail.com>
Date: Sat, 2 Mar 2024 12:53:16 -0500
Subject: [PATCH] fix drivetrain zeroing

---
 .../com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt    | 6 +++---
 .../robot2023/commands/drivetrain/DrivePathCommand.kt       | 6 +++---
 .../robot2023/subsystems/drivetrain/drive/Drivetrain.kt     | 5 ++++-
 .../robot2023/subsystems/superstructure/Superstructure.kt   | 5 ++---
 src/main/kotlin/com/team4099/robot2023/util/FMSData.kt      | 1 +
 5 files changed, 13 insertions(+), 10 deletions(-)

diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt
index 9ba606ac..b3920e52 100644
--- a/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt
+++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/FourNoteAutoPath.kt
@@ -46,7 +46,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
               )
             )
           },
-          keepTrapping = true
+          keepTrapping = false
         ),
         WaitCommand(0.5).andThen(superstructure.groundIntakeCommand())
       ),
@@ -97,7 +97,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
               )
             )
           },
-          keepTrapping = true
+          keepTrapping = false
         ),
         WaitCommand(1.0).andThen(superstructure.groundIntakeCommand())
       ),
@@ -148,7 +148,7 @@ class FourNoteAutoPath(val drivetrain: Drivetrain, val superstructure: Superstru
               ) // Subwoofer
             )
           },
-          keepTrapping = true
+          keepTrapping = false
         ),
         WaitCommand(0.5).andThen(superstructure.groundIntakeCommand())
       ),
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
index 39e9ed1c..4caa9db9 100644
--- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
+++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
@@ -217,7 +217,7 @@ private constructor(
     if (keepTrapping) {
       swerveDriveController.setTolerance(Pose2d(3.inches, 3.inches, 2.5.degrees).pose2d)
     } else {
-      swerveDriveController.setTolerance(Pose2d(6.inches, 6.inches, 5.degrees).pose2d)
+      swerveDriveController.setTolerance(Pose2d(6.inches, 6.inches, 10.degrees).pose2d)
     }
   }
 
@@ -420,8 +420,8 @@ private constructor(
   override fun isFinished(): Boolean {
     trajCurTime = Clock.fpgaTime - trajStartTime
     return endPathOnceAtReference &&
-      (swerveDriveController.atReference()) &&
-      trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds
+            ((swerveDriveController.atReference()) &&
+      trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds)
   }
 
   override fun end(interrupted: Boolean) {
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
index 625faa32..98862db4 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
@@ -425,6 +425,9 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
     val allianceFlippedDriveVector =
       Pair(driveVector.first * flipDrive, driveVector.second * flipDrive)
 
+    Logger.recordOutput("Drivetrain/driveVectorFirst", allianceFlippedDriveVector.first.inMetersPerSecond)
+    Logger.recordOutput("Drivetrain/driveVectorSecond", allianceFlippedDriveVector.second.inMetersPerSecond)
+
     val swerveModuleStates: Array<SwerveModuleState>
     var desiredChassisSpeeds: ChassisSpeeds
 
@@ -435,7 +438,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
           allianceFlippedDriveVector.first,
           allianceFlippedDriveVector.second,
           angularVelocity,
-          odomTRobot.rotation
+          odomTField.rotation
         )
     } else {
       desiredChassisSpeeds =
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
index 63af62ff..3e7a31ec 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
@@ -631,9 +631,8 @@ class Superstructure(
 
   fun groundIntakeCommand(): Command {
     val returnCommand =
-      runOnce { currentRequest = Request.SuperstructureRequest.GroundIntake() }.until {
-        isAtRequestedState && currentState == SuperstructureStates.GROUND_INTAKE
-      }
+      runOnce { currentRequest = Request.SuperstructureRequest.GroundIntake() }
+
     returnCommand.name = "GroundIntakeCommand"
     return returnCommand
   }
diff --git a/src/main/kotlin/com/team4099/robot2023/util/FMSData.kt b/src/main/kotlin/com/team4099/robot2023/util/FMSData.kt
index 338f8e1e..d1c1e8a7 100644
--- a/src/main/kotlin/com/team4099/robot2023/util/FMSData.kt
+++ b/src/main/kotlin/com/team4099/robot2023/util/FMSData.kt
@@ -7,4 +7,5 @@ object FMSData {
 
   val isBlue: Boolean
     get() = DriverStation.getAlliance().get() == DriverStation.Alliance.Blue
+
 }