[#29] wrong direction and gtg

frc-862 · Feb 17, 2024 · ff42641 · ff42641
1 parent 3f676d3
commit ff42641
Showing 1 changed file with 4 additions and 2 deletions.
diff --git a/src/main/java/frc/robot/command/AlignToTag.java b/src/main/java/frc/robot/command/AlignToTag.java
@@ -39,7 +39,7 @@ public class AlignToTag extends Command {
 	private XboxController driver;
 	private FieldCentric drive;
   private Limelights limelights;
-
+	private MoveToPose move;
 	private int limelightPrevPipeline = 0;
 
   private boolean aligning;
@@ -80,7 +80,9 @@ public void execute() {
       //    .whileTrue(AutoBuilder.followPath(path));
       if (!(new MoveToPose(target, drivetrain, drive).isFinished())) {
         System.out.println("AHASIDHASHASID");
-        new Trigger(() -> (true))).whileTrue(new MoveToPose(target, drivetrain, drive));
+        move = new MoveToPose(target, drivetrain, drive);
+        move.initialize();
+        move.execute();
       } else if (!aligning && (new MoveToPose(target, drivetrain, drive).isFinished()) ) {
         AutoBuilder.followPath(path);
         aligning = true;