[#29] for josh

src/main/deploy/pathplanner/autos/Amp autoalign.auto

@@ -0,0 +1,19 @@
+{
+  "version": 1.0,
+  "startingPose": null,
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Amp autoalign"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/New Path.path → src/main/deploy/pathplanner/paths/Amp autoalign.path

@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 1.84,
-        "y": 7.014791090923124
+        "x": 1.85,
+        "y": 6.5
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.57592514551521,
-        "y": 7.014791090923124
+        "x": 1.85,
+        "y": 6.550000000000001
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 4.0,
-        "y": 6.0
+        "x": 1.85,
+        "y": 7.5
       },
       "prevControl": {
-        "x": 3.0,
-        "y": 6.0
+        "x": 1.85,
+        "y": 7.45
       },
       "nextControl": null,
       "isLocked": false,
@@ -38,14 +38,14 @@
     "maxAngularAcceleration": 720.0
   },
   "goalEndState": {
-    "velocity": 0,
-    "rotation": 0,
+    "velocity": 0.0,
+    "rotation": 90.0,
     "rotateFast": false
   },
   "reversed": false,
   "folder": null,
   "previewStartingState": {
-    "rotation": 88.45184230102207,
+    "rotation": 90.0,
     "velocity": 0
   },
   "useDefaultConstraints": true

src/main/java/frc/robot/command/AlignToTag.java

@@ -35,6 +35,7 @@ public class AlignToTag extends Command {
 
 	private int limelightPrevPipeline = 0;
 
+  private boolean aligning;
   /** Creates a new AlignToTag. */
   public AlignToTag(Pose2d target, Swerve drivetrain, SwerveRequest.FieldCentric drive, Limelights limelights, XboxController driver) {
     // Use addRequirements() here to declare subsystem dependencies.
@@ -43,27 +44,39 @@ public AlignToTag(Pose2d target, Swerve drivetrain, SwerveRequest.FieldCentric d
     this.drive = drive;
     this.limelights = limelights;
 
-    limelight = limelights.getStopMe();
+    //limelight = limelights.getStopMe();
 
-		limelightPrevPipeline = limelight.getPipeline();
+		//limelightPrevPipeline = limelight.getPipeline();
 
-		limelight.setPipeline(VisionConstants.TAG_PIPELINE);
+		//limelight.setPipeline(VisionConstants.TAG_PIPELINE);
 
-		drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);//.withDeadband(DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SPEED_DB).withRotationalDeadband(DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_DB); // I want field-centric driving in closed loop
+		//drive = new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);//.withDeadband(DrivetrAinConstants.MaxSpeed * DrivetrAinConstants.SPEED_DB).withRotationalDeadband(DrivetrAinConstants.MaxAngularRate * DrivetrAinConstants.ROT_DB); // I want field-centric driving in closed loop
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    new MoveToPose(target, drivetrain, drive);
+    aligning = false;
+
     // pointAtTag = new PointAtTag(drivetrain, limelights, driver);
 
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    // limelight.getTargetPoseRobotSpace();
+    if (!aligning) {
+      if (!(new MoveToPose(target, drivetrain, drive).isFinished())) {
+        new MoveToPose(target, drivetrain, drive);
+      } else {
+        aligning = true;
+      }
+    } else {
+
+    }
+
+
+
 
   }