align to speaker command

Author: eilonhuppert

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

@@ -27,6 +27,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
@@ -202,27 +203,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/commands/SwerveCommands/AlignToSpeaker.java

@@ -0,0 +1,59 @@
+// 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.IntakeArm.minimumError;
+import static frc.robot.Constants.Swerve.minimumErrorTurn;
+
+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 = new PIDController(0, 0, 0);
+  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();
+    optimize();
+    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) < minimumErrorTurn ;
+  }
+
+  public void optimize() {
+    double delta = 180 - angleFromTarget;
+    if (Math.abs(delta) > 90){
+        angleFromTarget = delta > 90 ? (angleFromTarget -= 180) : (angleFromTarget += 180);
+    }
+  }
+}