From ef3f0b068606ac1d3b496f79dc2d6043c942769e Mon Sep 17 00:00:00 2001
From: yamamara <yamamrtyu@gmail.com>
Date: Tue, 13 Aug 2024 23:16:40 -0400
Subject: [PATCH] Binded driver leftJoystickButton and selectButton to
 SetZeroCommand

---
 src/main/kotlin/com/team4099/robot2023/RobotContainer.kt    | 4 ++++
 .../robot2023/commands/drivetrain/SetZeroCommand.kt         | 6 ++++++
 .../kotlin/com/team4099/robot2023/config/ControlBoard.kt    | 1 +
 3 files changed, 11 insertions(+)

diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
index ac661f4a..05fd6b37 100644
--- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
+++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -4,6 +4,7 @@ import com.ctre.phoenix6.hardware.TalonFX
 import com.team4099.lib.logging.LoggedTunableValue
 import com.team4099.robot2023.auto.AutonomousSelector
 import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand
+import com.team4099.robot2023.commands.drivetrain.SetZeroCommand
 import com.team4099.robot2023.commands.drivetrain.TargetAngleCommand
 import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand
 import com.team4099.robot2023.config.ControlBoard
@@ -49,6 +50,7 @@ import org.team4099.lib.smoothDeadband
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.inDegrees
+import javax.naming.ldap.Control
 import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
 
 object RobotContainer {
@@ -219,6 +221,8 @@ object RobotContainer {
 
     ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain))
 
+    ControlBoard.setZeroes.whileTrue(SetZeroCommand(drivetrain))
+
     ControlBoard.intake.whileTrue(superstructure.groundIntakeCommand())
 
     /*
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SetZeroCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SetZeroCommand.kt
index 864b1d83..f5240e90 100644
--- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SetZeroCommand.kt
+++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SetZeroCommand.kt
@@ -1,7 +1,9 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.robot2023.subsystems.drivetrain.Drivetrain
+import com.team4099.robot2023.util.CustomLogger
 import edu.wpi.first.wpilibj2.command.Command
+import org.team4099.lib.units.derived.inRadians
 
 class SetZeroCommand(val drivetrain: Drivetrain) : Command() {
     init {
@@ -9,8 +11,12 @@ class SetZeroCommand(val drivetrain: Drivetrain) : Command() {
     }
 
     override fun initialize() {
+        val currentPositions: List<Double> = drivetrain.swerveModules.map { swerveModule ->
+            swerveModule.inputs.steerPosition.inRadians
+        }
     }
 
     override fun execute() {
+        CustomLogger.recordDebugOutput("ActiveCommands/SetZeroCommand", true)
     }
 }
\ No newline at end of file
diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt
index 8be60251..95f04d79 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt
@@ -41,6 +41,7 @@ object ControlBoard {
     get() = driver.leftShoulderButton
 
   val resetGyro = Trigger { driver.startButton && driver.selectButton }
+  val setZeroes = Trigger { driver.leftJoystickButton && driver.selectButton }
 
   // sim triggers
   val score = Trigger { driver.leftTriggerAxis > 0.5 }