lineup command basic - not done

Author: SatchitK

src/main/java/frc/robot/vision/VisionConfig.java

@@ -99,10 +99,23 @@ public class VisionConfig {
 
 
     public class AlignmentConfig {
+        public static final double THETA_kP = 3.0;
+        public static final double THETA_kI = 0.0;
+        public static final double THETA_kD = 0.0;
+    
+        public static final double X_kP = 5.0;
+        public static final double X_kI = 0.0;
+        public static final double X_kD = 0.0;
+    
+        public static final double Y_kP = 5.0;
+        public static final double Y_kI = 0.0;
+        public static final double Y_kD = 0.0;
+    
+
         public static final Distance ALIGNMENT_TOLERANCE = Inches.of(0.5); //inches
         public static final LinearVelocity MAX_ALIGN_TRANSLATION_VELOCITY = TunerConstants.kSpeedAt12Volts.div(2);
         public static final LinearAcceleration MAX_ALIGN_TRANSLATION_ACCELERATION = MetersPerSecondPerSecond.of(6.0);
-        public static final AngularVelocity MAX_ALIGH_ANGULAR_VELOCITY = RotationsPerSecond.of(1.25).times(0.75);
+        public static final AngularVelocity MAX_ALIGN_ANGULAR_VELOCITY = RotationsPerSecond.of(1.25).times(0.75);
         public static final AngularAcceleration MAX_ALIGN_ANGULAR_ACCELERATION = RadiansPerSecondPerSecond.of(6.0 * Math.PI);
 
         //TODO: MIGHT NEED TO PLAY AROUND WITH ALL THE POSES BELOW

src/main/java/frc/robot/vision/commands/VisionCommands.java

@@ -0,0 +1,64 @@
+package frc.robot.vision.commands;
+
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond;
+
+import com.ctre.phoenix6.swerve.SwerveRequest;
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.estimator.PoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.vision.PoseEstimatorSubsystem;
+import frc.robot.vision.VisionConfig;
+
+public class VisionCommands {
+    
+    public Command executeLineup() {
+        Pose2d estimatedPose = PoseEstimatorSubsystem.getInstance().getCurrentPose();
+
+        //add ProfiledPID controllers for Translation and Rotation
+        final TrapezoidProfile.Constraints TRANSLATION_CONSTRAINTS = new TrapezoidProfile.Constraints(
+                    VisionConfig.AlignmentConfig.MAX_ALIGN_TRANSLATION_VELOCITY.in(MetersPerSecond),
+                    VisionConfig.AlignmentConfig.MAX_ALIGN_TRANSLATION_ACCELERATION.in(MetersPerSecondPerSecond));
+            
+        final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(
+                    VisionConfig.AlignmentConfig.MAX_ALIGN_ANGULAR_VELOCITY.in(RadiansPerSecond),
+                    VisionConfig.AlignmentConfig.MAX_ALIGN_ANGULAR_ACCELERATION.in(RadiansPerSecondPerSecond));
+
+
+        final ProfiledPIDController xDistanceController = new ProfiledPIDController(
+            VisionConfig.AlignmentConfig.X_kP,
+            VisionConfig.AlignmentConfig.X_kI,
+            VisionConfig.AlignmentConfig.X_kD,
+            TRANSLATION_CONSTRAINTS);
+
+        final ProfiledPIDController yDistanceController = new ProfiledPIDController(
+            VisionConfig.AlignmentConfig.Y_kP,
+            VisionConfig.AlignmentConfig.Y_kI,
+            VisionConfig.AlignmentConfig.Y_kD,
+            TRANSLATION_CONSTRAINTS);
+
+        final ProfiledPIDController thetaController = new ProfiledPIDController(
+            VisionConfig.AlignmentConfig.THETA_kP,
+            VisionConfig.AlignmentConfig.THETA_kI,
+            VisionConfig.AlignmentConfig.THETA_kD,
+            THETA_CONSTRAINTS);
+        
+        final SwerveRequest.RobotCentric robotCentricRequest = new SwerveRequest.RobotCentric()
+            .withDriveRequestType(DriveRequestType.Velocity)
+            .withSteerRequestType(SteerRequestType.MotionMagicExpo);
+            
+
+        return new WaitCommand(0);
+    }
+
+
+
+}