Merge pull request #9 from primo4586/swerve-turn-to-a-point

align to speaker

src/main/java/frc/robot/Constants.java

@@ -12,6 +12,7 @@
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
@@ -27,6 +28,7 @@
 import edu.wpi.first.math.numbers.N3;
 
 public final class Constants {
+
     public static final double stickDeadband = 0.1;
 
     public static final class IntakeArm{ //TODO: This must be tuned to specific robot
@@ -56,6 +58,9 @@ 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 aligningPID = new PIDController(0, 0, 0);
+
         public static final int pigeonID = 10;
         public static final boolean invertGyro = false; // Always ensure Gyro is CCW+ CW-
 
@@ -200,27 +205,27 @@ public static final class AutoConstants { //TODO: The below constants are used i
     }
 
     public static class Vision {
-    public static final String kRightCameraName = "Arducam_OV9281_USB_Camera";
-    public static final String kLeftCameraName = "YOUR CAMERA NAME";
-    // Cam mounted facing forward, half a meter forward of center, half a meter up
-    // from center.
-    public static final Transform3d kRightRobotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5),
-        new Rotation3d(0, 0, 0));
-    public static final Transform3d kLeftRobotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5),
-        new Rotation3d(0, 0, 0));
-
-    // The layout of the AprilTags on the field
-    public static final AprilTagFieldLayout kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); 
-
-    // The standard deviations of our vision estimated poses, which affect
-    // correction rate
-    // (Fake values. Experiment and determine estimation noise on an actual robot.)
-    public static final Matrix<N3, N1> kRightSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
-    public static final Matrix<N3, N1> kRightMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
-
-    public static final Matrix<N3, N1> kLeftSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
-    public static final Matrix<N3, N1> kLeftMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
-
-    public static final Pose2d target = new Pose2d(1, 1, new Rotation2d(Units.degreesToRadians(0)));
-  }
+        public static final String kRightCameraName = "Arducam_OV9281_USB_Camera";
+        public static final String kLeftCameraName = "YOUR CAMERA NAME";
+        // Cam mounted facing forward, half a meter forward of center, half a meter up
+        // from center.
+        public static final Transform3d kRightRobotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5),
+            new Rotation3d(0, 0, 0));
+        public static final Transform3d kLeftRobotToCam = new Transform3d(new Translation3d(0.5, 0.0, 0.5),
+            new Rotation3d(0, 0, 0));
+
+        // The layout of the AprilTags on the field
+        public static final AprilTagFieldLayout kTagLayout = AprilTagFields.kDefaultField.loadAprilTagLayoutField(); 
+
+        // The standard deviations of our vision estimated poses, which affect
+        // correction rate
+        // (Fake values. Experiment and determine estimation noise on an actual robot.)
+        public static final Matrix<N3, N1> kRightSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
+        public static final Matrix<N3, N1> kRightMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
+
+        public static final Matrix<N3, N1> kLeftSingleTagStdDevs = VecBuilder.fill(4, 4, 8);
+        public static final Matrix<N3, N1> kLeftMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1);
+
+        public static final Pose2d target = new Pose2d(1, 1, new Rotation2d(Units.degreesToRadians(0)));
+    }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -8,7 +8,7 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.Utils.PathPlanner.PathPlannerHelper;
-import frc.robot.commands.*;
+import frc.robot.commands.SwerveCommands.*;
 import frc.robot.subsystems.*;
 
 /**

src/main/java/frc/robot/commands/SwerveCommands/AlignToSpeaker.java

@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.SwerveCommands;
+
+import static frc.robot.Constants.Swerve.*;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.Utils.vision.Vision;
+import frc.robot.subsystems.SwerveSubsystem;
+
+public class AlignToSpeaker extends Command {
+  /** Creates a new TurnToDegree. */
+  SwerveSubsystem swerve = SwerveSubsystem.getInstance();
+  private PIDController pid = aligningPID;
+  private Vision vision = Vision.getInstance();
+  double angleFromTarget = vision.GetAngleFromTarget().getDegrees();
+
+  public AlignToSpeaker() {
+
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    angleFromTarget = vision.GetAngleFromTarget().getDegrees();
+    calculateOptimalRotation();
+    double rotation = pid.calculate(angleFromTarget, 180);
+    swerve.drive(new Translation2d(), rotation, true, false, false);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return Math.abs(vision.GetAngleFromTarget().getDegrees() - 180) < minimumErrorAligning ;
+  }
+
+  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;
+  }
+}

src/main/java/frc/robot/commands/TeleopSwerve.java → src/main/java/frc/robot/commands/SwerveCommands/TeleopSwerve.java

@@ -1,4 +1,4 @@
-package frc.robot.commands;
+package frc.robot.commands.SwerveCommands;
 
 import frc.robot.Constants;
 import frc.robot.subsystems.SwerveSubsystem;