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 679f07f..236b237 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); @@ -227,21 +227,22 @@ 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 { - 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); 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; - public static final double XY_TOLERANCE = 0.1; - public static final double ROT_TOLERANCE = 1; + 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()); // public static final Pose2d targetpos = @@ -249,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) @@ -282,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 @@ -339,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 @@ -354,8 +357,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 8d9505f..9db5009 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -36,7 +36,9 @@ 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.MoveForTimeCommand; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; @@ -73,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); @@ -105,7 +107,26 @@ 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 WaitCommand(1), + new LidarAlign(m_robotSwerveDrive, m_lidar) + ); + private Command AprilLidarLeft = new SequentialCommandGroup( + AutoGotoPosition.asProxy(), + 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)//, + // new MoveForTimeCommand(m_robotSwerveDrive, + // new Translation2d(0, 0.5), new Translation2d(), 1000, true) + ); private Command placeCoral = new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.stopModules()), @@ -131,7 +152,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); @@ -208,6 +229,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())); @@ -217,10 +247,11 @@ 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)); + .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive, m_lidar)); DualJoystickButton(getDeadbandedOperatorController(), getVirtualOperatorController(), XboxController.A_BUTTON) 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/commands/GotoPositionCommand.java b/src/main/java/frc4388/robot/commands/GotoLastApril.java similarity index 89% rename from src/main/java/frc4388/robot/commands/GotoPositionCommand.java rename to src/main/java/frc4388/robot/commands/GotoLastApril.java index 4b4b115..60f87ac 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,12 +34,18 @@ 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); } + + public static double tagRelativeXError = -1; + private static void setTagRelativeXError(double val){ + tagRelativeXError = val; + } + @Override public void initialize() { xPID.initialize(); @@ -65,8 +71,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 ); @@ -75,9 +81,10 @@ public class GotoPositionCommand extends Command { 0 ); - // 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); } @@ -86,6 +93,10 @@ public class GotoPositionCommand 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/LidarAlign.java b/src/main/java/frc4388/robot/commands/LidarAlign.java new file mode 100644 index 0000000..a40a878 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/LidarAlign.java @@ -0,0 +1,94 @@ +// 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.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; + +/* 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 double speed; + // private final boolean constructedHeadedRight; + + /** Creates a new LidarAlign. */ + public LidarAlign(SwerveDrive swerveDrive, Lidar lidar) {//, boolean headedRight) { + // Use addRequirements() here to declare subsystem dependencies. + + 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.4; // TODO: find good speed for this + this.foundReef = false; + this.headedRight = !(GotoLastApril.tagRelativeXError < 0); + } + + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (lidar.withinDistance()) { + swerveDrive.softStop(); + foundReef = true; + return; + } + + if (currentFinderTick > 100) { //arbutrary threshhold for now. + headedRight = !headedRight; + currentFinderTick *= -1; + } + 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 { + swerveDrive.driveRelativeLockedAngle(new Translation2d(0, speed), Rotation2d.fromDegrees(relAngle)); + } + + currentFinderTick++; + } + + // 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() { + if (foundReef && lidar.withinDistance()) { // spot on + swerveDrive.stopModules(); + return true; + } else if (foundReef && !lidar.withinDistance()) { // over shot + speed = speed / 2; + headedRight = !headedRight; + currentFinderTick = 0; + foundReef = false; + return false; + } else { + return false; + } + } +} 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..88166a5 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/MoveForTimeCommand.java @@ -0,0 +1,48 @@ +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) { + addRequirements(swerveDrive); + + 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 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 0820891..9c2a1dc 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -8,10 +8,13 @@ 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; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -28,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; @@ -149,6 +154,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() @@ -210,6 +216,41 @@ 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 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) @@ -241,13 +282,27 @@ 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() { 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()); @@ -256,10 +311,11 @@ 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(); + vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time)); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index aeed160..a66a00a 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,6 @@ public class Vision extends Subsystem { private Pose2d lastPhysOdomPose = new Pose2d(); private Matrix curStdDevs; - private final PhotonPoseEstimator photonEstimator; private Field2d field = new Field2d(); @@ -66,59 +69,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}; + + 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); + + 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 +162,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 +192,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 +207,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 +302,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()); } 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; }