Good turn to angle 😋

nerdherd · Oct 26, 2024 · 625f07e · 625f07e
1 parent b4a7c76
commit 625f07e
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 19 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -172,8 +172,8 @@ public void initDefaultCommands_teleop() {
         () -> {
           if (swerveDrive.getTurnToAngleMode()) {
             return (
-            driverController.getRightX() > 0.0 
-            || driverController.getRightY() > 0.0
+            Math.abs(driverController.getRightX()) > 0.05 
+            || Math.abs(driverController.getRightY()) > 0.05
             );
           } 
           else {
@@ -182,23 +182,16 @@ public void initDefaultCommands_teleop() {
         }, 
         // () -> false, // Turn to angle (disabled)
         () -> { // Turn To angle Direction
-          if (Math.abs(driverController.getRightY()) > Math.abs(driverController.getRightX())) {
-            if (driverController.getRightY() < 0) {
-            return 0.0;
+          double xValue = commandDriverController.getRightX();
+            double yValue = commandDriverController.getRightY();
+            double magnitude = Math.sqrt((xValue*xValue) + (yValue*yValue));
+            if (magnitude > 0.49) {
+              double angle = (90 + NerdyMath.radiansToDegrees(Math.atan2(commandDriverController.getRightY(), commandDriverController.getRightX())));
+              angle = (((-1 * angle) % 360) + 360) % 360;
+              SmartDashboard.putNumber("desired angle", angle);
+              return angle;
             }
-            else if (driverController.getRightY() > 0) {
-              return 180.0;
-            } 
-          }
-          else if ((Math.abs(driverController.getRightY()) < Math.abs(driverController.getRightX()))) {
-            if (driverController.getRightX() < 0) {
-              return 90.0;
-            }
-            else if (driverController.getRightX() > 0) {
-              return 270.0;
-            }
-          }
-          return 0.0;
+            return 1000.0;
         });
 
       swerveDrive.setDefaultCommand(swerveJoystickCommand);

diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/swerve/SwerveDrivetrain.java
@@ -480,7 +480,7 @@ public Command turnToSubwoofer(double angleTolerance) {
         return command;
     }
 
-    public boolean turnToAngleMode = false;
+    public boolean turnToAngleMode = true;
 
     public Command toggleTurnToAngleMode() {
       return Commands.runOnce(() -> turnToAngleMode = !turnToAngleMode);