mostly done

redshiftrobotics · Feb 29, 2024 · b2d991f · b2d991f
1 parent 7e1700f
commit b2d991f
Show file tree

Hide file tree

Showing 2 changed files with 23 additions and 0 deletions.
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -251,6 +251,11 @@ public static class SwerveDrivetrainConstants {
 		public static final double MODULE_LOCATION_X;
 	}
 
+	public static class AutoConstants {
+		// preffered distance to tag, specifically for autopositioning the robot to in front of the tag
+		public static final double PREFERRED_TAG_DISTANCE=2;
+	}
+
 	public static class VisionConstants {
 
 		public static final Transform3d CAMERA_POSE = new Transform3d(0.5, 0, 0.25, new Rotation3d());

diff --git a/src/main/java/frc/robot/commands/AutoPosition.java b/src/main/java/frc/robot/commands/AutoPosition.java
@@ -2,11 +2,21 @@
 
 import java.util.Map;
 
+import org.photonvision.targeting.PhotonTrackedTarget;
+
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.Constants.RobotMovementConstants;
+import frc.robot.Constants.SwerveDrivetrainConstants;
+import frc.robot.Constants.AutoConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
 import frc.robot.subsystems.Vision;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 
 // How to make Command: https://compendium.readthedocs.io/en/latest/tasks/commands/commands.html (ignore image instructions, code is out of date, just look at written general instructions)
 // Command based programming: https://docs.wpilib.org/en/stable/docs/software/commandbased/what-is-command-based.html
@@ -52,6 +62,14 @@ public AutoPosition(SwerveDrivetrain drivetrain, Vision vision) {
 	@Override
 	public void initialize() {
 		drivetrain.toDefaultStates();
+		Transform3d dist3d = vision.getDistToTag();
+		double angle = dist3d.getRotation().getZ();
+		Translation2d trans = new Translation2d(dist3d.getX()-AutoConstants.PREFERRED_TAG_DISTANCE*Math.cos(angle),dist3d.getY()-AutoConstants.PREFERRED_TAG_DISTANCE*Math.sin(angle));
+		Rotation2d rot = new Rotation2d(angle);
+		Commands.sequence(
+			new AutoRotateTo(drivetrain, rot),
+			new AutoDriveTo(drivetrain, trans));
+
 	}
 
 	/**