fixed optimization of rotation

primo4586 · Jan 19, 2024 · f440afd · f440afd
1 parent 2b042ad
commit f440afd
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 12 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -59,7 +59,7 @@ public static final class IntakeArm{ //TODO: This must be tuned to specific robo
     }
     public static final class Swerve {
         public static final double minimumErrorAligning = 0; // TODO: This must be tuned to specific robot
-        public static final PIDController swervePID = new PIDController(0, 0, 0);
+        public static final PIDController aligningPID = new PIDController(0, 0, 0);
 
         public static final int pigeonID = 10;
         public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW-

diff --git a/src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.java b/src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.java
@@ -4,22 +4,18 @@
 
 package frc.robot.commands.SwerveCommands;
 
-import static frc.robot.Constants.IntakeArm.minimumError;
 import static frc.robot.Constants.Swerve.*;
 
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.Utils.vision.Vision;
-import frc.robot.commands.SwerveCommands.*;
 import frc.robot.subsystems.SwerveSubsystem;
 
 public class AlignToSpeaker extends Command {
   /** Creates a new TurnToDegree. */
   SwerveSubsystem swerve = SwerveSubsystem.getInstance();
-  private PIDController pid = swervePID;
+  private PIDController pid = aligningPID;
   private Vision vision = Vision.getInstance();
   double angleFromTarget = vision.GetAngleFromTarget().getDegrees();
 
@@ -35,7 +31,7 @@ public void initialize() {}
   @Override
   public void execute() {
     angleFromTarget = vision.GetAngleFromTarget().getDegrees();
-    optimize();
+    calculateOptimalRotation();
     double rotation = pid.calculate(angleFromTarget, 180);
     swerve.drive(new Translation2d(), rotation, true, false, false);
   }
@@ -50,10 +46,18 @@ public boolean isFinished() {
     return Math.abs(vision.GetAngleFromTarget().getDegrees() - 180) < minimumErrorAligning ;
   }
 
-  public void optimize() {
-    double delta = 180 - angleFromTarget;
-    if (Math.abs(delta) > 90){
-        angleFromTarget = delta > 90 ? (angleFromTarget -= 180) : (angleFromTarget += 180);
-    }
+  public void calculateOptimalRotation() {
+    double currentRotation = angleFromTarget;
+    double targetRotation = 180;
+
+    // Calculate the difference between the target and current rotation
+    double rotationDifference = (targetRotation - currentRotation) % 360;
+
+    // Choose the optimal direction
+    double optimalRotation = (rotationDifference <= 360 / 2)
+        ? rotationDifference
+        : rotationDifference - 360;
+
+    angleFromTarget = optimalRotation;
   }
 }