From 5a3849bd51738ab50f835ef378ad0c083d4729ef Mon Sep 17 00:00:00 2001
From: ProfessorAtomicManiac
 <59379639+ProfessorAtomicManiac@users.noreply.github.com>
Date: Thu, 23 Mar 2023 18:42:47 -0700
Subject: [PATCH] added buttons to rotate 35 degrees

---
 .../java/org/carlmontrobotics/robotcode2023/Constants.java   | 5 ++++-
 .../org/carlmontrobotics/robotcode2023/RobotContainer.java   | 5 +++++
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/Constants.java b/src/main/java/org/carlmontrobotics/robotcode2023/Constants.java
index 4e26db7b..2ad746fd 100644
--- a/src/main/java/org/carlmontrobotics/robotcode2023/Constants.java
+++ b/src/main/java/org/carlmontrobotics/robotcode2023/Constants.java
@@ -108,7 +108,7 @@ public static final class Drivetrain {
         public static final SwerveConfig swerveConfig = new SwerveConfig(wheelDiameterMeters, driveGearing, mu, autoCentripetalAccel, kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZero, driveInversion, reversed, driveModifier, turnInversion);
 
         public static final Limelight.Transform limelightTransformForPoseEstimation = Transform.BOTPOSE_WPIBLUE;
-
+        public static final double turnFaceLimelightDeg = 35;
         //#endregion
 
         //#region Ports
@@ -390,6 +390,9 @@ public static final class OI {
         public static final class Driver {
             public static final int port = 0;
 
+            public static final Axis angleLeftButton = Axis.kLeftTrigger;
+            public static final Axis angleRightButton = Axis.kRightTrigger;
+            
             public static final int slowDriveButton = Button.kLeftBumper.value;
             public static final int chargeStationAlignButton = Button.kBack.value;
             public static final int resetFieldOrientationButton = Button.kRightBumper.value;
diff --git a/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java b/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java
index 399b32c8..fa6dc48a 100644
--- a/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java
+++ b/src/main/java/org/carlmontrobotics/robotcode2023/RobotContainer.java
@@ -5,6 +5,7 @@
 package org.carlmontrobotics.robotcode2023;
 
 import static org.carlmontrobotics.robotcode2023.Constants.OI.MIN_AXIS_TRIGGER_VALUE;
+import static org.carlmontrobotics.robotcode2023.Constants.Drivetrain.turnFaceLimelightDeg;
 
 import java.util.HashMap;
 import java.util.function.BooleanSupplier;
@@ -25,6 +26,7 @@
 import org.carlmontrobotics.robotcode2023.subsystems.Drivetrain;
 import org.carlmontrobotics.robotcode2023.subsystems.Roller;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Joystick;
@@ -90,6 +92,9 @@ private void configureButtonBindingsDriver() {
     new JoystickButton(driverController, Driver.rotateToFieldRelativeAngle90Deg).onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(-90), drivetrain));
     new JoystickButton(driverController, Driver.rotateToFieldRelativeAngle180Deg).onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(180), drivetrain));
     new JoystickButton(driverController, Driver.rotateToFieldRelativeAngle270Deg).onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(90), drivetrain));
+    axisTrigger(driverController, Driver.angleLeftButton).onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(MathUtil.inputModulus(drivetrain.getHeadingDeg() - turnFaceLimelightDeg, -180, 180)), drivetrain));
+    axisTrigger(driverController, Driver.angleRightButton).onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(MathUtil.inputModulus(drivetrain.getHeadingDeg() + turnFaceLimelightDeg, -180, 180)), drivetrain));
+    
   }
 
   private void configureButtonBindingsManipulator() {