Added turntoAngle

Binded turntoAngle to a controller button.

Co-Authored-By: ShasshU <[email protected]>
Co-Authored-By: Newbie9012 <[email protected]>

Author: 3 people

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(

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;
     }

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