[#29] crashed wall

honest mistake tbh jons fault for not getting up at light speed to press enter 🤷 :shush: :deaf_man_tone_5:

src/main/java/frc/robot/Constants.java

@@ -139,7 +139,9 @@ public static class AutonomousConstants {
         // TODO get real poses to pathfind to
         public static final Pose2d TARGET_POSE = new Pose2d(new Translation2d(-1, 2), new Rotation2d(0d));
 
-        public static final PathConstraints PATH_CONSTRAINTS = new PathConstraints(2.0, 1, 1.0, 0.5); //TODO get constants
+
+        // For AlignToTag      1.85, 6.5
+        public static final Pose2d AMP_POSE = new Pose2d(new Translation2d(3, 5), new Rotation2d(0d)); 
     }
 
     public static class TunerConstants {

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

@@ -6,6 +6,9 @@
 
 
 import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
+
+import javax.swing.plaf.TreeUI;
+
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.FieldCentric;
 import com.pathplanner.lib.auto.AutoBuilder;
@@ -14,6 +17,7 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
 import frc.robot.subsystems.Limelights;
 import frc.robot.subsystems.Swerve;
 import frc.thunder.vision.Limelight;
@@ -60,27 +64,34 @@ public AlignToTag(Pose2d target, Swerve drivetrain, SwerveRequest.FieldCentric d
   public void initialize() {
     aligning = false;
     path = PathPlannerPath.fromPathFile("Amp autoalign");
+    System.out.println("STARTED!!!!!!!11");
     // pointAtTag = new PointAtTag(drivetrain, limelights, driver);
 
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (!aligning) {
+      System.out.println("EXECUTINGINIGNIGN??????????????");
+      System.out.println(aligning);
+      //new Trigger(() -> (!(new MoveToPose(target, drivetrain, drive).isFinished())))
+      //    .whileTrue(new MoveToPose(target, drivetrain, drive).schedule());
+      //new Trigger(() -> (new MoveToPose(target, drivetrain, drive).isFinished()))
+      //    .whileTrue(AutoBuilder.followPath(path));
       if (!(new MoveToPose(target, drivetrain, drive).isFinished())) {
-        new MoveToPose(target, drivetrain, drive);
-      } else {
+        System.out.println("AHASIDHASHASID");
+        new Trigger(() -> (true))).whileTrue(new MoveToPose(target, drivetrain, drive));
+      } else if (!aligning && (new MoveToPose(target, drivetrain, drive).isFinished()) ) {
+        AutoBuilder.followPath(path);
         aligning = true;
       }
-    } else {
-      AutoBuilder.followPath(path);
-    }
-  }
 
+  }
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {}
+  public void end(boolean interrupted) {
+    drivetrain.setControl(drive.withVelocityX(0).withVelocityY(0));
+  }
 
   // Returns true when the command should end.
   @Override

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

@@ -25,8 +25,8 @@ public class MoveToPose extends Command {
     private Pose2d current;
     private double dx;
     private double dy;
-    private final double kp = 1.8; 
-    private final double minSpeed = 0.9;
+    private final double kp = 0.3; //1.8 old
+    private final double minSpeed = 0.2; //0.9 old
     private double powerx;
     private double powery;
     /** 
@@ -51,7 +51,7 @@ public void initialize() {
     // Called every time the scheduler runs while the command is scheduled.
     @Override
     public void execute() {
-
+        System.out.println("I'm working heheheha!!!!!################################333333!");
         LightningShuffleboard.setDouble("MoveToPose", "dx", dx);
         LightningShuffleboard.setDouble("MoveToPose", "dy", dy);
         LightningShuffleboard.setDouble("MoveToPose", "targetX", target.getX());