From 0f59608cca00480e56535cebfb11f33793a48774 Mon Sep 17 00:00:00 2001
From: mebrahimaleem <mebrahimaleem@gmail.com>
Date: Sat, 6 Jan 2024 11:35:15 -0800
Subject: [PATCH] feat: Alignment logic

---
 src/main/java/frc/robot/Constants.java        | 35 ++++++++
 .../frc/robot/commands/AlignToTagCommand.java | 83 +++++++++++++++++++
 2 files changed, 118 insertions(+)
 create mode 100644 src/main/java/frc/robot/Constants.java
 create mode 100644 src/main/java/frc/robot/commands/AlignToTagCommand.java

diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
new file mode 100644
index 0000000..f0b9720
--- /dev/null
+++ b/src/main/java/frc/robot/Constants.java
@@ -0,0 +1,35 @@
+package frc.robot;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+public final class Constants {
+	public final static class TagAlignConstants {
+		public final static double kTagPIDkPxy = 0;
+		public final static double kTagPIDkIxy = 0;
+		public final static double kTagPIDkDxy = 0;
+
+		public final static double kTagPIDkPomega = 0;
+		public final static double kTagPIDkIomega = 0;
+		public final static double kTagPIDkDomega = 0;
+
+		public static final Pose2d[] kTargetPoses = new Pose2d[] {}; // Poses are in the same order as the enumerator.
+		// TODO: populate poses
+
+		//TODO: tune all these parameters
+		public static final double kTolxy = 0.1; // tolerance in meters for x and y
+		public static final double kTolomega = 0.1; // tolerance in radians for omega
+		public static final double kConsectol = 10; // onsecutive cycles of being within tolerance needed to end command
+		public static final double kMinProx = 1; // minimum proximity radius in meters.
+		// To avoid accidents, the robot must be
+		// within the minimum proximity for the tag alignment command to run.
+		// If the robot exceeds the minimum proximity the command will finish
+
+		public static enum AlignPosition { // DON'T CHANGE THE ORDER OF THESE. Add new items to the end
+			Amp,
+			SourceLeft,
+			SourceCenter,
+			SourceRight,
+			Speaker
+		}
+	}
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/commands/AlignToTagCommand.java b/src/main/java/frc/robot/commands/AlignToTagCommand.java
new file mode 100644
index 0000000..30991ac
--- /dev/null
+++ b/src/main/java/frc/robot/commands/AlignToTagCommand.java
@@ -0,0 +1,83 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import frc.robot.Constants.TagAlignConstants;
+import frc.robot.Constants.TagAlignConstants.AlignPosition;
+
+public class AlignToTagCommand extends CommandBase {
+
+	final Subsystem m_subsystem; // TODO: change type to drive subsystem
+	final Pose2d m_target;
+	final PIDController m_PIDxy;
+	final PIDController m_PIDomega;
+
+	double m_consecCount; // counter of number of consecutive cycles within tolerance
+
+	/**
+	 * 
+	 * @param subsystem Subsystem to require. Subsystem must implement a drive
+	 *                  method and a getPosition method.
+	 * @param alignPos  Position to align to
+	 */
+	public AlignToTagCommand(Subsystem subsystem, AlignPosition alignPos) {
+		m_subsystem = subsystem;
+		addRequirements(m_subsystem);
+		m_target = TagAlignConstants.kTargetPoses[alignPos.ordinal()];
+		m_PIDxy = new PIDController(TagAlignConstants.kTagPIDkPxy, TagAlignConstants.kTagPIDkIxy,
+				TagAlignConstants.kTagPIDkDxy);
+		m_PIDomega = new PIDController(TagAlignConstants.kTagPIDkPomega, TagAlignConstants.kTagPIDkIomega,
+				TagAlignConstants.kTagPIDkDomega);
+		m_PIDomega.enableContinuousInput(-Math.PI, Math.PI);
+	}
+
+	@Override
+	public void initialize() {
+		m_PIDxy.reset();
+		m_PIDomega.reset();
+		m_consecCount = 0;
+	}
+
+	@Override
+	public void execute() {
+		Pose2d current = new Pose2d(); // TODO: get pose from april tag
+
+		double xSpeed = MathUtil.clamp(m_PIDxy.calculate(current.getX(), m_target.getX()),
+				-0, 0); // TODO: change clamp parameters and units
+
+		double ySpeed = MathUtil.clamp(
+				m_PIDxy.calculate(current.getY(), m_target.getY()),
+				-0, 0);
+
+		double omegaSpeed = MathUtil.clamp(
+				m_PIDomega.calculate(current.getRotation().getRadians(), m_target.getRotation().getRadians()),
+				-0, 0);
+
+		// Check if within tolerance
+		if (Math.abs(current.getX() - m_target.getX()) < TagAlignConstants.kTolxy && // x within range
+				Math.abs(current.getY() - m_target.getY()) < TagAlignConstants.kTolxy && // y within range
+				Math.abs( // omega within range
+						current.getRotation().getRadians() - m_target.getRotation().getRadians()) < TagAlignConstants.kTolomega) {
+			m_consecCount++;
+		} else
+			m_consecCount = 0;
+
+		// TODO: call drive function
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		// TODO: log finish reason to NT (either reached, proximity, or interrupted)
+	}
+
+	@Override
+	public boolean isFinished() {
+		Pose2d current = new Pose2d(); // TODO: get pose from april tag
+		return Math.pow(current.getX() - m_target.getX(), 2) + Math.pow(current.getY() - m_target.getY(), 2) > Math
+				.pow(TagAlignConstants.kMinProx, 2) || // Exceed proximity
+				m_consecCount >= TagAlignConstants.kConsectol; // Reached setpoint
+	}
+}
\ No newline at end of file