Add code from yesterday

src/main/java/frc4388/robot/Constants.java

@@ -241,8 +241,8 @@ public final class Constants {
             public static final int LIDAR_MICROS_TO_CM = 10;
             public static final int SECONDS_TO_MICROS = 1000000;
                     
-            public static final double XY_TOLERANCE = 0.1;
-            public static final double ROT_TOLERANCE = 1;
+            public static final double XY_TOLERANCE = 0.05; // Meters
+            public static final double ROT_TOLERANCE = 1; // Degrees
                     
             // public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());
             // public static final Pose2d targetpos = 

src/main/java/frc4388/robot/RobotContainer.java

@@ -38,6 +38,7 @@ import edu.wpi.first.wpilibj2.command.Commands;
 import frc4388.utility.controller.VirtualController;
 import frc4388.robot.commands.GotoLastApril;
 import frc4388.robot.commands.LidarAlign;
+import frc4388.robot.commands.MoveForTimeCommand;
 import frc4388.robot.commands.Swerve.neoJoystickPlayback;
 import frc4388.robot.commands.Swerve.neoJoystickRecorder;
 
@@ -110,6 +111,7 @@ public class RobotContainer {
     private Command AprilLidarAlign = new SequentialCommandGroup(
         new GotoLastApril(m_robotSwerveDrive, m_vision),
         new InstantCommand(() -> System.out.println("Soup")),
+        new WaitCommand(1),
         new LidarAlign(m_robotSwerveDrive, m_lidar, true)
     );
     private Command AprilLidarLeft = new SequentialCommandGroup(
@@ -120,7 +122,10 @@ public class RobotContainer {
     private Command AprilLidarRight = new SequentialCommandGroup(
         AutoGotoPosition.asProxy(),
         new InstantCommand(() -> System.out.println("Soup")),
-        new LidarAlign(m_robotSwerveDrive, m_lidar, true)
+        new WaitCommand(1),
+        new LidarAlign(m_robotSwerveDrive, m_lidar, true),
+        new MoveForTimeCommand(m_robotSwerveDrive, 
+        new Translation2d(0, 0.5), new Translation2d(), 1000, true)
     );
 
     private Command placeCoral = new SequentialCommandGroup(

src/main/java/frc4388/robot/commands/GotoLastApril.java

@@ -75,6 +75,14 @@ public class GotoLastApril extends Command {
            0
         );
 
+        Rotation2d error = new Translation2d(xerr, yerr).getAngle();
+
+        error = error.rotateBy(Rotation2d.fromDegrees(-rotoutput));
+
+        double tagRelativeXError = error.getSin();
+
+        System.out.println(tagRelativeXError);
+
         // SmartDashboard.putNumber("PID X Output", xoutput);
         // SmartDashboard.putNumber("PID Y Output", youtput);
         // // SmartDashboard.putNumber("PID Y Output", youtput);
@@ -86,6 +94,10 @@ public class GotoLastApril extends Command {
     public final boolean isFinished() {
         boolean finished = (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE);
         // System.out.println(finished);
+
+        if(finished)
+            swerveDrive.softStop();
+
         return finished;
                 // this statement is a boolean in and of itself
     }

src/main/java/frc4388/robot/commands/MoveForTimeCommand.java

@@ -0,0 +1,47 @@
+package frc4388.robot.commands;
+
+import java.time.Instant;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc4388.robot.subsystems.SwerveDrive;
+
+// Command to repeat a joystick movement for a specific time.
+public class MoveForTimeCommand extends Command {
+    private final SwerveDrive swerveDrive;
+    private final Translation2d leftStick;
+    private final Translation2d rightStick;
+    private final long duration;
+    private final boolean robotRelative;
+
+    private Instant startTime;
+
+    public MoveForTimeCommand(
+        SwerveDrive swerveDrive, 
+        Translation2d leftStick, 
+        Translation2d rightStick, 
+        long millis,
+        boolean robotRelative) {
+
+        this.swerveDrive = swerveDrive;
+        this.leftStick = leftStick;
+        this.rightStick = rightStick;
+        this.duration = millis;
+        this.robotRelative = robotRelative;
+    }
+
+    @Override
+    public void initialize() {
+        startTime = Instant.now();
+    }
+
+    @Override
+    public void execute() {
+        swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration;
+    }
+}