automatic reef alignment controls

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

@@ -9,6 +9,7 @@ import frc.robot.constants.ClimberPivotConstants;
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.ManipulatorConstants;
 import frc.robot.constants.OIConstants;
+import frc.robot.constants.VisionConstants;
 import frc.robot.subsystems.ManipulatorPivot;
 import frc.robot.subsystems.Vision;
 import frc.robot.subsystems.ClimberPivot;
@@ -16,11 +17,12 @@ import frc.robot.subsystems.ClimberRollers;
 import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Manipulator;
+
+import java.util.function.IntSupplier;
+
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
@@ -47,8 +49,11 @@ public class RobotContainer {
 	private CommandXboxController operator;
 
 	private SendableChooser<Command> autoChooser;
+
 	private Vision vision;
 
+	private IntSupplier closestTag;
+
 	public RobotContainer() {
 		climberPivot = new ClimberPivot();
 
@@ -70,6 +75,8 @@ public class RobotContainer {
 
 		autoChooser = AutoBuilder.buildAutoChooser();
 
+		closestTag = drivetrain::getClosestTag;
+
 		configureButtonBindings();
 		//elevatorSysIDBindings();
 		//elevatorOnlyBindings();
@@ -157,6 +164,24 @@ public class RobotContainer {
 		driver.povRight().whileTrue(climberRollers.runRoller(-0.5));
 		driver.a().whileTrue(manipulator.runManipulator(() -> 0.5, false));
 		
+		/* 
+		driver.rightBumper().whileTrue(
+				drivetrain.goToPose(
+					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][2], 
+					() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][3], 
+					() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
+				)
+			);
+
+		driver.leftBumper().whileTrue(
+			drivetrain.goToPose(
+				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][0], 
+				() -> VisionConstants.reefSetpointsMap[closestTag.getAsInt()][1], 
+				() -> 360-VisionConstants.globalTagCoords[closestTag.getAsInt()][3]
+			)
+		);
+		*/
+
 		//Operator inputs
 		operator.povUp().onTrue(
 			safeMoveManipulator(

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -57,6 +57,12 @@ public class DrivetrainConstants {
 
     public static final boolean kGyroReversed = true;
 
+    public static final double kHeadingP = 0.0;
+
+    public static final double kXTranslationP = 0.0;
+    public static final double kYTranslationP = 0.0;
+
+
     // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
 
     // Distance between front and back wheels on robot

src/main/java/frc/robot/constants/VisionConstants.java

@@ -1,7 +1,5 @@
 package frc.robot.constants;
 
-import java.util.HashMap;
-
 public class VisionConstants {
     
     // global coordinate map of all tags. index is the tag id.
@@ -35,22 +33,30 @@ public class VisionConstants {
     //map of coral placing setpoints based on the tag that is on the same reef face
     // and the on the left or right branch of that side of the reef
     // <tag_number, {left_x, left_y, right_x, right_y}>
-    public HashMap<Integer, double[]> blueReefMap = new HashMap<>();
+    public static final double[][] reefSetpointsMap = {
-    public HashMap<Integer, double[]> redReefMap = new HashMap<>();
+        {},
+        {},
+        {},
+        {},
+        {},
+        {},
+        {4.993+12.272, 2.816, 5.272+12.272, 2.996},//6
+        {5.789+12.272, 3.862, 5.789+12.272, 4.194},
+        {5.275+12.272, 5.075, 4.991+12.272, 5.246},
+        {3.986+12.272, 5.24, 3.701+12.272, 5.076},
+        {3.183+12.272, 4.191, 3.183, 3.857},
+        {3.703+12.272, 3.975, 3.982+12.272, 2.806},//11
+        {},
+        {},
+        {},
+        {},
+        {},
+        {3.703, 3.975, 3.982, 2.806},
+        {3.183, 4.191, 3.183, 3.857},
+        {3.986, 5.24, 3.701, 5.076},
+        {5.275, 5.075, 4.991, 5.246},
+        {5.789, 3.862, 5.789, 4.194},
+        {4.993, 2.816, 5.272, 2.996}
+    };
 
-    VisionConstants(){
-        blueReefMap.put(17, new double[] {3.703, 3.975, 3.982, 2.806});
-        blueReefMap.put(18, new double[] {3.183, 4.191, 3.183, 3.857});
-        blueReefMap.put(19, new double[] {3.986, 5.24, 3.701, 5.076});
-        blueReefMap.put(20, new double[] {5.275, 5.075, 4.991, 5.246});
-        blueReefMap.put(21, new double[] {5.789, 3.862, 5.789, 4.194});
-        blueReefMap.put(22, new double[] {4.993, 2.816, 5.272, 2.996});
-        
-        blueReefMap.put(11, new double[] {3.703+12.272, 3.975, 3.982+12.272, 2.806});
-        blueReefMap.put(10, new double[] {3.183+12.272, 4.191, 3.183, 3.857});
-        blueReefMap.put(9, new double[] {3.986+12.272, 5.24, 3.701+12.272, 5.076});
-        blueReefMap.put(8, new double[] {5.275+12.272, 5.075, 4.991+12.272, 5.246});
-        blueReefMap.put(7, new double[] {5.789+12.272, 3.862, 5.789+12.272, 4.194});
-        blueReefMap.put(6, new double[] {4.993+12.272, 2.816, 5.272+12.272, 2.996});
-    }
 }

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -15,6 +15,7 @@ import com.studica.frc.AHRS;
 import com.studica.frc.AHRS.NavXComType;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -31,6 +32,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.constants.OIConstants;
+import frc.robot.constants.VisionConstants;
 
 public class Drivetrain extends SubsystemBase {
     // Create MAXSwerveModules
@@ -50,6 +52,10 @@ public class Drivetrain extends SubsystemBase {
     public Orchestra m_orchestra = new Orchestra();
     private Timer musicTimer = new Timer();
 
+    private PIDController pidHeading;
+    private PIDController pidTranslationX;
+    private PIDController pidTranslationY;
+
     /** Creates a new DriveSubsystem. */
     public Drivetrain() {
         m_frontLeft = new MAXSwerveModule(
@@ -90,6 +96,12 @@ public class Drivetrain extends SubsystemBase {
             new Pose2d()
         );
 
+        pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
+        pidHeading.enableContinuousInput(-180, 180);
+
+        pidTranslationX = new PIDController(DrivetrainConstants.kXTranslationP,0,0);
+        pidTranslationY = new PIDController(DrivetrainConstants.kYTranslationP,0,0);
+
         AutoBuilder.configure(
             this::getPose, 
             this::resetOdometry, 
@@ -152,7 +164,7 @@ public class Drivetrain extends SubsystemBase {
         }
         */
         
-        if(musicTimer.get()>8){
+        if(musicTimer.get()>4){
             if (m_orchestra.isPlaying()) {
                 m_orchestra.stop();
             }
@@ -245,6 +257,50 @@ public class Drivetrain extends SubsystemBase {
         });
     }
 
+    public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, DoubleSupplier headingSetpoint){
+        return run(() -> {
+            drive(pidTranslationX.calculate(m_estimator.getEstimatedPosition().getX(), xSetpoint.getAsDouble()),
+                pidTranslationY.calculate(m_estimator.getEstimatedPosition().getY(), ySetpoint.getAsDouble()), 
+                pidHeading.calculate(getHeading(), headingSetpoint.getAsDouble()), 
+                true);
+            });
+    }
+
+    public int getClosestTag(){
+
+		if(DriverStation.getAlliance().equals(DriverStation.Alliance.Blue)){
+			int closestTag = 17;
+			double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2)
+			+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2));
+			
+			for(int i = 17; i <= 22; ++i){
+				double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2)
+				+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2));
+
+				if(distance < closestTagDist){
+					closestTag = i;
+					closestTagDist = distance;
+				}
+			}
+			return closestTag;
+		}else{
+			int closestTag = 6;
+			double closestTagDist = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[17][0], 2)
+			+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[17][1], 2));
+			
+			for(int i = 6; i <= 11; ++i){
+				double distance = Math.sqrt(Math.pow(getPose().getX()-VisionConstants.globalTagCoords[i][0], 2)
+				+ Math.pow(getPose().getY()-VisionConstants.globalTagCoords[i][1], 2));
+
+				if(distance < closestTagDist){
+					closestTag = i;
+					closestTagDist = distance;
+				}
+			}
+			return closestTag;
+		}
+	}
+
     /**
      * Sets the wheels into an X formation to prevent movement.
      */