Limelight lidar stuff

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

@@ -231,7 +231,7 @@ public final class Constants {
         }
     
         public static final class AutoConstants {
-            public static final Gains XY_GAINS = new Gains(3,0.01,0);
+            public static final Gains XY_GAINS = new Gains(3,0.01,0.0);
             public static final Gains ROT_GAINS = new Gains(0.05,0,0.0);
 
             public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.05, 0);
@@ -241,7 +241,7 @@ 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.05; // Meters
+            public static final double XY_TOLERANCE = 0.07; // Meters
             public static final double ROT_TOLERANCE = 1; // Degrees
                     
             // public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d());

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

@@ -112,20 +112,20 @@ public class RobotContainer {
         new GotoLastApril(m_robotSwerveDrive, m_vision),
         new InstantCommand(() -> System.out.println("Soup")),
         new WaitCommand(1),
-        new LidarAlign(m_robotSwerveDrive, m_lidar, true)
+        new LidarAlign(m_robotSwerveDrive, m_lidar)
     );
     private Command AprilLidarLeft = new SequentialCommandGroup(
         AutoGotoPosition.asProxy(),
-        new LidarAlign(m_robotSwerveDrive, m_lidar, false)
+        new LidarAlign(m_robotSwerveDrive, m_lidar)
     );
 
     private Command AprilLidarRight = new SequentialCommandGroup(
         AutoGotoPosition.asProxy(),
         new InstantCommand(() -> System.out.println("Soup")),
         new WaitCommand(1),
-        new LidarAlign(m_robotSwerveDrive, m_lidar, true),
-        new MoveForTimeCommand(m_robotSwerveDrive, 
-        new Translation2d(0, 0.5), new Translation2d(), 1000, true)
+        new LidarAlign(m_robotSwerveDrive, m_lidar)//,
+        // new MoveForTimeCommand(m_robotSwerveDrive, 
+        // new Translation2d(0, 0.5), new Translation2d(), 1000, true)
     );
 
     private Command placeCoral = new SequentialCommandGroup(
@@ -251,7 +251,7 @@ public class RobotContainer {
             .onTrue(AprilLidarRight);
         
         new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
-            .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); 
+            .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar)); 
 
         
         DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON)

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

@@ -40,6 +40,12 @@ public class GotoLastApril extends Command {
         // addRequirements(swerveDrive);
     }
 
+
+    public static double tagRelativeXError = -1;
+    private static void setTagRelativeXError(double val){
+        tagRelativeXError = val;
+    }
+
     @Override
     public void initialize() {
         xPID.initialize();
@@ -75,17 +81,10 @@ 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);
+        setTagRelativeXError(
+            new Translation2d(xerr, yerr).getAngle()
+            .rotateBy(targetpos.getRotation())
+            .getCos());
 
         swerveDrive.driveWithInput(leftStick, rightStick, true);
     }

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

@@ -21,12 +21,11 @@ public class LidarAlign extends Command {
   private boolean foundReef;
   private boolean headedRight;
   private double speed;
-  private final boolean constructedHeadedRight;
+  // private final boolean constructedHeadedRight;
 
   /** Creates a new LidarAlign. */
-  public LidarAlign(SwerveDrive swerveDrive, Lidar lidar, boolean headedRight) {
+  public LidarAlign(SwerveDrive swerveDrive, Lidar lidar) {//, boolean headedRight) {
     // Use addRequirements() here to declare subsystem dependencies.
-    constructedHeadedRight = headedRight;
 
     this.swerveDrive = swerveDrive;
     this.lidar = lidar;
@@ -40,7 +39,7 @@ public class LidarAlign extends Command {
     this.currentFinderTick = 0;
     this.speed = 0.4; // TODO: find good speed for this
     this.foundReef = false;
-    this.headedRight = constructedHeadedRight;
+    this.headedRight = !(GotoLastApril.tagRelativeXError < 0);
   }
 
 

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

@@ -22,6 +22,7 @@ public class MoveForTimeCommand extends Command {
         Translation2d rightStick, 
         long millis,
         boolean robotRelative) {
+            addRequirements(swerveDrive);
 
         this.swerveDrive = swerveDrive;
         this.leftStick = leftStick;

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -31,6 +31,8 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 // import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.commands.GotoLastApril;
+import frc4388.robot.commands.LidarAlign;
 import frc4388.utility.Status;
 import frc4388.utility.Subsystem;
 import frc4388.utility.Status.ReportLevel;
@@ -313,6 +315,7 @@ public class SwerveDrive extends Subsystem {
         SmartDashboard.putNumber("RotTartget", rotTarget);
 
         double time = Vision.getTime();
+        
 
         vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time));
 

src/main/java/frc4388/utility/ReefPositionHelper.java

@@ -50,14 +50,11 @@ public class ReefPositionHelper {
         for(int i = 1; i < locations.length; i++){
             double dist = distanceTo(locations[i],position);
             if(dist < minDistance){
-                System.out.println(i);
                 minPos = locations[i];
                 minDistance = dist;
             }
         }
 
-        System.out.println(minPos);
-
         return minPos;
     }