From 20f324ebf8b10843f86fb6589e5b0b827e2fc21b Mon Sep 17 00:00:00 2001
From: John-Cams <143854397+John-Cams@users.noreply.github.com>
Date: Sat, 26 Oct 2024 11:40:30 -0700
Subject: [PATCH] Added turntoAngle

Binded turntoAngle to a controller button.

Co-Authored-By: ShasshU <143851930+ShasshU@users.noreply.github.com>
Co-Authored-By: Newbie9012 <175752595+Newbie9012@users.noreply.github.com>
---
 src/main/java/frc/robot/RobotContainer.java           |  7 ++++---
 .../frc/robot/commands/SwerveJoystickCommand.java     |  6 +++++-
 .../frc/robot/subsystems/swerve/SwerveDrivetrain.java | 11 +++++++++++
 3 files changed, 20 insertions(+), 4 deletions(-)

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index a4ee7ad..baa2530 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -76,7 +76,6 @@ public class RobotContainer {
   
   public CANdleSubSystem CANdle = new CANdleSubSystem();
   private SwerveJoystickCommand swerveJoystickCommand;
-  public boolean turnToAngleMode = false;
   
   /**
    * The container for the robot. Contain
@@ -171,7 +170,7 @@ public void initDefaultCommands_teleop() {
         // driverController::getR2Button, // Precision/"Sniper Button"
         () -> driverController.getR2Button(), // Precision mode (disabled)
         () -> {
-          if (turnToAngleMode) {
+          if (swerveDrive.getTurnToAngleMode()) {
             return (
             driverController.getRightX() > 0.0 
             || driverController.getRightY() > 0.0
@@ -306,6 +305,8 @@ public void configureBindings_teleop() {
       )
     );
 
+    
+
     commandOperatorController.povUp().onTrue(
       Commands.runOnce(() -> superSystem.incrementOffset(0.5))
     );
@@ -317,7 +318,7 @@ public void configureBindings_teleop() {
     );
     commandDriverController.touchpad().whileTrue(superSystem.shoot())
                                       .whileFalse(superSystem.stow());
-    commandDriverController.
+    commandDriverController.triangle().onTrue(swerveDrive.toggleTurnToAngleMode());
     
     commandDriverController.L2().whileTrue(Commands.sequence(
       Commands.race(
diff --git a/src/main/java/frc/robot/commands/SwerveJoystickCommand.java b/src/main/java/frc/robot/commands/SwerveJoystickCommand.java
index f176ae8..614e323 100644
--- a/src/main/java/frc/robot/commands/SwerveJoystickCommand.java
+++ b/src/main/java/frc/robot/commands/SwerveJoystickCommand.java
@@ -16,6 +16,7 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.Constants.ControllerConstants;
 import frc.robot.Constants.SwerveAutoConstants;
 import frc.robot.Constants.SwerveDriveConstants;
@@ -26,7 +27,6 @@
 import frc.robot.util.filters.Filter;
 import frc.robot.util.filters.FilterSeries;
 import frc.robot.util.filters.ScaleFilter;
-
 public class SwerveJoystickCommand extends Command {
     private final SwerveDrivetrain swerveDrive;
     private final Supplier<Double> xSpdFunction, ySpdFunction, turningSpdFunction;
@@ -122,6 +122,7 @@ public void initialize() {}
 
     @Override
     public void execute() {
+
         if (towSupplier.get()) {
             swerveDrive.setModuleStates(SwerveDriveConstants.towModuleStates);
             return;
@@ -199,6 +200,9 @@ public void execute() {
         
     }
 
+    
+    
+
     public double getTargetAngle() {
         return targetAngle;
     }
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java
index 489497e..39abe0f 100644
--- a/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java
+++ b/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java
@@ -479,6 +479,17 @@ public Command turnToSubwoofer(double angleTolerance) {
         command.addRequirements(this);
         return command;
     }
+
+    public boolean turnToAngleMode = false;
+
+    public Command toggleTurnToAngleMode() {
+      return Commands.runOnce(() -> turnToAngleMode = !turnToAngleMode);
+    }
+
+    public boolean getTurnToAngleMode() {
+        return turnToAngleMode;
+    }
+
     /**
      * Get the position of each swerve module
      * @return An array of swerve module positions