From 426d9f231a1eb20b278cbe992ef6e71efa7e46f5 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 4 Feb 2025 19:44:24 -0700 Subject: [PATCH] Limelight lidar stuff --- src/main/java/frc4388/robot/Constants.java | 4 ++-- .../java/frc4388/robot/RobotContainer.java | 12 +++++------ .../frc4388/robot/commands/GotoLastApril.java | 21 +++++++++---------- .../frc4388/robot/commands/LidarAlign.java | 7 +++---- .../robot/commands/MoveForTimeCommand.java | 1 + .../frc4388/robot/subsystems/SwerveDrive.java | 3 +++ .../frc4388/utility/ReefPositionHelper.java | 3 --- 7 files changed, 25 insertions(+), 26 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9680c8b..ec01c5d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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()); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 11c3882..b84bf62 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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) diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index f574652..60f87ac 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/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); } diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java index 7525718..a40a878 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/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); } diff --git a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java index 9913cbe..88166a5 100644 --- a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java +++ b/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; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 812f16a..9c2a1dc 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/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)); diff --git a/src/main/java/frc4388/utility/ReefPositionHelper.java b/src/main/java/frc4388/utility/ReefPositionHelper.java index 47a7991..3890a20 100644 --- a/src/main/java/frc4388/utility/ReefPositionHelper.java +++ b/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; }