Binded driver leftJoystickButton and selectButton to SetZeroCommand

team4099 · Aug 14, 2024 · ef3f0b0 · ef3f0b0
1 parent a813b55
commit ef3f0b0
Show file tree

Hide file tree

Showing 3 changed files with 11 additions and 0 deletions.
diff --git 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
@@ -1,16 +1,22 @@
 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 {
         addRequirements(drivetrain)
     }
 
     override fun initialize() {
+        val currentPositions: List<Double> = drivetrain.swerveModules.map { swerveModule ->
+            swerveModule.inputs.steerPosition.inRadians
+        }
     }
 
     override fun execute() {
+        CustomLogger.recordDebugOutput("ActiveCommands/SetZeroCommand", true)
     }
 }
diff --git 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 }