From 73a04a77e9a13bd0d5bc33250a0c947747d4f99d Mon Sep 17 00:00:00 2001
From: HeroSoLos <olosoreh2@gmail.com>
Date: Sat, 17 Feb 2024 15:07:32 -0500
Subject: [PATCH] [#29] no auton working dum

---
 src/main/java/frc/robot/command/AlignToTag.java | 12 +++++++++---
 src/main/java/frc/robot/command/MoveToPose.java | 10 ++++++----
 2 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/src/main/java/frc/robot/command/AlignToTag.java b/src/main/java/frc/robot/command/AlignToTag.java
index 66774aad..33d57cf7 100644
--- a/src/main/java/frc/robot/command/AlignToTag.java
+++ b/src/main/java/frc/robot/command/AlignToTag.java
@@ -41,7 +41,7 @@ public class AlignToTag extends Command {
   private Limelights limelights;
 	private MoveToPose move;
 	private int limelightPrevPipeline = 0;
-  
+  private boolean MoveToPoseDone;
   private boolean aligning;
   /** Creates a new AlignToTag. */
   public AlignToTag(Pose2d target, Swerve drivetrain, SwerveRequest.FieldCentric drive, XboxController driver) {
@@ -78,14 +78,20 @@ public void execute() {
       //    .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())) {
+      MoveToPoseDone = Math.sqrt((target.getTranslation().getX() - drivetrain.getPose().get().getTranslation().getX())*(target.getTranslation().getX() - drivetrain.getPose().get().getTranslation().getX()) + (target.getTranslation().getY() - drivetrain.getPose().get().getTranslation().getY())*(target.getTranslation().getY() - drivetrain.getPose().get().getTranslation().getY()))<0.4;
+      System.out.println(Math.sqrt((target.getTranslation().getX() - drivetrain.getPose().get().getTranslation().getX())*(target.getTranslation().getX() - drivetrain.getPose().get().getTranslation().getX()) + (target.getTranslation().getY() - drivetrain.getPose().get().getTranslation().getY())*(target.getTranslation().getY() - drivetrain.getPose().get().getTranslation().getY()))<0.4);
+      System.out.println(new MoveToPose(target, drivetrain, drive).isFinished());
+      if (!aligning && !MoveToPoseDone/*(new MoveToPose(target, drivetrain, drive).isFinished())*/) {
         System.out.println("AHASIDHASHASID");
         move = new MoveToPose(target, drivetrain, drive);
         move.initialize();
         move.execute();
-      } else if (!aligning && (new MoveToPose(target, drivetrain, drive).isFinished()) ) {
+      } else if (!aligning) {
+        System.out.println("IM DONEWITHMOVETOPOSE AUTOBUILDING!!!!!!!!!!!!!!!!!!!!!!!!!!1*(!*)");
         AutoBuilder.followPath(path);
         aligning = true;
+      }else{
+        System.out.println("running path");
       }
 
   }
diff --git a/src/main/java/frc/robot/command/MoveToPose.java b/src/main/java/frc/robot/command/MoveToPose.java
index 5091cd86..f95e6157 100644
--- a/src/main/java/frc/robot/command/MoveToPose.java
+++ b/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 = 0.3; //1.8 old
-    private final double minSpeed = 0.2; //0.9 old
+    private final double kp = 0.8; //1.8 old
+    private final double minSpeed = 0.4; //0.9 old
     private double powerx;
     private double powery;
     /** 
@@ -58,9 +58,11 @@ public void execute() {
         LightningShuffleboard.setDouble("MoveToPose", "targetY", target.getY());
 
         current = drivetrain.getPose().get(); 
+        System.out.println(current);
         dx = target.getTranslation().getX() - current.getTranslation().getX();
         dy = target.getTranslation().getY() - current.getTranslation().getY();
-    
+        System.out.println(dx);
+        System.out.println(dy);
         powerx = dx * kp;
         powery = dy * kp;
         
@@ -81,7 +83,7 @@ public void execute() {
         }
 
         var dist = Math.sqrt(dx*dx + dy*dy);
-        if (dist < 0.3) {
+        if (dist < 0.4) {
             powerx = 0;
             powery = 0;
             finished = true;