diff --git a/simgui-ds.json b/simgui-ds.json index 49cf3aa..69b1a3c 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,7 +91,7 @@ ], "robotJoysticks": [ { - "guid": "78696e70757401000000000000000000" + "guid": "Keyboard0" } ] } diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 1aaef2f..0108060 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -10,6 +10,10 @@ package frc4388.robot; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Rotations; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; import com.ctre.phoenix6.configs.CurrentLimitsConfigs; @@ -27,6 +31,20 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerFeedbackType; import com.ctre.phoenix6.swerve.SwerveModuleConstants.SteerMotorArrangement; import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; @@ -137,7 +155,6 @@ public final class Constants { public static final CanDevice RIGHT_BACK_ENCODER = new CanDevice("RIGHT_BACK_ENCODER", 13); public static final CanDevice DRIVE_PIGEON = new CanDevice("DRIVE_PIGEON", 14); - public static final CanDevice e = new CanDevice("NONEXISTANT_CAN", 50); } public static final class PIDConstants { @@ -169,13 +186,14 @@ public final class Constants { } public static final class AutoConstants { - public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); - public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); - public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0); - public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune - - public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value - public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value + public static final Gains XY_GAINS = new Gains(3,0,0); + public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); + + + public static final double XY_TOLERANCE = 0.05; + public static final double ROT_TOLERANCE = 1; + + public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d()); } @@ -269,7 +287,26 @@ public final class Constants { public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; } - public static final class VisionConstants { + public static final class VisionConstants { + public static final String CAMERA_NAME = "Camera_Module_v1"; + + public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413*0, .2794), new Rotation3d(0,0.52333,Math.PI)); + + // public static final AprilTagFieldLayout kTagLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); + + + // Test april tag field layout + public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout( + Arrays.asList(new AprilTag[] { + new AprilTag(1, new Pose3d( + new Translation3d(0.,0.,0.26035), new Rotation3d(0.,0.,0.) + )), + }), 100, 100); + + // The standard deviations of our vision estimated poses, which affect correction rate + // (Fake values. Experiment and determine estimation noise on an actual robot.) + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); } public static final class DriveConstants { @@ -289,4 +326,4 @@ public final class Constants { public static final double LEFT_AXIS_DEADBAND = 0.1; } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6f8887c..b127f61 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -20,6 +20,7 @@ import frc4388.utility.controller.XboxController; import frc4388.utility.controller.DeadbandedXboxController; import frc4388.robot.Constants.OIConstants; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.Constants.SwerveDriveConstants.AutoConstants; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -30,11 +31,13 @@ import edu.wpi.first.wpilibj2.command.RunCommand; // Autos import frc4388.utility.controller.VirtualController; +import frc4388.robot.commands.GotoPositionCommand; import frc4388.robot.commands.Swerve.neoJoystickPlayback; import frc4388.robot.commands.Swerve.neoJoystickRecorder; // Subsystems // import frc4388.robot.subsystems.LED; +import frc4388.robot.subsystems.Vision; import frc4388.robot.subsystems.SwerveDrive; // Utilites @@ -55,8 +58,11 @@ public class RobotContainer { /* Subsystems */ // private final LED m_robotLED = new LED(); + public final Vision m_vision = new Vision(m_robotMap.camera); + + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); + // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -172,6 +178,13 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUpRot())); // ? /* Operator Buttons */ + + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos)); + + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); + // creates an empty command & requires the swerve drive, subsystems can run only 1 command at a time // ? /* Programer Buttons (Controller 3)*/ @@ -222,7 +235,8 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return autoPlayback; + //return autoPlayback; + return new GotoPositionCommand(m_robotSwerveDrive, m_vision, AutoConstants.targetpos); } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 4990efa..3541a58 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -8,6 +8,9 @@ package frc4388.robot; import com.ctre.phoenix6.hardware.TalonFX; + +import org.photonvision.PhotonCamera; + import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveDrivetrain; @@ -18,6 +21,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; // import edu.wpi.first.wpilibj.motorcontrol.Spark; // import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.SwerveDriveConstants; +import frc4388.robot.Constants.VisionConstants; // import frc4388.robot.subsystems.SwerveModule; import frc4388.utility.RobotGyro; @@ -28,6 +32,8 @@ import frc4388.utility.RobotGyro; 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 RobotMap() { configureDriveMotorControllers(); diff --git a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java new file mode 100644 index 0000000..e7d82ed --- /dev/null +++ b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java @@ -0,0 +1,170 @@ +package frc4388.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +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.Constants.SwerveDriveConstants.AutoConstants; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Vision; +import frc4388.utility.Gains; +import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame; +import frc4388.utility.UtilityStructs.AutoRecordingFrame; +import frc4388.utility.controller.VirtualController; + +public class GotoPositionCommand extends Command { + + + // private Translation2d translation2d= new Translation2d(14.579471999999997,0.24587199999999998); + // private Translation2d translation2d= new Translation2d(16.579342-0.15,5.547867999999999); + + private PID xPID = new PID(AutoConstants.XY_GAINS, 0); + private PID yPID = new PID(AutoConstants.XY_GAINS, 0); + private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0); + private Pose2d targetpos; + + SwerveDrive swerveDrive; + Vision vision; + + /** + * Command to drive robot to position. + * @param SwerveDrive m_robotSwerveDrive + */ + + public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision, Pose2d targetpos) { + this.swerveDrive = swerveDrive; + this.vision = vision; + this.targetpos = targetpos; + addRequirements(swerveDrive); + } + + @Override + public void initialize() { + xPID.initialize(); + yPID.initialize(); + } + + double xerr; + double yerr; + double roterr; + + @Override + public void execute() { + xerr = targetpos.getX() - vision.getPose2d().getX(); + yerr = targetpos.getY() - vision.getPose2d().getY(); + roterr = targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees(); + + // SmartDashboard.putNumber("PID X Error", xerr); + // SmartDashboard.putNumber("PID Y Error", yerr); + + double xoutput = xPID.update(xerr); + double youtput = yPID.update(yerr); + double rotoutput = rotPID.update(roterr); + + Translation2d leftStick = new Translation2d( + Math.max(Math.min(youtput, 1), -1), + Math.max(Math.min(xoutput, 1), -1) + // 0,0 + ); + + Translation2d rightStick = new Translation2d( + Math.max(Math.min(rotoutput, 1), -1), + 0 + ); + + // SmartDashboard.putNumber("PID X Output", xoutput); + // SmartDashboard.putNumber("PID Y Output", youtput); + // // SmartDashboard.putNumber("PID Y Output", youtput); + + swerveDrive.driveWithInput(leftStick, rightStick, true); + } + + @Override + public final boolean isFinished() { + return (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE); + // this statement is a boolean in and of itself + } + + // @Override + // public void end(boolean interrupted) { + + // } + + + + // @Override + // public double getError() { + // return e; + // } + + + + // @Override + // public void runWithOutput(double output) { + // // TODO Auto-generated method stub + // Translation2d leftStick = new Translation2d(Math.max(Math.min(output, 1), -1),0); + // Translation2d rightStick = new Translation2d(); + // // System.out.println("Output = " + -output); + // SmartDashboard.putNumber("PID Output", output); + // swerveDrive.driveWithInput(leftStick, rightStick, true); + // } + + + + + + + + + + + + + + + + + + private class PID { + protected Gains gains; + private double output = 0; + private double tolerance = 0; + + /** Creates a new PelvicInflammatoryDisease. */ + public PID(double kp, double ki, double kd, double kf, double tolerance) { + gains = new Gains(kp, ki, kd, kf, 0); + this.tolerance = tolerance; + } + + public PID(Gains gains, double tolerance) { + this.gains = gains; + this.tolerance = tolerance; + } + + // Called when the command is initially scheduled. + public final void initialize() { + output = 0; + } + + private double prevError, cumError = 0; + + // Called every time the scheduler runs while the command is scheduled. + public double update(double error) { + cumError += error * .02; // 20 ms + double delta = error - prevError; + + output = error * gains.kP; + output += cumError * gains.kI; + output += delta * gains.kD; + output += gains.kF; + + return output; + } + + // // Returns true when the command should end. + // public final boolean isFinished() { + // return Math.abs(getError()) < tolerance; + // } + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index db8aba0..14bf6a8 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -26,13 +26,15 @@ import edu.wpi.first.wpilibj.smartdashboard.Field2d; 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.utility.Status; import frc4388.utility.Subsystem; import frc4388.utility.Status.ReportLevel; public class SwerveDrive extends Subsystem { private SwerveDrivetrain swerveDriveTrain; + + private Vision vision; private int gear_index = SwerveDriveConstants.STARTING_GEAR; private boolean stopped = false; @@ -45,16 +47,15 @@ public class SwerveDrive extends Subsystem { public Rotation2d orientRotTarget = new Rotation2d(); public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - private Field2d field = new Field2d(); - /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveDrivetrain swerveDriveTrain) { + public SwerveDrive(SwerveDrivetrain swerveDriveTrain, Vision vision) { + // public SwerveDrive(SwerveDrivetrain swerveDriveTrain) { super(); - SmartDashboard.putData(field); - this.swerveDriveTrain = swerveDriveTrain; + this.vision = vision; } + // public void oneModuleTest(SwerveModule module, Translation2d leftStick, Translation2d rightStick){ // // double ang = Math.atan2(rightStick.getY(), rightStick.getX()); // // rightStick.getAngle() @@ -154,6 +155,23 @@ public class SwerveDrive extends Subsystem { return false; } + public void driveWithInputRotation(Translation2d leftStick, Rotation2d rot) { + // if (leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going: + // stopModules(); // stop the swerve + + // if (leftStick.getNorm() < 0.05) //if no imput + // return; // don't bother doing swerve drive math and return early. + + leftStick = leftStick.rotateBy(Rotation2d.fromDegrees(SwerveDriveConstants.FORWARD_OFFSET)); + + swerveDriveTrain.setControl(new SwerveRequest.FieldCentricFacingAngle() + .withVelocityX(leftStick.getX()*-speedAdjust) + .withVelocityY(leftStick.getY()*speedAdjust) + .withTargetDirection(rot) + ); + // double + } + public double getGyroAngle() { return swerveDriveTrain.getRotation3d().getAngle(); } @@ -172,10 +190,15 @@ public class SwerveDrive extends Subsystem { SmartDashboard.putNumber("Gyro", getGyroAngle()); SmartDashboard.putNumber("RotTartget", rotTarget); - Optional e = swerveDriveTrain.samplePoseAt(Utils.getCurrentTimeSeconds()); + double time = Vision.getTime(); - if(e.isPresent()) - field.setRobotPose(e.get()); + vision.setLastOdomPose(swerveDriveTrain.samplePoseAt(time)); + + if(vision.isTag()){ + swerveDriveTrain.addVisionMeasurement(vision.getPose2d(), time); + } + + // if(e.isPresent()) } private void reset_index() { diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java new file mode 100644 index 0000000..07ca483 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -0,0 +1,281 @@ +package frc4388.robot.subsystems; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + +import java.util.List; +import java.util.Optional; + +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.simulation.PhotonCameraSim; +import org.photonvision.simulation.SimCameraProperties; +import org.photonvision.simulation.VisionSystemSim; +import org.photonvision.targeting.MultiTargetPNPResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +import com.ctre.phoenix6.Utils; + +import edu.wpi.first.networktables.GenericEntry; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts; +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.Constants.VisionConstants; +import frc4388.utility.Status; +import frc4388.utility.Subsystem; + +public class Vision extends Subsystem { + + private PhotonCamera camera; + + private boolean isTag = false; + private Pose2d lastVisionPose = new Pose2d(); + private Pose2d lastPhysOdomPose = new Pose2d(); + + private Matrix curStdDevs; + private final PhotonPoseEstimator photonEstimator; + + private Field2d field = new Field2d(); + + + ShuffleboardLayout subsystemLayout = Shuffleboard.getTab("Subsystems") + .getLayout(getSubsystemName(), BuiltInLayouts.kList) + .withSize(2, 2); + + GenericEntry sbTag = subsystemLayout + .add("Tag Detected", false) + .withWidget(BuiltInWidgets.kBooleanBox) + .getEntry(); + + GenericEntry sbCamConnected = subsystemLayout + .add("Camera Connnected", false) + .withWidget(BuiltInWidgets.kBooleanBox) + .getEntry(); + + public Vision(PhotonCamera camera){ + this.camera = camera; + SmartDashboard.putData(field); + + photonEstimator = new PhotonPoseEstimator(VisionConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, VisionConstants.CAMERA_POS); + photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY); + } + + @Override + public void periodic() { + var result = camera.getLatestResult(); + isTag = result.hasTargets(); + + // Optional multitag = result.getMultiTagResult(); + + // if (multitag.isEmpty()) { + // Transform3d fieldToCamera = result.getMultiTagResult().estimatedPose.best; + // }else if() + + + // sbTag.setBoolean(isTag); + // field.setRobotPose(getPose2d()); + + // sbCamConnected.setBoolean(camera); + + // System.out.println(isTag); + + if(!isTag){ + sbTag.setBoolean(isTag); + field.setRobotPose(getPose2d()); + return; + } + + var EstimatedRobotPose = getEstimatedGlobalPose(); + + // In case the pose estimator fails to estimate the pose, fallback to physical odometry. + if(EstimatedRobotPose.isEmpty()){ + isTag = false; + sbTag.setBoolean(isTag); + field.setRobotPose(getPose2d()); + return; + } + + lastVisionPose = EstimatedRobotPose.get().estimatedPose.toPose2d(); + // lastVisionPose.rotateBy(lastVisionPose.getRotation().minus(lastPhysOdomPose.getRotation())); + // lastVisionPose = new Pose2d( + // lastVisionPose.getTranslation(), + // lastPhysOdomPose.getRotation() + // ); + + + + field.setRobotPose(getPose2d()); + } + + + + + /** + * The latest estimated robot pose on the field from vision data. This may be empty. This should + * only be called once per loop. + * + *

Also includes updates for the standard deviations, which can (optionally) be retrieved with + * {@link getEstimationStdDevs} + * + * @return An {@link EstimatedRobotPose} with an estimated pose, estimate timestamp, and targets + * used for estimation. + */ + public Optional getEstimatedGlobalPose() { + Optional visionEst = Optional.empty(); + for (var change : camera.getAllUnreadResults()) { + visionEst = photonEstimator.update(change); + updateEstimationStdDevs(visionEst, change.getTargets()); + + // if (Robot.isSimulation()) { + // visionEst.ifPresentOrElse( + // est -> + // getSimDebugField() + // .getObject("VisionEstimation") + // .setPose(est.estimatedPose.toPose2d()), + // () -> { + // getSimDebugField().getObject("VisionEstimation").setPoses(); + // }); + // } + } + return visionEst; + } + + + + + + + + /** + * Calculates new standard deviations This algorithm is a heuristic that creates dynamic standard + * deviations based on number of tags, estimation strategy, and distance from the tags. + * + * @param estimatedPose The estimated pose to guess standard deviations for. + * @param targets All targets in this camera frame + */ + private void updateEstimationStdDevs( + Optional estimatedPose, List targets) { + if (estimatedPose.isEmpty()) { + // No pose input. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + + } else { + // Pose present. Start running Heuristic + var estStdDevs = VisionConstants.kSingleTagStdDevs; + int numTags = 0; + double avgDist = 0; + + // Precalculation - see how many tags we found, and calculate an average-distance metric + for (var tgt : targets) { + var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) continue; + numTags++; + avgDist += + tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + } + + if (numTags == 0) { + // No tags visible. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + } else { + // One or more tags visible, run the full heuristic. + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) estStdDevs = VisionConstants.kMultiTagStdDevs; + // Increase std devs based on (average) distance + if (numTags == 1 && avgDist > 4) + estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + else estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + curStdDevs = estStdDevs; + } + } + } + + /** + * Returns the latest standard deviations of the estimated pose from {@link + * #getEstimatedGlobalPose()}, for use with {@link + * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}. This should + * only be used when there are targets visible. + */ + public Matrix getEstimationStdDevs() { + return curStdDevs; + } + + + + + + + + + + public void setLastOdomPose(Optional pose){ + if(pose.isPresent()) + lastPhysOdomPose = pose.get(); + } + + public Pose2d getPose2d() { + if(isTag) + return lastVisionPose; + else + return lastPhysOdomPose; + } + + public static double getTime() { + return Utils.getCurrentTimeSeconds(); + } + + public boolean isTag(){ + return isTag; + } + + + + + + + + + + + + @Override + public String getSubsystemName() { + return "Vision"; + } + +// GenericEntry sbShiftState = subsystemLayout +// .add("Shift State", 0) +// .withWidget(BuiltInWidgets.kNumberBar) +// .getEntry(); + + + @Override + public void queryStatus() { + sbTag.setBoolean(isTag); + sbCamConnected.setBoolean(camera.isConnected()); + // field.setRobotPose(getPose2d()); + } + + @Override + public Status diagnosticStatus() { + Status status = new Status(); + + return status; + } + +} diff --git a/src/main/java/frc4388/utility/CanDevice.java b/src/main/java/frc4388/utility/CanDevice.java index ec05fdb..1805403 100644 --- a/src/main/java/frc4388/utility/CanDevice.java +++ b/src/main/java/frc4388/utility/CanDevice.java @@ -35,13 +35,13 @@ public class CanDevice { System.out.println(getName() + " - " + str); } - public Status queryStatus() { - Status s = new Status(); + // public Status queryStatus() { + // Status s = new Status(); - s.addReport(ReportLevel.INFO, "TODO"); + // s.addReport(ReportLevel.INFO, "TODO"); - return s; - } + // return s; + // } public Status diagnosticStatus() { Status s = new Status(); diff --git a/vendordeps/PathplannerLib-2025.1.1.json b/vendordeps/PathplannerLib-2025.1.1.json new file mode 100644 index 0000000..6322388 --- /dev/null +++ b/vendordeps/PathplannerLib-2025.1.1.json @@ -0,0 +1,38 @@ +{ + "fileName": "PathplannerLib-2025.1.1.json", + "name": "PathplannerLib", + "version": "2025.1.1", + "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", + "frcYear": "2025", + "mavenUrls": [ + "https://3015rangerrobotics.github.io/pathplannerlib/repo" + ], + "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", + "javaDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-java", + "version": "2025.1.1" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.pathplanner.lib", + "artifactId": "PathplannerLib-cpp", + "version": "2025.1.1", + "libName": "PathplannerLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal", + "linuxathena", + "linuxarm32", + "linuxarm64" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..db43d6d --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2025.0.0-beta-8", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2025", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-8", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2025.0.0-beta-8", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2025.0.0-beta-8", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2025.0.0-beta-8" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2025.0.0-beta-8" + } + ] +} \ No newline at end of file