From 2bc1d6eb9c0eb49fec8ec17ea5cd8335eeed6456 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 28 Jan 2025 19:00:44 -0700 Subject: [PATCH 1/9] lidar align --- .../frc4388/robot/commands/LidarAlign.java | 57 +++++++++++++++++++ 1 file changed, 57 insertions(+) create mode 100644 src/main/java/frc4388/robot/commands/LidarAlign.java diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java new file mode 100644 index 0000000..dfe3d34 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -0,0 +1,57 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc4388.robot.subsystems.Lidar; +import frc4388.robot.subsystems.SwerveDrive; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class LidarAlign extends Command { + private SwerveDrive swerveDrive; + private Lidar lidar; + + private int currentFinderTick; + // private int tickFoundPipe; + private boolean foundReef; + private boolean headedRight; + private final boolean constructedHeadedRight; + + /** Creates a new LidarAlign. */ + public LidarAlign(SwerveDrive swerveDrive, Lidar lidar, boolean headedRight) { + // Use addRequirements() here to declare subsystem dependencies. + constructedHeadedRight = headedRight; + + this.swerveDrive = swerveDrive; + this.lidar = lidar; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.currentFinderTick = 0; + this.foundReef = false; + this.headedRight = constructedHeadedRight; + } + + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double speed = 0.05; // TODO: find good speed for this + double relAngle = Math.round(swerveDrive.getGyroAngle() / 60.d) * 60; // Relative driving to the side of the reef + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} From 42e7896f5fa86fad1272f943c36d2ad03d59948e Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Wed, 29 Jan 2025 17:06:55 -0700 Subject: [PATCH 2/9] make lidar align theoretically work --- src/main/java/frc4388/robot/Constants.java | 1 + .../frc4388/robot/commands/LidarAlign.java | 43 +++++++++++++++++-- .../frc4388/robot/subsystems/SwerveDrive.java | 25 +++++++++++ 3 files changed, 65 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 679f07f..615b8ee 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -227,6 +227,7 @@ public final class Constants { .withKS(0).withKV(0.124); public static final Gains DRIFT_CORRECTION_GAINS = new Gains(2.5, 0, 0.1); + public static final Gains RELATIVE_LOCKED_ANGLE_GAINS = new Gains(5, 0, 0.1); // TODO: TEST } public static final class AutoConstants { diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java index dfe3d34..7f0e528 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -4,6 +4,8 @@ package frc4388.robot.commands; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.SwerveDrive; @@ -17,6 +19,7 @@ public class LidarAlign extends Command { // private int tickFoundPipe; private boolean foundReef; private boolean headedRight; + private double speed; private final boolean constructedHeadedRight; /** Creates a new LidarAlign. */ @@ -26,12 +29,15 @@ public class LidarAlign extends Command { this.swerveDrive = swerveDrive; this.lidar = lidar; + + addRequirements(swerveDrive, lidar); } // Called when the command is initially scheduled. @Override public void initialize() { this.currentFinderTick = 0; + this.speed = 0.05; // TODO: find good speed for this this.foundReef = false; this.headedRight = constructedHeadedRight; } @@ -40,18 +46,47 @@ public class LidarAlign extends Command { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double speed = 0.05; // TODO: find good speed for this + if (lidar.withinDistance()) { + swerveDrive.softStop(); + foundReef = true; + return; + } + + if (currentFinderTick > 100) { //arbutrary threshhold for now. + headedRight = !headedRight; + currentFinderTick *= -1; + } + double relAngle = Math.round(swerveDrive.getGyroAngle() / 60.d) * 60; // Relative driving to the side of the reef - + if (!headedRight) { + swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle)); + } else { + swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle)); + } + + currentFinderTick++; } // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + + } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + if (foundReef && lidar.withinDistance()) { + swerveDrive.stopModules(); + return true; + } else if (foundReef && !lidar.withinDistance()) { + speed = speed / 2; + headedRight = !headedRight; + currentFinderTick = 0; + foundReef = false; + return false; + } else { + return false; + } } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 0820891..9e403e7 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -210,6 +210,21 @@ public class SwerveDrive extends Subsystem { .withTargetDirection(rightStick.getAngle())); } + public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) { + leftStick = leftStick.rotateBy(heading); + + var ctrl = new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX() * speedAdjust) + .withVelocityY(leftStick.getY() * speedAdjust) + .withTargetDirection(Rotation2d.fromDegrees(rotTarget)); + ctrl.HeadingController.setPID( + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kP, + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kI, + SwerveDriveConstants.PIDConstants.RELATIVE_LOCKED_ANGLE_GAINS.kD + ); + swerveDriveTrain.setControl(ctrl); + } + public boolean rotateToTarget(double angle) { swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(0) @@ -248,6 +263,16 @@ public class SwerveDrive extends Subsystem { swerveDriveTrain.tareEverything(); } + + public void softStop() { + stopped = true; + swerveDriveTrain.setControl(new SwerveRequest.FieldCentric() + .withVelocityX(0) + .withVelocityY(0) + .withRotationalRate(0) + ); // stop the modules without breaking + } + public void stopModules() { stopped = true; swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake()); From 4afbf1656fa77a6d7fc4b3d5ebed8cefa089c933 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Thu, 30 Jan 2025 16:32:45 -0700 Subject: [PATCH 3/9] lidar debugging --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/commands/LidarAlign.java | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 615b8ee..27a0469 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -237,7 +237,7 @@ public final class Constants { public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.05, 0); public static final int LIDAR_DETECT_DISTANCE = 100; // Min distance to detect pole - public static final int LIDAR_DIO_CHANNEL = 0; + public static final int LIDAR_DIO_CHANNEL = 2; public static final int LIDAR_MICROS_TO_CM = 10; public static final int SECONDS_TO_MICROS = 1000000; diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java index 7f0e528..5ce0a7e 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -37,7 +37,7 @@ public class LidarAlign extends Command { @Override public void initialize() { this.currentFinderTick = 0; - this.speed = 0.05; // TODO: find good speed for this + this.speed = 0.1; // TODO: find good speed for this this.foundReef = false; this.headedRight = constructedHeadedRight; } @@ -76,10 +76,10 @@ public class LidarAlign extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - if (foundReef && lidar.withinDistance()) { + if (foundReef && lidar.withinDistance()) { // spot on swerveDrive.stopModules(); return true; - } else if (foundReef && !lidar.withinDistance()) { + } else if (foundReef && !lidar.withinDistance()) { // over shot speed = speed / 2; headedRight = !headedRight; currentFinderTick = 0; From e379794da1ca894fa05699f78472287fef35eadd Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 30 Jan 2025 19:28:46 -0700 Subject: [PATCH 4/9] lidar align works --- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/RobotContainer.java | 13 ++++++++++++- .../frc4388/robot/commands/GotoPositionCommand.java | 4 ++-- .../java/frc4388/robot/commands/LidarAlign.java | 8 ++++---- .../java/frc4388/robot/subsystems/SwerveDrive.java | 2 ++ 5 files changed, 22 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 27a0469..3ff474b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -105,8 +105,8 @@ public final class Constants { public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270 public static final boolean DRIFT_CORRECTION_ENABLED = true; - public static final boolean INVERT_X = true; - public static final boolean INVERT_Y = false; + public static final boolean INVERT_X = false; + public static final boolean INVERT_Y = true; public static final boolean INVERT_ROTATION = false; // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8d9505f..b73b7e5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -37,6 +37,7 @@ import edu.wpi.first.wpilibj2.command.Commands; // Autos import frc4388.utility.controller.VirtualController; import frc4388.robot.commands.GotoPositionCommand; +import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; @@ -106,6 +107,15 @@ public class RobotContainer { new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, true, false); private Command AutoGotoPosition = new GotoPositionCommand(m_robotSwerveDrive, m_vision); + private Command AprilLidarLeft = new SequentialCommandGroup( + AutoGotoPosition.asProxy(), + new LidarAlign(m_robotSwerveDrive, m_lidar, false) + ); + + private Command AprilLidarRight = new SequentialCommandGroup( + AutoGotoPosition.asProxy(), + new LidarAlign(m_robotSwerveDrive, m_lidar, true) + ); private Command placeCoral = new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.stopModules()), @@ -217,7 +227,8 @@ public class RobotContainer { // ? /* Operator Buttons */ new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(AutoGotoPosition); + // .onTrue(AutoGotoPosition); + .onTrue(AprilLidarRight); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); diff --git a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java index 4b4b115..593e812 100644 --- a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java +++ b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java @@ -65,8 +65,8 @@ public class GotoPositionCommand extends Command { double rotoutput = rotPID.update(roterr); Translation2d leftStick = new Translation2d( - Math.max(Math.min(youtput, 1), -1), - Math.max(Math.min(xoutput, 1), -1) + Math.max(Math.min(-youtput, 1), -1), + Math.max(Math.min(-xoutput, 1), -1) // 0,0 ); diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java index 5ce0a7e..8b96ad9 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -37,7 +37,7 @@ public class LidarAlign extends Command { @Override public void initialize() { this.currentFinderTick = 0; - this.speed = 0.1; // TODO: find good speed for this + this.speed = 0.4; // TODO: find good speed for this this.foundReef = false; this.headedRight = constructedHeadedRight; } @@ -57,11 +57,11 @@ public class LidarAlign extends Command { currentFinderTick *= -1; } - double relAngle = Math.round(swerveDrive.getGyroAngle() / 60.d) * 60; // Relative driving to the side of the reef + double relAngle = (Math.round(swerveDrive.getGyroAngle() / 60.d) * 60) + 90; // Relative driving to the side of the reef if (!headedRight) { - swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle)); - } else { swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle)); + } else { + swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle)); } currentFinderTick++; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 9e403e7..a16246f 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -12,6 +12,7 @@ import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.swerve.SwerveDrivetrain; import com.ctre.phoenix6.swerve.SwerveRequest; +import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -149,6 +150,7 @@ public class SwerveDrive extends Subsystem { .withVelocityX(leftStick.getX() * speedAdjust) .withVelocityY(leftStick.getY() * speedAdjust) .withRotationalRate(rightStick.getX() * rotSpeedAdjust)); + // .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective)); SmartDashboard.putBoolean("drift correction", false); } else { var ctrl = new SwerveRequest.FieldCentricFacingAngle() From 7fa93e689277606f2177086ea2b8e3316bb8f5fa Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Sat, 1 Feb 2025 15:52:06 -0700 Subject: [PATCH 5/9] Improve lidar align accuracy --- .../paths/Center Red Cage to Reef.path | 10 +++---- src/main/java/frc4388/robot/Constants.java | 11 +++---- .../java/frc4388/robot/RobotContainer.java | 21 +++++++++++-- ...ositionCommand.java => GotoLastApril.java} | 4 +-- .../frc4388/robot/commands/LidarAlign.java | 7 +++-- .../frc4388/robot/subsystems/Elevator.java | 9 ++++-- .../frc4388/robot/subsystems/SwerveDrive.java | 30 +++++++++++++++++-- 7 files changed, 71 insertions(+), 21 deletions(-) rename src/main/java/frc4388/robot/commands/{GotoPositionCommand.java => GotoLastApril.java} (97%) diff --git a/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path index 9597e5f..a2d7feb 100644 --- a/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.5546611575005915, - "y": 2.13657272729177 + "x": 5.96969696969697, + "y": 1.128219696969696 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.6833333333333327 }, "prevControl": { - "x": 5.563570773660507, - "y": 2.6306173444056773 + "x": 5.75625, + "y": 2.256439393939394 }, "nextControl": null, "isLocked": false, @@ -42,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -62.35402463626126 + "rotation": -60.52411099675423 }, "reversed": false, "folder": null, diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3ff474b..2e2cc3a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -250,8 +250,8 @@ public final class Constants { public static final class Configurations { - public static final double OPEN_LOOP_RAMP_RATE = 0.2; // Todo: Test. think this will help. - public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // Todo: Test. think this will help. + public static final double OPEN_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. + public static final double CLOSED_LOOP_RAMP_RATE = 0.4; // Todo: Test. think this will help. public static final double NEUTRAL_DEADBAND = 0.04; // POWER! (limiting) @@ -283,7 +283,7 @@ public final class Constants { // new ClosedLoopRampsConfigs() // .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) ); - private static final double SLIP_CURRENT = 100; // TODO: Tune??? + public static final double SLIP_CURRENT = 60; // TODO: Tune??? } // No mans land @@ -355,8 +355,9 @@ public final class Constants { public static final class FieldConstants { public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); - public static final double HORISONTAL_SCORING_POSITION_OFFSET = .2794 - ; + // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(9.5); + public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); + ;;;;;;;;;;;;;;;;;;;;;;;;;;;; // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b73b7e5..710dd27 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -36,7 +36,7 @@ import edu.wpi.first.wpilibj2.command.Commands; // Autos import frc4388.utility.controller.VirtualController; -import frc4388.robot.commands.GotoPositionCommand; +import frc4388.robot.commands.GotoLastApril; import frc4388.robot.commands.LidarAlign; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; @@ -106,7 +106,12 @@ public class RobotContainer { () -> autoplaybackName.get(), // lastAutoName new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, true, false); - private Command AutoGotoPosition = new GotoPositionCommand(m_robotSwerveDrive, m_vision); + private Command AutoGotoPosition = new GotoLastApril(m_robotSwerveDrive, m_vision); + private Command AprilLidarAlign = new SequentialCommandGroup( + new GotoLastApril(m_robotSwerveDrive, m_vision), + new InstantCommand(() -> System.out.println("Soup")), + new LidarAlign(m_robotSwerveDrive, m_lidar, true) + ); private Command AprilLidarLeft = new SequentialCommandGroup( AutoGotoPosition.asProxy(), new LidarAlign(m_robotSwerveDrive, m_lidar, false) @@ -114,6 +119,7 @@ public class RobotContainer { private Command AprilLidarRight = new SequentialCommandGroup( AutoGotoPosition.asProxy(), + new InstantCommand(() -> System.out.println("Soup")), new LidarAlign(m_robotSwerveDrive, m_lidar, true) ); @@ -141,7 +147,7 @@ public class RobotContainer { public RobotContainer() { NamedCommands.registerCommand("AutoGotoPosition", AutoGotoPosition); - NamedCommands.registerCommand("april-allign", AutoGotoPosition); + NamedCommands.registerCommand("april-allign", AprilLidarAlign); NamedCommands.registerCommand("place-coral", placeCoral); NamedCommands.registerCommand("grab-coral", grabCoral); @@ -218,6 +224,15 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.deactivateLuigiMode())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + new Trigger(() -> getDeadbandedDriverController().getPOV() == 270) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDownRot())); diff --git a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java similarity index 97% rename from src/main/java/frc4388/robot/commands/GotoPositionCommand.java rename to src/main/java/frc4388/robot/commands/GotoLastApril.java index 593e812..168055e 100644 --- a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java +++ b/src/main/java/frc4388/robot/commands/GotoLastApril.java @@ -15,7 +15,7 @@ import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; import frc4388.utility.UtilityStructs.AutoRecordingFrame; import frc4388.utility.controller.VirtualController; -public class GotoPositionCommand extends Command { +public class GotoLastApril extends Command { // private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998); @@ -34,7 +34,7 @@ public class GotoPositionCommand extends Command { * @param SwerveDrive m_robotSwerveDrive */ - public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) { + public GotoLastApril(SwerveDrive swerveDrive, Vision vision) { this.swerveDrive = swerveDrive; this.vision = vision; // addRequirements(swerveDrive); diff --git a/src/main/java/frc4388/robot/commands/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java index 8b96ad9..7525718 100644 --- a/src/main/java/frc4388/robot/commands/LidarAlign.java +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -6,6 +6,7 @@ package frc4388.robot.commands; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import frc4388.robot.subsystems.Lidar; import frc4388.robot.subsystems.SwerveDrive; @@ -56,8 +57,10 @@ public class LidarAlign extends Command { headedRight = !headedRight; currentFinderTick *= -1; } - - double relAngle = (Math.round(swerveDrive.getGyroAngle() / 60.d) * 60) + 90; // Relative driving to the side of the reef + double currentHeading = (swerveDrive.getGyroAngle() * 180) / Math.PI; + double relAngle = (Math.round(currentHeading / 60.d) * 60); // Relative driving to the side of the reef + SmartDashboard.putNumber("Rel Angle", relAngle); + SmartDashboard.putNumber("heading", currentHeading); if (!headedRight) { swerveDrive.driveRelativeLockedAngle(new Translation2d(0, -speed), Rotation2d.fromDegrees(relAngle)); } else { diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java index f7fa030..78c7588 100644 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ b/src/main/java/frc4388/robot/subsystems/Elevator.java @@ -24,9 +24,14 @@ public class Elevator extends SubsystemBase { public enum CoordinationState { Waiting, // for coral into the though Ready, // Has coral in enefector + PrimedThree, // Arm and elevator Waiting to score in the level 3 position ScoringThree, // Arm and elevator in the level three position - ScoringFour // Arm and elevator in the level four position - + PrimedFour, // Arm and elevator Waiting to score in the level 4 position + ScoringFour, // Arm and elevator in the level four position + BallRemoverL2Primed, // Arm and elevator ready to remove the ball in the level 2 reef. + BallRemoverL2Go, // Arm and elevator removing the ball in the level 2 reef. + BallRemoverL3Primed, // Arm and elevator ready to remove the ball in the level 3 reef. + BallRemoverL3Go, // Arm and elevator removing the ball in the level 3 reef. } private CoordinationState currentState; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index a16246f..812f16a 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -8,9 +8,11 @@ import java.util.Optional; import java.util.function.DoubleSupplier; import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.swerve.SwerveDrivetrain; +import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; @@ -227,6 +229,26 @@ public class SwerveDrive extends Subsystem { swerveDriveTrain.setControl(ctrl); } + public void setLimits(double limitInAmps) { + for (SwerveModule module : swerveDriveTrain.getModules()) { + var talonFXConfigurator = module.getDriveMotor().getConfigurator(); + var talonFXConfigs = new TalonFXConfiguration(); + + talonFXConfigurator.refresh(talonFXConfigs); + talonFXConfigs.CurrentLimits.StatorCurrentLimit = limitInAmps; + talonFXConfigs.CurrentLimits.SupplyCurrentLimit = limitInAmps+10; + talonFXConfigurator.apply(talonFXConfigs); + } + } + + public void activateLuigiMode() { + setLimits(20); + } + + public void deactivateLuigiMode() { + setLimits(SwerveDriveConstants.Configurations.SLIP_CURRENT); + } + public boolean rotateToTarget(double angle) { swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() .withVelocityX(0) @@ -258,7 +280,11 @@ public class SwerveDrive extends Subsystem { } public double getGyroAngle() { - return swerveDriveTrain.getRotation3d().getAngle(); + return getPose2d().getRotation().getRadians(); + } + + public Pose2d getPose2d() { + return swerveDriveTrain.samplePoseAt(Vision.getTime()).orElse(initalPose2d); } public void resetGyro() { @@ -283,7 +309,7 @@ public class SwerveDrive extends Subsystem { @Override public void periodic() { // This method will be called once per scheduler run\ - SmartDashboard.putNumber("Gyro", getGyroAngle()); + SmartDashboard.putNumber("Gyro", (getGyroAngle() * 180) / Math.PI); SmartDashboard.putNumber("RotTartget", rotTarget); double time = Vision.getTime(); From 3cacb7743e21fae7194049b8e286f54bf5034865 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 4 Feb 2025 16:34:54 -0700 Subject: [PATCH 6/9] Add code from yesterday --- src/main/java/frc4388/robot/Constants.java | 4 +- .../java/frc4388/robot/RobotContainer.java | 7 ++- .../frc4388/robot/commands/GotoLastApril.java | 12 +++++ .../robot/commands/MoveForTimeCommand.java | 47 +++++++++++++++++++ 4 files changed, 67 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/MoveForTimeCommand.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2e2cc3a..9680c8b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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 = diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 710dd27..11c3882 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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( diff --git a/src/main/java/frc4388/robot/commands/GotoLastApril.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java index 168055e..f574652 100644 --- a/src/main/java/frc4388/robot/commands/GotoLastApril.java +++ b/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 } diff --git a/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java new file mode 100644 index 0000000..9913cbe --- /dev/null +++ b/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; + } +} \ No newline at end of file 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 7/9] 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; } From 0bfd2f0b5b7aa58af8ba9181cc26b3bb194fb336 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 6 Feb 2025 13:02:39 -0700 Subject: [PATCH 8/9] Add multiple cameras --- src/main/java/frc4388/robot/Constants.java | 6 +- .../java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/RobotMap.java | 3 +- .../java/frc4388/robot/subsystems/Vision.java | 162 ++++++++++-------- 4 files changed, 100 insertions(+), 73 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index ec01c5d..236b237 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -340,9 +340,11 @@ public final class Constants { } public static final class VisionConstants { - public static final String CAMERA_NAME = "Camera_Module_v1"; + public static final String LEFT_CAMERA_NAME = "CAMERA_LEFT"; + public static final String RIGHT_CAMERA_NAME = "CAMERA_RIGHT"; - public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI)); + public static final Transform3d LEFT_CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI)); + public static final Transform3d RIGHT_CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI)); public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b84bf62..9db5009 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -75,7 +75,7 @@ public class RobotContainer { /* Subsystems */ // private final LED m_robotLED = new LED(); - public final Vision m_vision = new Vision(m_robotMap.camera); + public final Vision m_vision = new Vision(m_robotMap.leftCamera, m_robotMap.rightCamera); public final Lidar m_lidar = new Lidar(); public final Elevator m_robotElevator= new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endefectorLimitSwitch); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index a5a630a..d996288 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -35,7 +35,8 @@ public class RobotMap { // private Pigeon2 m_pigeon2 = new Pigeon2(SwerveDriveConstants.IDs.DRIVE_PIGEON.id); // public RobotGyro gyro = new RobotGyro(m_pigeon2); - public PhotonCamera camera = new PhotonCamera(VisionConstants.CAMERA_NAME); + public PhotonCamera leftCamera = new PhotonCamera(VisionConstants.LEFT_CAMERA_NAME); + public PhotonCamera rightCamera = new PhotonCamera(VisionConstants.RIGHT_CAMERA_NAME); public RobotMap() { configureDriveMotorControllers(); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index aeed160..b9d4122 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -39,7 +39,11 @@ import frc4388.utility.Subsystem; public class Vision extends Subsystem { - private PhotonCamera camera; + // private PhotonCamera leftCamera; + // private PhotonCamera rightCamera; + + private PhotonCamera[] cameras; + private PhotonPoseEstimator[] estimators; private boolean isTagDetected = false; private boolean isTagProcessed = false; @@ -47,7 +51,8 @@ public class Vision extends Subsystem { private Pose2d lastPhysOdomPose = new Pose2d(); private Matrix curStdDevs; - private final PhotonPoseEstimator photonEstimator; + private final PhotonPoseEstimator photonEstimatorLeft; + private final PhotonPoseEstimator photonEstimatorRight; private Field2d field = new Field2d(); @@ -66,59 +71,87 @@ public class Vision extends Subsystem { .withWidget(BuiltInWidgets.kBooleanBox) .getEntry(); - GenericEntry sbCamConnected = subsystemLayout - .add("Camera Connnected", false) + GenericEntry sbLeftCamConnected = subsystemLayout + .add("Left Camera Connnected", false) + .withWidget(BuiltInWidgets.kBooleanBox) + .getEntry(); + + GenericEntry sbRightCamConnected = subsystemLayout + .add("Right Camera Connnected", false) .withWidget(BuiltInWidgets.kBooleanBox) .getEntry(); - public Vision(PhotonCamera camera){ - this.camera = camera; + public Vision(PhotonCamera leftCamera, PhotonCamera rightCamera){ SmartDashboard.putData(field); - photonEstimator = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.CAMERA_POS); - photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + this.cameras = new PhotonCamera[]{leftCamera, rightCamera}; + + photonEstimatorLeft = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.LEFT_CAMERA_POS); + photonEstimatorRight = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.RIGHT_CAMERA_POS); + + photonEstimatorLeft.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + + this.estimators = new PhotonPoseEstimator[]{photonEstimatorLeft, photonEstimatorRight}; } @Override public void periodic() { - // var result = camera.getLatestResult(); - var results = camera.getAllUnreadResults(); - if (results.size() == 0) return; - var result = results.get(results.size()-1); - - isTagDetected = result.hasTargets(); - isTagProcessed = false; - - - if(!isTagDetected){ - // sbTagDetected.setBoolean(isTagDetected); - field.setRobotPose(getPose2d()); - return; - } - - var EstimatedRobotPose = getEstimatedGlobalPose(result); - - // In case the pose estimator fails to estimate the pose, fallback to physical odometry. - if(EstimatedRobotPose.isEmpty()){ - field.setRobotPose(getPose2d()); - return; - } - - isTagProcessed = true; - - lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d(); - // lastVisionPose.rotateBy(Rotation2d.k180deg); - // lastVisionPose = new Pose2d( - // lastVisionPose.getTranslation(), - // lastPhysOdomPose.getRotation() - // ); - - - + update(); field.setRobotPose(getPose2d()); } + private void update() { + isTagProcessed = false; + isTagDetected = false; + + + int cams = 0; + + double X = 0; + double Y = 0; + double Yaw = 0; + + for(int i = 0; i < cameras.length; i++){ + PhotonCamera camera = cameras[i]; + PhotonPoseEstimator estimator = estimators[i]; + + var results = camera.getAllUnreadResults(); + + // If there are no more updates from the camera + if (results.size() == 0) + continue; + + + var result = results.get(results.size()-1); + + isTagDetected = isTagDetected | result.hasTargets(); + + // If there are no tags + if(!result.hasTargets()) + continue; + + Optional estimatedRobotPose = getEstimatedGlobalPose(result, estimator); + + // If the tag was failed to be processed + if(estimatedRobotPose.isEmpty()) + continue; + + Pose2d pose = estimatedRobotPose.get().estimatedPose.toPose2d(); + X += pose.getX(); + Y += pose.getY(); + Yaw += pose.getRotation().getDegrees(); + cams++; + isTagProcessed = true; + + + } + + if(isTagProcessed){ + lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(Yaw/cams)); + } + } /** @@ -131,36 +164,24 @@ public class Vision extends Subsystem { * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets * used for estimation. */ - public Optional getEstimatedGlobalPose(PhotonPipelineResult change) { + public Optional getEstimatedGlobalPose(PhotonPipelineResult change, PhotonPoseEstimator estimator) { Optional visionEst = Optional.empty(); - // for (var change : camera.getAllUnreadResults()) { - var targets = change.getTargets(); - for(int i = targets.size()-1; i >= 0; i--){ - Transform3d pos = targets.get(i).getBestCameraToTarget(); - double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2)); - if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) { - change.targets.remove(i); - } + var targets = change.getTargets(); + for(int i = targets.size()-1; i >= 0; i--){ + Transform3d pos = targets.get(i).getBestCameraToTarget(); + double distance = Math.sqrt(Math.pow(pos.getX(),2) + Math.pow(pos.getY(),2) + Math.pow(pos.getZ(),2)); + if (distance > VisionConstants.MIN_ESTIMATION_DISTANCE) { + change.targets.remove(i); } + } - if(targets.size() <= 0) - return visionEst; // Will be empty + if(targets.size() <= 0) + return visionEst; // Will be empty - visionEst = photonEstimator.update(change); - updateEstimationStdDevs(visionEst, change.getTargets()); + visionEst = estimator.update(change); + updateEstimationStdDevs(visionEst, change.getTargets(), estimator); - // if (Robot.isSimulation()) { - // visionEst.ifPresentOrElse( - // est -> - // getSimDebugField() - // .getObject("VisionEstimation") - // .setPose(est.estimatedPose.toPose2d()), - // () -> { - // getSimDebugField().getObject("VisionEstimation").setPoses(); - // }); - // } - // } return visionEst; } @@ -173,7 +194,9 @@ public class Vision extends Subsystem { * @param targets All targets in this camera frame */ private void updateEstimationStdDevs( - Optional estimatedPose, List targets) { + Optional estimatedPose, + List targets, + PhotonPoseEstimator estimator) { if (estimatedPose.isEmpty()) { // No pose input. Default to single-tag std devs curStdDevs = VisionConstants.kSingleTagStdDevs; @@ -186,7 +209,7 @@ public class Vision extends Subsystem { // Precalculation - see how many tags we found, and calculate an average-distance metric for (var tgt : targets) { - var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + var tagPose = estimator.getFieldTags().getTagPose(tgt.getFiducialId()); if (tagPose.isEmpty()) continue; double distance = tagPose @@ -281,7 +304,8 @@ public class Vision extends Subsystem { public void queryStatus() { sbTagDetected.setBoolean(isTagDetected); sbTagProcessed.setBoolean(isTagProcessed); - sbCamConnected.setBoolean(camera.isConnected()); + sbLeftCamConnected.setBoolean(cameras[0].isConnected()); + sbRightCamConnected.setBoolean(cameras[1].isConnected()); // field.setRobotPose(getPose2d()); } From d88eff2f84268e9c4d06e92f099517e7fac2585d Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Thu, 6 Feb 2025 13:23:16 -0700 Subject: [PATCH 9/9] Forgot to remove variables --- src/main/java/frc4388/robot/subsystems/Vision.java | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index b9d4122..a66a00a 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -51,8 +51,6 @@ public class Vision extends Subsystem { private Pose2d lastPhysOdomPose = new Pose2d(); private Matrix curStdDevs; - private final PhotonPoseEstimator photonEstimatorLeft; - private final PhotonPoseEstimator photonEstimatorRight; private Field2d field = new Field2d(); @@ -86,8 +84,8 @@ public class Vision extends Subsystem { this.cameras = new PhotonCamera[]{leftCamera, rightCamera}; - photonEstimatorLeft = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.LEFT_CAMERA_POS); - photonEstimatorRight = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.RIGHT_CAMERA_POS); + PhotonPoseEstimator photonEstimatorLeft = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.LEFT_CAMERA_POS); + PhotonPoseEstimator photonEstimatorRight = new PhotonPoseEstimator(FieldConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.RIGHT_CAMERA_POS); photonEstimatorLeft.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); photonEstimatorRight.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY);