diff --git a/.idea/modules.xml b/.idea/modules.xml deleted file mode 100644 index 1a56902e..00000000 --- a/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713c..4726e2df 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,13 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + }, + { + "guid": "Keyboard1" + } ] } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ce872885..493579d4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,6 +7,7 @@ import java.util.Optional; import java.util.concurrent.atomic.AtomicReference; +import frc.robot.constants.GameConstants; import frc.robot.utils.diag.Diagnostics; import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; @@ -122,6 +123,10 @@ public void robotPeriodic() { SmartDashboard.putNumber("driverXbox::getRightX", driverXbox.getRightX()); if(!Constants.TESTBED){ Logger.recordOutput("MyPose", robotContainer.getDriveBase().getPose()); + Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getOdom()); + if (Constants.currentMode == GameConstants.Mode.SIM) { + Logger.recordOutput("SimPose", robotContainer.getDriveBase().getSimulationPose().get()); + } // Puts data on the elastic dashboard SmartDashboard.putString("Alliance Color", Robot.allianceColorString()); SmartDashboard.putBoolean("Hub Active?", hubActive()); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0edebafe..26116d42 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -63,6 +63,9 @@ //import frc.robot.subsystems.RollerSubsystem; //import frc.robot.subsystems.TiltSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.subsystems.swervedrive.vision.truster.ConstantVisionTruster; +import frc.robot.subsystems.swervedrive.vision.truster.SquareVisionTruster; +import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; import frc.robot.utils.logging.io.gyro.RealGyroIo; import frc.robot.utils.logging.io.gyro.ThreadedGyro; import frc.robot.utils.logging.io.gyro.ThreadedGyroSwerveIMU; @@ -81,6 +84,8 @@ import choreo.auto.AutoRoutine; import choreo.auto.AutoTrajectory; +import static frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator.visionMeasurementStdDevs2; + /** * This class is where the bulk of the robot should be declared. Since * Command-based is a @@ -118,6 +123,7 @@ public class RobotContainer { private AutoFactory autoFactory; private static AutoRoutine straightRoutine; private static AutoTrajectory straightTrajectory; + private final VisionTruster truster = new SquareVisionTruster(visionMeasurementStdDevs2, Constants.VISION_STD_DEV_CONST); // Replace with CommandPS4Controller or CommandJoystick if needed // new CommandXboxController(OperatorConstants.kDriverControllerPort);private @@ -151,7 +157,7 @@ public RobotContainer() { drivebase = !Constants.TESTBED ? new SwerveSubsystem( new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), swerveIMU) : null; - apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase) : null; + apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null; controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; } @@ -169,7 +175,7 @@ public RobotContainer() { // No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null; - apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase) : null; + apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase, truster) : null; controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; } @@ -190,7 +196,7 @@ public RobotContainer() { // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null; controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; - apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(), drivebase) : null; + apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(truster,drivebase), drivebase, truster) : null; } default -> { @@ -510,7 +516,7 @@ public void putShuffleboardCommands() { public Command getAutonomousCommand() { return autoChooser.getCommand(); // return straightRoutine.cmd(straightTrajectory.done()); - + // return new ExampleAuto(drivebase, autoFactory); } @@ -534,7 +540,7 @@ public SwerveSubsystem getDriveBase() { return drivebase; } - public ShootingState getShootingState() { + public ShootingState getShootingState() { return shootState; } } diff --git a/src/main/java/frc/robot/apriltags/SimApriltagIO.java b/src/main/java/frc/robot/apriltags/SimApriltagIO.java index 9c30fa5f..6b4c7816 100644 --- a/src/main/java/frc/robot/apriltags/SimApriltagIO.java +++ b/src/main/java/frc/robot/apriltags/SimApriltagIO.java @@ -1,14 +1,64 @@ package frc.robot.apriltags; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.*; +import edu.wpi.first.math.numbers.N3; import frc.robot.constants.Constants; +import frc.robot.subsystems.ApriltagSubsystem; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator; +import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter; +import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; +import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; import frc.robot.utils.Apriltag; import frc.robot.utils.logging.io.BaseIoImpl; + +import java.util.Optional; import java.util.Queue; +import java.util.Random; +import java.util.function.Supplier; + +import frc.robot.utils.math.ObjectUtils; import org.littletonrobotics.junction.Logger; public class SimApriltagIO extends TCPApriltagIo { - public SimApriltagIO(String name, ApriltagInputs inputs, SimTCPServer server) { + private final Random random = new Random(); + private final VisionTruster truster; + private final SwerveSubsystem swerveSubsystem; + public SimApriltagIO(String name, ApriltagInputs inputs, SimTCPServer server, VisionTruster truster, SwerveSubsystem swerveSubsystem) { super(name, inputs, server); + this.truster = truster; + this.swerveSubsystem = swerveSubsystem; + } + public void simReadings() { + if (swerveSubsystem.getSimulationPose().isPresent()) { + Pose2d pose = swerveSubsystem.getSimulationPose().get(); + for (Apriltag tag : Apriltag.values()) { + Pose3d cameraPos = new Pose3d(pose).transformBy(Constants.ROBOT_TO_CAMERA); + if (ObjectUtils.canSee(tag.getPose(), cameraPos, Constants.HORIZONTAL_FOV, Constants.VERTICAL_FOV)) { + Translation3d adjPose2 = cameraPos.relativeTo(tag.getPose()).getTranslation(); + double distance = adjPose2.getNorm(); + double distanceTimesCosIncidenceAngle = adjPose2.getX(); + if (distanceTimesCosIncidenceAngle!=0 && distance*distance/distanceTimesCosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) { + VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), tag.getTagInfo(), distance, 0); + Vector stdDevs = truster.calculateTrust(measurement, cameraPos); + double readingX = pose.getX() + random.nextGaussian() * stdDevs.get(0); + double readingY = pose.getY() + random.nextGaussian() * stdDevs.get(1); + double readingYaw = pose.getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2); + Pose2d readingPos = new Pose2d(readingX, readingY, Rotation2d.fromDegrees(readingYaw)); + distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation()); + if (BasicVisionFilter.inBounds(readingPos)) { + addReading(new ApriltagReading(readingX, readingY, readingYaw, + distance, tag.number(), Constants.AVERAGE_CAM_LATENCY + random.nextGaussian() * Constants.AVERAGE_CAM_LATENCY_STD_DEV, Logger.getTimestamp() / 1000.0)); + } + } + } + } + } + } + @Override + public void periodic() { + super.periodic(); + simReadings(); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/apriltags/TCPApriltagServer.java b/src/main/java/frc/robot/apriltags/TCPApriltagServer.java index 70314611..86d152c5 100644 --- a/src/main/java/frc/robot/apriltags/TCPApriltagServer.java +++ b/src/main/java/frc/robot/apriltags/TCPApriltagServer.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj.Timer; import java.io.DataInputStream; import java.io.IOException; +import frc.robot.constants.Constants; public class TCPApriltagServer extends TCPServer { @@ -20,7 +21,7 @@ protected ApriltagReading extractFromStream(DataInputStream stream) throws IOExc double posY = -1; double poseYaw = -1; double distanceToTag = -1; - double timestamp = -1; + double latency = -1; int apriltagNumber = -1; double now = 0; while (posX == -1 @@ -28,16 +29,16 @@ protected ApriltagReading extractFromStream(DataInputStream stream) throws IOExc && poseYaw == -1 && distanceToTag == -1 && apriltagNumber == -1 - && timestamp == -1) { + && latency == -1) { posX = stream.readDouble(); posY = stream.readDouble(); poseYaw = stream.readDouble(); distanceToTag = stream.readDouble(); - timestamp = stream.readDouble(); + latency = stream.readDouble(); apriltagNumber = stream.readInt(); - now = Timer.getFPGATimestamp() * 1000; + now = Timer.getFPGATimestamp() * 1000-Constants.AVERAGE_PIR_LATENCY-latency; } - return new ApriltagReading(posX, posY, poseYaw, distanceToTag, apriltagNumber, timestamp, now); + return new ApriltagReading(posX, posY, poseYaw, distanceToTag, apriltagNumber, latency, now); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/drive/DriveCircle.java b/src/main/java/frc/robot/commands/drive/DriveCircle.java new file mode 100644 index 00000000..76366ba8 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/DriveCircle.java @@ -0,0 +1,44 @@ +package frc.robot.commands.drive; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class DriveCircle extends LoggableCommand { + private final SwerveSubsystem drivebase; + private final double radiusMeters; + private final double angularVelocityRadPerSec; + private final double time; + private final Timer timer; + public DriveCircle(SwerveSubsystem drivebase, double radiusMeters, double angularVelocityRadPerSec, double time) { + this.drivebase = drivebase; + this.radiusMeters = radiusMeters; + this.angularVelocityRadPerSec = angularVelocityRadPerSec; + this.time = time; + this.timer = new Timer(); + addRequirements(drivebase); + } + + @Override + public void initialize() { + timer.restart(); + } + + @Override + public void execute() { + double tangentialSpeed = radiusMeters * angularVelocityRadPerSec; + double yVelocity = -tangentialSpeed; + drivebase.drive(new Translation2d(0, yVelocity), angularVelocityRadPerSec, false); + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(time); + } + + @Override + public void end(boolean interrupted) { + drivebase.drive(new Translation2d(), 0, false); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 614f0a90..49cdb149 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -1,6 +1,11 @@ package frc.robot.constants; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; 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.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import swervelib.telemetry.SwerveDriveTelemetry.TelemetryVerbosity; @@ -116,7 +121,7 @@ public enum Mode { public static final double ANGLER_FIXED_ANGLE = 10; //Fixed encoder position of Angler in Fixed ShootState public static final double ANGLER_LIMIT_SPEED = 0.2; - + // turret (pan angle) PID public static final double TURRET_P = .5; public static final double TURRET_I = 0.000001; @@ -158,6 +163,20 @@ public enum Mode { public static final int ENDGAME_START = 30; public static final double VISION_CONSISTENCY_THRESHOLD = 0.25; //How close 2 vision measurements have to be (needs to be tuned potentially but seemingly from my testing it also might not be needed) + public static final double VISION_STD_THRESHOLD = 0.25; public static final boolean ENABLE_VISION = true; public static final double POSE_BUFFER_STORAGE_TIME = 2; //how many past measurements are stored in the buffer (might increase if we need further back) + public static final double FIELD_LENGTH = 16.5; //TODO: Change Later + public static final double FIELD_WIDTH = 8.1; //TODO: Change Later + // Vision + public static final Transform3d ROBOT_TO_CAMERA = new Transform3d(0,0,0, new Rotation3d(0,0,0)); + public static final double HORIZONTAL_FOV = Units.degreesToRadians(110); // radians; TODO: Change Later + public static final double VERTICAL_FOV = Units.degreesToRadians(90); // radians; TODO: Change Later + public static final double AVERAGE_CAM_LATENCY = 0; // seconds; TODO: change Later + public static final double AVERAGE_CAM_LATENCY_STD_DEV = 0; // seconds; TODO: change Later + public static final double MAX_VISION_DISTANCE_SIMULATION = 3.5; + public static final double AVERAGE_PIR_LATENCY = 20; //ms + public static final double VISION_SMOOTHER = 100.0; + public static final Vector INITIAL_VISION_STD_DEVS = VecBuilder.fill(0,0,0); // TODO: Change later + public static final double VISION_STD_DEV_CONST = 1.0/148.0; // TODO: Change later } diff --git a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java index adb2bfc9..7439c7c2 100644 --- a/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ApriltagSubsystem.java @@ -1,12 +1,26 @@ package frc.robot.subsystems; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; 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.numbers.N3; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.apriltags.*; +import frc.robot.constants.Constants; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.swervedrive.vision.estimation.PoseEstimator; +import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter; +import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; +import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; +import frc.robot.utils.Apriltag; import frc.robot.utils.logging.io.BaseIoImpl; +import frc.robot.utils.math.ObjectUtils; +import org.littletonrobotics.junction.Logger; + +import java.util.Random; +import java.util.function.Supplier; public class ApriltagSubsystem extends SubsystemBase { @@ -15,10 +29,11 @@ public class ApriltagSubsystem extends SubsystemBase { private final PoseEstimator estimator; private final SwerveSubsystem drivebase; - public ApriltagSubsystem(ApriltagIO io, SwerveSubsystem drivebase) { + + public ApriltagSubsystem(ApriltagIO io, SwerveSubsystem drivebase, VisionTruster truster) { this.drivebase = drivebase; this.io = io; - estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this); + estimator = new PoseEstimator(drivebase.getKinematics(), drivebase, 0, this, truster); } public static ApriltagIO createRealIo() { @@ -30,8 +45,8 @@ public static ApriltagIO createMockIo() { return new MockApriltagIo(LOGGING_NAME, new ApriltagInputs()); } - public static ApriltagIO createSimIo() { - return new SimApriltagIO(LOGGING_NAME, new ApriltagInputs(), new SimTCPServer(0)); // port doesnt matter at all + public static ApriltagIO createSimIo(VisionTruster truster, SwerveSubsystem drivebase) { + return new SimApriltagIO(LOGGING_NAME, new ApriltagInputs(), new SimTCPServer(0), truster, drivebase); // port doesnt matter at all } // This is used to inject april tag readings manually and will pretty much only be used for simulation. public void addSimReading(ApriltagReading reading) { @@ -40,7 +55,6 @@ public void addSimReading(ApriltagReading reading) { public ApriltagIO getIO(){ return io; } - @Override public void periodic() { estimator.updateVision(); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 570876f4..cb9c8939 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -9,20 +9,26 @@ import static edu.wpi.first.units.Units.Meter; import edu.wpi.first.math.controller.SimpleMotorFeedforward; 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.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.constants.Constants; +import frc.robot.constants.GameConstants; import frc.robot.utils.logging.io.gyro.ThreadedGyro; +import org.littletonrobotics.junction.Logger; import swervelib.SwerveController; import swervelib.SwerveDrive; import swervelib.SwerveDriveTest; @@ -37,6 +43,8 @@ import java.io.File; import java.util.Arrays; +import java.util.Optional; +import java.util.concurrent.ConcurrentLinkedDeque; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -48,6 +56,10 @@ public class SwerveSubsystem extends SubsystemBase { */ private final SwerveDrive swerveDrive; private Vector variance = VecBuilder.fill(0.1,0.1,0.1); + private SwerveDriveOdometry rawOdometry; + private final ConcurrentLinkedDeque poseError = new ConcurrentLinkedDeque<>(); + private record PoseErrorRecord(double timestamp, double error) { + } /** * Initialize {@link SwerveDrive} with the directory provided. * The SwerveIMU (which can be null) is the instance of the SwerveIMU to use. If non-null, @@ -86,7 +98,12 @@ public SwerveSubsystem(File directory, SwerveIMU swerveIMU) { swerveDrive.setModuleEncoderAutoSynchronize(Constants.SET_MODULE_ENCODER_AUTO_SYNCHRONIZE, Constants.SET_MODULE_ENCODER_AUTO_SYNCHRONIZE_DEADBAND); // Enable if you want to resynchronize your absolute encoders and motor encoders periodically when they are not moving. // swerveDrive.pushOffsetsToEncoders(); // Set the absolute encoder to be used over the internal encoder and push the offsets onto it. Throws warning if not possible - + rawOdometry = new SwerveDriveOdometry( + swerveDrive.kinematics, + swerveDrive.getOdometryHeading(), + swerveDrive.getModulePositions(), + startingPose + ); } /** @@ -108,6 +125,18 @@ public SwerveSubsystem(SwerveDriveConfiguration driveCfg, SwerveControllerConfig public void periodic() { //add vision pose here //addVisionMeasurement(new Pose2d(new Translation2d(16, 2), new Rotation2d())); + rawOdometry.update( + swerveDrive.getOdometryHeading(), + swerveDrive.getModulePositions() + ); + + if (Constants.currentMode == GameConstants.Mode.SIM) { + double currentTime = Logger.getTimestamp() / 1000000.0; + double oneSecondAgo = currentTime - 1.0; + poseError.removeIf(record -> record.timestamp < oneSecondAgo); + poseError.add(new PoseErrorRecord(currentTime, getError())); + Logger.recordOutput("AveragePoseError", getAverageError()); + } } @Override @@ -285,8 +314,14 @@ public SwerveDriveKinematics getKinematics() { * * @param initialHolonomicPose The pose to set the odometry to */ + // might be broken public void resetOdometry(Pose2d initialHolonomicPose) { swerveDrive.resetOdometry(initialHolonomicPose); + SwerveModulePosition[] modules = new SwerveModulePosition[4]; + for (int i=0; i<4; i++) { + modules[i] = new SwerveModulePosition(); + } + rawOdometry.resetPosition(initialHolonomicPose.getRotation(), modules, initialHolonomicPose); } /** @@ -297,9 +332,26 @@ public void resetOdometry(Pose2d initialHolonomicPose) { public Pose2d getPose() { return swerveDrive.getPose(); } + public Pose3d getCameraPose() { + if (Constants.currentMode == GameConstants.Mode.SIM) { + return new Pose3d(getSimulationPose().get()).transformBy(Constants.ROBOT_TO_CAMERA); + } else { + return new Pose3d(getPose()).transformBy(Constants.ROBOT_TO_CAMERA); + } + } + public double getError() { + return getPose().getTranslation().getDistance((getSimulationPose().get().getTranslation())); + } + public double getAverageError(){ + return poseError.stream().mapToDouble(record -> record.error).average().orElse(0); + } + + public Optional getSimulationPose() { + return swerveDrive.getSimulationDriveTrainPose(); + } // Todo: fix to only get odomtry public Pose2d getOdom() { - return swerveDrive.getPose(); + return rawOdometry.getPoseMeters(); } /** @@ -470,7 +522,7 @@ public SwerveDrive getSwerveDrive() { return swerveDrive; } public void setVariance(Vector variance){ - this.variance = variance; + this.variance = variance.times(Constants.VISION_SMOOTHER); } public void addVisionMeasurement(Pose2d pose, double visionTimestamp){ swerveDrive.addVisionMeasurement(pose, visionTimestamp, variance); diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java index 0d04e405..72e45b7d 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/FilterablePoseManager.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.numbers.N3; @@ -12,10 +13,10 @@ import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; -import java.util.ArrayList; -import java.util.LinkedHashMap; -import java.util.List; -import java.util.Map; +import java.util.*; +import java.util.concurrent.ConcurrentLinkedDeque; + +import frc.robot.utils.Apriltag; import org.littletonrobotics.junction.Logger; /** @@ -24,7 +25,13 @@ */ public class FilterablePoseManager extends PoseManager { private final VisionFilter filter; - + private record MeasurementRecord(Apriltag tag, double timestamp, FilterResult result) { } + public record VisionLog(VisionMeasurement measurement, FilterResult result) {} + private final ConcurrentLinkedDeque lastSecondMeasurements = new ConcurrentLinkedDeque<>(); + private final List validMeasurementsPose = new ArrayList<>(); + private final List invalidMeasurementsPose = new ArrayList<>();; + private final List acceptedTagsPose = new ArrayList<>(); + private final List log = new ArrayList<>(); public FilterablePoseManager( PoseDeviation PoseDeviation, SwerveDriveKinematics kinematics, @@ -53,31 +60,49 @@ public FilterablePoseManager( @Override public void processQueue() { - LinkedHashMap filteredData = - filter.filter(visionMeasurementQueue); - visionMeasurementQueue.clear(); - List validMeasurements = new ArrayList<>(); - List invalidMeasurements = new ArrayList<>(); - for (Map.Entry entry : filteredData.entrySet()) { - VisionMeasurement v = entry.getKey(); - FilterResult r = entry.getValue(); - switch (r) { - case ACCEPTED -> { - setVisionSTD(visionTruster.calculateTrust(v)); - validMeasurements.add(v.measurement()); - addVisionMeasurement(v); - } - case NOT_PROCESSED -> visionMeasurementQueue.add(v); - case REJECTED -> { - invalidMeasurements.add(v.measurement()); + double currentTime = Logger.getTimestamp()/1000000.0; + double oneSecondAgo = currentTime - 1.0; + lastSecondMeasurements.removeIf(record -> record.timestamp < oneSecondAgo); + for (Map.Entry> queueEntry : visionMeasurementQueueMap.entrySet()) { + Queue visionMeasurementQueue = queueEntry.getValue(); + LinkedHashMap filteredData = + filter.filter(visionMeasurementQueue,drivebase.getCameraPose()); + visionMeasurementQueue.clear(); + for (Map.Entry filterEntry : filteredData.entrySet()) { + VisionMeasurement v = filterEntry.getKey(); + Apriltag tag = v.tag().tag(); + FilterResult r = filterEntry.getValue(); + log.add(new VisionLog(v, r)); + lastSecondMeasurements.add(new MeasurementRecord(tag, v.timeOfMeasurement(), r)); + switch (r) { + case ACCEPTED -> { + setVisionSTD(visionTruster.calculateTrust(v,drivebase.getCameraPose())); + validMeasurementsPose.add(v.measurement()); + addVisionMeasurement(v); + acceptedTagsPose.add(tag.getPose()); + + } + case NOT_PROCESSED -> visionMeasurementQueue.add(v); + case REJECTED -> { + invalidMeasurementsPose.add(v.measurement()); + } } } } - Logger.recordOutput("Apriltag/acceptedMeasurements", validMeasurements.toArray(Pose2d[]::new)); - Logger.recordOutput( - "Apriltag/rejectedMeasurements", invalidMeasurements.toArray(Pose2d[]::new)); } - + public void log() { + Logger.recordOutput("Apriltag/acceptedMeasurementsPose", validMeasurementsPose.toArray(Pose2d[]::new)); + Logger.recordOutput("Apriltag/rejectedMeasurementsPose", invalidMeasurementsPose.toArray(Pose2d[]::new)); + Logger.recordOutput("Apriltag/numberAcceptedLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.ACCEPTED).count()); + Logger.recordOutput("Apriltag/numberNotProcessedLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.NOT_PROCESSED).count()); + Logger.recordOutput("Apriltag/numberRejectedLastSecond", lastSecondMeasurements.stream().filter(record -> record.result == FilterResult.REJECTED).count()); + Logger.recordOutput("Apriltag/acceptedTagPose", acceptedTagsPose.toArray(Pose3d[]::new)); + Logger.recordOutput("Apriltag/Log", log.toArray(VisionLog[]::new)); + validMeasurementsPose.clear(); + invalidMeasurementsPose.clear(); + acceptedTagsPose.clear(); + log.clear(); + } public VisionTruster getVisionTruster() { return visionTruster; } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java index e5ef49ae..b60b0348 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseEstimator.java @@ -16,9 +16,7 @@ import frc.robot.constants.Constants; import frc.robot.subsystems.ApriltagSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; -import frc.robot.subsystems.swervedrive.vision.truster.BasicVisionFilter; -import frc.robot.subsystems.swervedrive.vision.truster.ConstantVisionTruster; -import frc.robot.subsystems.swervedrive.vision.truster.VisionMeasurement; +import frc.robot.subsystems.swervedrive.vision.truster.*; import frc.robot.utils.Apriltag; import frc.robot.utils.math.ArrayUtils; @@ -50,7 +48,7 @@ public class PoseEstimator { private static final double visionStdRateOfChange = 1; /* standard deviation of vision readings, the lower the numbers arm, the more we trust vision */ - public static final Vector visionMeasurementStdDevs2 = VecBuilder.fill(0.3, 0.3, 100); + public static final Vector visionMeasurementStdDevs2 = VecBuilder.fill(0, 0, 100); private final FilterablePoseManager poseManager; public PoseEstimator( @@ -61,7 +59,8 @@ public PoseEstimator( SwerveDriveKinematics kinematics, SwerveSubsystem drivebase, double initGyroValueDeg, - ApriltagSubsystem apriltagSystem) { + ApriltagSubsystem apriltagSystem, + VisionTruster truster) { /*this.frontLeft = frontLeftMotor; this.frontRight = frontRightMotor; this.backLeft = backLeftMotor; @@ -80,17 +79,16 @@ public PoseEstimator( TimeInterpolatableBuffer.createBuffer(Constants.POSE_BUFFER_STORAGE_TIME); this.poseManager = new FilterablePoseManager( - visionMeasurementStdDevs2, + Constants.INITIAL_VISION_STD_DEVS, kinematics, drivebase, m1Buffer, - new BasicVisionFilter(m1Buffer) { + new BasicVisionFilter(m1Buffer,truster) { @Override public Pose2d getVisionPose(VisionMeasurement measurement) { return measurement.measurement(); } - }, - new ConstantVisionTruster(visionMeasurementStdDevs2)); + }, truster); SmartDashboard.putData(field); } @@ -128,7 +126,7 @@ private void updateVision(int... invalidApriltagNumbers) { && !ArrayUtils.contains( invalidApriltagNumbers, apriltagSystem.getIO().getInputs().apriltagNumber[i])) { VisionMeasurement measurement = getVisionMeasurement(pos, i); - poseManager.registerVisionMeasurement(measurement); + poseManager.registerVisionMeasurement(measurement,measurement.tag().tag().number()); } else { invalidCounter++; Logger.recordOutput("Apriltag/ValidationFailureCount", invalidCounter); @@ -137,16 +135,17 @@ private void updateVision(int... invalidApriltagNumbers) { } long end = System.currentTimeMillis(); Logger.recordOutput("RegisteringVisionTimeMillis", end - start); - poseManager.processQueue(); + poseManager.processQueue(); + poseManager.log(); } private VisionMeasurement getVisionMeasurement(double[] pos, int index) { double serverTime = apriltagSystem.getIO().getInputs().serverTime[index]; //double timestamp = 0; // latency is not right we are assuming zero double timestamp = apriltagSystem.getIO().getInputs().timestamp[index]; - Pose2d visionPose = new Pose2d(pos[0], pos[1], getEstimatedPose().getRotation()); + Pose2d visionPose = new Pose2d(pos[0], pos[1], Rotation2d.fromDegrees(pos[2])); double distanceFromTag = apriltagSystem.getIO().getInputs().distanceToTag[index]; - return new VisionMeasurement(visionPose, distanceFromTag, timestamp/1000); + return new VisionMeasurement(visionPose, Apriltag.of(apriltagSystem.getIO().getInputs().apriltagNumber[index]).getTagInfo(),distanceFromTag, timestamp/1000); } /** @@ -154,7 +153,7 @@ private VisionMeasurement getVisionMeasurement(double[] pos, int index) { * are sent to the {@link PoseManager} for further processing */ public void updateVision() { - updateVision(15, 4, 14, 5, 16, 3); + updateVision(0); } public void updateVision(Apriltag focusedTag) { @@ -200,6 +199,9 @@ public FilterablePoseManager getPoseManager() { public void addMockVisionMeasurement() { poseManager.registerVisionMeasurement( - new VisionMeasurement(getEstimatedPose(), 0, Logger.getTimestamp() / 1e6)); + new VisionMeasurement(getEstimatedPose(), Apriltag.of(1).getTagInfo(), 0, Logger.getTimestamp() / 1e6),1); } + public VisionTruster getVisionTruster() { + return poseManager.getVisionTruster(); + } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java index 3f979f9f..7046e721 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/estimation/PoseManager.java @@ -12,6 +12,7 @@ import frc.robot.subsystems.swervedrive.vision.truster.VisionTruster; import org.littletonrobotics.junction.Logger; +import java.util.LinkedHashMap; import java.util.LinkedList; import java.util.Optional; import java.util.Queue; @@ -23,8 +24,8 @@ public class PoseManager { private final TimeInterpolatableBuffer estimatedPoseBuffer; //private final SwerveDrivePoseEstimator poseEstimator; - protected final Queue visionMeasurementQueue = new LinkedList<>(); - private final SwerveSubsystem drivebase; + protected final LinkedHashMap> visionMeasurementQueueMap = new LinkedHashMap<>(); + protected final SwerveSubsystem drivebase; protected final VisionTruster visionTruster; public PoseManager( @@ -62,23 +63,25 @@ public void addOdomMeasurement(Pose2d pose, long timestamp) { estimatedPoseBuffer.addSample(timestamp, pose); } - public void registerVisionMeasurement(VisionMeasurement measurement) { + public void registerVisionMeasurement(VisionMeasurement measurement, int tagId) { if (measurement == null) { return; } - while (visionMeasurementQueue.size() >= 3) { - visionMeasurementQueue.poll(); + while (visionMeasurementQueueMap.computeIfAbsent(tagId,k -> new LinkedList<>()).size() >= 3) { + visionMeasurementQueueMap.get(tagId).poll(); } - visionMeasurementQueue.add(measurement); + visionMeasurementQueueMap.get(tagId).add(measurement); } // override for filtering public void processQueue() { - VisionMeasurement m = visionMeasurementQueue.poll(); - while (m != null) { - setVisionSTD(visionTruster.calculateTrust(m)); - addVisionMeasurement(m); - m = visionMeasurementQueue.poll(); + for (Queue visionMeasurementQueue : visionMeasurementQueueMap.values()) { + VisionMeasurement m = visionMeasurementQueue.poll(); + while (m != null) { + setVisionSTD(visionTruster.calculateTrust(m,drivebase.getCameraPose())); + addVisionMeasurement(m); + m = visionMeasurementQueue.poll(); + } } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java index 5d160e28..8ad8c483 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/BasicVisionFilter.java @@ -1,7 +1,10 @@ package frc.robot.subsystems.swervedrive.vision.truster; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +import edu.wpi.first.math.numbers.N3; import frc.robot.constants.Constants; import java.util.LinkedHashMap; import java.util.Optional; @@ -17,14 +20,16 @@ public abstract class BasicVisionFilter implements VisionFilter, VisionTransformer { private final TimeInterpolatableBuffer poseBuffer; + private final VisionTruster truster; - public BasicVisionFilter(TimeInterpolatableBuffer poseBuffer) { + public BasicVisionFilter(TimeInterpolatableBuffer poseBuffer, VisionTruster truster) { this.poseBuffer = poseBuffer; + this.truster = truster; } @Override public LinkedHashMap filter( - Queue measurements) { + Queue measurements, Pose3d cameraPose) { LinkedHashMap resultMap = new LinkedHashMap<>(); VisionMeasurement m1 = measurements.poll(); VisionMeasurement m2 = measurements.peek(); @@ -54,7 +59,7 @@ public LinkedHashMap filter( Pose2d vision1Pose = getVisionPose(m1); Pose2d vision2Pose = getVisionPose(m2); boolean valid1 = - filterVision(vision1Pose, vision2Pose, m1.timeOfMeasurement(), m2.timeOfMeasurement()); + filterVision(m1, m2, cameraPose); resultMap.put(m1, valid1 ? FilterResult.ACCEPTED : FilterResult.REJECTED); m1 = measurements.poll(); m2 = measurements.peek(); @@ -63,23 +68,27 @@ public LinkedHashMap filter( return resultMap; } - private boolean filterVision(Pose2d m1Pose, Pose2d m2Pose, double m1Time, double m2Time) { - Optional odomPoseAtVis1 = poseBuffer.getSample(m1Time); - Optional odomPoseAtVis2 = poseBuffer.getSample(m2Time); + private boolean filterVision(VisionMeasurement m1, VisionMeasurement m2, Pose3d cameraPose) { + Optional odomPoseAtVis1 = poseBuffer.getSample(m1.timeOfMeasurement()); + Optional odomPoseAtVis2 = poseBuffer.getSample(m2.timeOfMeasurement()); if (odomPoseAtVis1.isEmpty() || odomPoseAtVis2.isEmpty()) { return false; } - if (!inBounds(m1Pose) || !inBounds(m2Pose)) { + if (!inBounds(m1.measurement()) || !inBounds(m2.measurement())) { return false; } double odomDiff1To2 = odomPoseAtVis1.get().getTranslation().getDistance(odomPoseAtVis2.get().getTranslation()); - double visionDiff1To2 = m1Pose.getTranslation().getDistance(m2Pose.getTranslation()); + double visionDiff1To2 = m1.measurement().getTranslation().getDistance(m2.measurement().getTranslation()); double diff = Math.abs(odomDiff1To2 - visionDiff1To2); + Vector std = truster.calculateTrust(m1, cameraPose); + if (std.get(0) > Constants.VISION_STD_THRESHOLD) { + return false; + } return Math.abs(diff) <= Constants.VISION_CONSISTENCY_THRESHOLD; } - private boolean inBounds(Pose2d pose2d) { - return pose2d.getX() > 0 && pose2d.getX() < 20 && pose2d.getY() > 0 && pose2d.getY() < 20; + public static boolean inBounds(Pose2d pose2d) { + return pose2d.getX() > 0 && pose2d.getX() < Constants.FIELD_LENGTH && pose2d.getY() > 0 && pose2d.getY() < Constants.FIELD_WIDTH; } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java index 3017ccb1..fe133bbe 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/ConstantVisionTruster.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.swervedrive.vision.truster; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N3; @@ -11,7 +12,7 @@ public ConstantVisionTruster(Vector initialSTD) { } @Override - public Vector calculateTrust(VisionMeasurement measurement) { + public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { return initialSTD; } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java index e7f75ea6..78e6f094 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/LinearVisionTruster.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N3; public class LinearVisionTruster extends DistanceVisionTruster { @@ -14,7 +15,7 @@ public LinearVisionTruster(Vector initialSTD, double slopeSTD) { } @Override - public Vector calculateTrust(VisionMeasurement measurement) { + public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { double std = measurement.distanceFromTag() * slopeSTD; return initialSTD.plus(VecBuilder.fill(std, std, std)); } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java index 7a925fc8..6b1d76d2 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/SquareVisionTruster.java @@ -2,6 +2,10 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N3; public class SquareVisionTruster extends DistanceVisionTruster { @@ -15,8 +19,10 @@ public SquareVisionTruster(Vector initialSTD, double constant) { } @Override - public Vector calculateTrust(VisionMeasurement measurement) { - double std = Math.pow(measurement.distanceFromTag() - 0.4572, 2) * constant; - return initialSTD.plus(VecBuilder.fill(std, std, std)); + public Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose) { + Translation3d adjPose2 = cameraPose.relativeTo(measurement.tag().tag().getPose()).getTranslation(); + double distanceTimesCosIncidenceAngle = adjPose2.getX()/adjPose2.getNorm(); + double std = Math.pow(measurement.distanceFromTag(), 2) * constant / distanceTimesCosIncidenceAngle; + return initialSTD.plus(VecBuilder.fill(std, std, std*10000)); } } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionFilter.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionFilter.java index e1b94831..6f156bad 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionFilter.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionFilter.java @@ -1,8 +1,10 @@ package frc.robot.subsystems.swervedrive.vision.truster; +import edu.wpi.first.math.geometry.Pose3d; + import java.util.LinkedHashMap; import java.util.Queue; public interface VisionFilter { - LinkedHashMap filter(Queue measurements); + LinkedHashMap filter(Queue measurements, Pose3d cameraPose); } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java index edd9af38..82b54ead 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionMeasurement.java @@ -1,6 +1,7 @@ package frc.robot.subsystems.swervedrive.vision.truster; import edu.wpi.first.math.geometry.Pose2d; +import frc.robot.utils.Apriltag; /** * @param measurement estimated robot position (meters) calculated from apriltag tag what tag produced the @@ -9,4 +10,4 @@ * @param timeOfMeasurement time when the pose was measured (seconds) */ public record VisionMeasurement( - Pose2d measurement, double distanceFromTag, double timeOfMeasurement) {} + Pose2d measurement, Apriltag.TagPose tag, double distanceFromTag, double timeOfMeasurement) {} diff --git a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java index 7ce7c3d4..36279178 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/vision/truster/VisionTruster.java @@ -1,8 +1,9 @@ package frc.robot.subsystems.swervedrive.vision.truster; import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N3; public interface VisionTruster { - Vector calculateTrust(VisionMeasurement measurement); + Vector calculateTrust(VisionMeasurement measurement, Pose3d cameraPose); } diff --git a/src/main/java/frc/robot/utils/Apriltag.java b/src/main/java/frc/robot/utils/Apriltag.java index 66449f22..8b134452 100644 --- a/src/main/java/frc/robot/utils/Apriltag.java +++ b/src/main/java/frc/robot/utils/Apriltag.java @@ -1,40 +1,49 @@ package frc.robot.utils; -import edu.wpi.first.math.geometry.Translation3d; - -public enum Apriltag { //Andymark field: - ONE(467.08,291.79, 35.00), //Trench, Red z rotation:180 - TWO(468.56,182.08, 44.25), //Hub, Red z rotation:90 - THREE( 444.80,172.32, 44.25), //Hub, Red z rotation:180 - FOUR(444.80,158.32, 44.25), //Hub, Red z rotation:180 - FIVE(468.56,134.56, 44.25), //Hub, Red z rotation:270 - SIX(467.08,24.85, 35.00), //Trench, Red z rotation:180 - SEVEN(470.03,24.85, 35.00), //Trench, Red z rotation:0 - EIGHT(482.56,134.56, 44.25), //Hub, Red z rotation:270 - NINE(492.33,144.32, 44.25), //Hub, Red z rotation:0 - TEN(492.33,158.32, 44.25), //Hub, Red z rotation:0 - ELEVEN(482.56,182.08, 44.25), //Hub, Red z rotation:90 - TWELVE(470.03,291.79, 35.00), //Trench, Red z rotation:0 - THIRTEEN(649.58,291.02, 21.75), //Outpost, Red z rotation:180 - FOURTEEN(649.58,274.02, 21.75), //Outpost, Red z rotation:180 - FIFTEEN(649.57,169.78, 21.75), //Tower, Red z rotation:180 - SIXTEEN(649.57,152.78, 21.75), //Tower, Red z rotation:180 - SEVENTEEN(183.03,24.85, 35.00), //Trench, Blue z rotation:0 - EIGHTEEN(181.56,134.56, 44.25), //Hub, Blue z rotation:270 - NINETEEN(205.32,144.32, 44.25), //Hub, Blue z rotation:0 - TWENTY(205.32,158.32, 44.52), //Hub, Blue z rotation:0 - TWENTY_ONE(181.56,182.08, 44.25), //Hub, Blue z rotation:90 - TWENTY_TWO(183.03, 291.79, 35.00), //Trench, Blue z rotation:0 - TWENTY_THREE(180.08, 291.79, 35.00),//Trench, Blue z rotation:180 - TWENTY_FOUR(167.56, 182.08, 44.25),//Hub, Blue z rotation:90 - TWENTY_FIVE(157.79, 172.32, 44.25),//Hub, Blue z rotation:180 - TWENTY_SIX(157.79, 158.32, 44.25),//Hub, Blue z rotation:180 - TWENTY_SEVEN(167.58, 134.56, 44.25),//Hub, Blue z rotation:270 - TWENTY_EIGHT(180.08, 24.85, 35.00),//Trench, Blue z rotation:180 - TWENTY_NINE(0.54, 25.62, 21.75),//Outpost, Blue z rotation:0 - THIRTY(0.54, 42.62, 21.75),//Outpost, Blue z rotation:0 - THIRTY_ONE(0.55, 146.86, 21.75),//Tower, Blue z rotation:0 - THIRTY_TWO(0.55, 163.86, 21.75);//Tower, Blue z rotation:0 +import edu.wpi.first.math.geometry.*; + +import java.lang.Math.*; + +import edu.wpi.first.math.util.Units; + +import static edu.wpi.first.units.Units.Radians; +import static java.lang.Math.PI; +import static java.lang.Math.abs; + +public enum Apriltag { + //Andymark field: + ONE(467.08,291.79, 35.00, PI), //Trench, Red z rotation:180 + TWO(468.56,182.08, 44.25, PI/2), //Hub, Red z rotation:90 + THREE( 444.80,172.32, 44.25, PI), //Hub, Red z rotation:180 + FOUR(444.80,158.32, 44.25, PI), //Hub, Red z rotation:180 + FIVE(468.56,134.56, 44.25, 3*PI/2), //Hub, Red z rotation:270 + SIX(467.08,24.85, 35.00,PI), //Trench, Red z rotation:180 + SEVEN(470.03,24.85, 35.00, 0), //Trench, Red z rotation:0 + EIGHT(482.56,134.56, 44.25, 3*PI/2), //Hub, Red z rotation:270 + NINE(492.33,144.32, 44.25, 0), //Hub, Red z rotation:0 + TEN(492.33,158.32, 44.25, 0), //Hub, Red z rotation:0 + ELEVEN(482.56,182.08, 44.25, PI/2), //Hub, Red z rotation:90 + TWELVE(470.03,291.79, 35.00, 0), //Trench, Red z rotation:0 + THIRTEEN(649.58,291.02, 21.75, PI), //Outpost, Red z rotation:180 + FOURTEEN(649.58,274.02, 21.75, PI), //Outpost, Red z rotation:180 + FIFTEEN(649.57,169.78, 21.75, PI), //Tower, Red z rotation:180 + SIXTEEN(649.57,152.78, 21.75, PI), //Tower, Red z rotation:180 + SEVENTEEN(183.03,24.85, 35.00, 0), //Trench, Blue z rotation:0 + EIGHTEEN(181.56,134.56, 44.25, 3*PI/2), //Hub, Blue z rotation:270 + NINETEEN(205.32,144.32, 44.25, 0), //Hub, Blue z rotation:0 + TWENTY(205.32,158.32, 44.52, 0), //Hub, Blue z rotation:0 + TWENTY_ONE(181.56,182.08, 44.25, PI/2), //Hub, Blue z rotation:90 + TWENTY_TWO(183.03, 291.79, 35.00, 0), //Trench, Blue z rotation:0 + TWENTY_THREE(180.08, 291.79, 35.00, PI),//Trench, Blue z rotation:180 + TWENTY_FOUR(167.56, 182.08, 44.25, PI/2),//Hub, Blue z rotation:90 + TWENTY_FIVE(157.79, 172.32, 44.25, PI),//Hub, Blue z rotation:180 + TWENTY_SIX(157.79, 158.32, 44.25, PI),//Hub, Blue z rotation:180 + TWENTY_SEVEN(167.58, 134.56, 44.25, 3*PI/2),//Hub, Blue z rotation:270 + TWENTY_EIGHT(180.08, 24.85, 35.00, PI),//Trench, Blue z rotation:180 + TWENTY_NINE(0.54, 25.62, 21.75, 0),//Outpost, Blue z rotation:0 + THIRTY(0.54, 42.62, 21.75, 0),//Outpost, Blue z rotation:0 + THIRTY_ONE(0.55, 146.86, 21.75, 0),//Tower, Blue z rotation:0 + THIRTY_TWO(0.55, 163.86, 21.75, 0);//Tower, Blue z rotation:0 /* Welded Field: @@ -73,15 +82,21 @@ public enum Apriltag { THIRTY_ONE(0.32, 147.47, 21.75),//Tower, Blue z rotation:0 THIRTY_TWO(0.32, 164.47, 21.75);//Tower, Blue z rotation:0 */ + public record TagPose(Apriltag tag, Pose3d pose) {} + private final double xMeters; + private final double yMeters; + private final double zMeters; + private final Pose3d pose; + private final Translation3d translation; + private final double rotation; - private final double x; - private final double y; - private final double z; - - Apriltag(double x, double y, double z) { - this.x = x; - this.y = y; - this.z = z; + Apriltag(double xInches, double yInches, double zInches, double rotation) { + this.xMeters = Units.inchesToMeters(xInches); + this.yMeters = Units.inchesToMeters(yInches); + this.zMeters = Units.inchesToMeters(zInches); + this.translation = new Translation3d(xMeters,yMeters,zMeters); + this.rotation = rotation; + this.pose = new Pose3d(translation,new Rotation3d(0,0,rotation)); } public static Apriltag of(int number) { @@ -92,22 +107,33 @@ public static Apriltag of(int number) { } public double getX() { - return x; + return xMeters; } public double getY() { - return y; + return yMeters; } public double getZ() { - return z; + return zMeters; + } + + public double getRotation() { + return rotation; } public Translation3d getTranslation() { - return new Translation3d(x, y, z); + return translation; + } + + public Pose3d getPose() { + return pose; } public int number() { - return ordinal(); + return ordinal()+1; + } + public TagPose getTagInfo() { + return new TagPose(this, pose); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/math/ObjectUtils.java b/src/main/java/frc/robot/utils/math/ObjectUtils.java new file mode 100644 index 00000000..d203a7d8 --- /dev/null +++ b/src/main/java/frc/robot/utils/math/ObjectUtils.java @@ -0,0 +1,28 @@ +package frc.robot.utils.math; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N3; + +import static java.lang.Math.PI; +import static java.lang.Math.abs; + +public class ObjectUtils { + // TODO: Implement obstruction + public static boolean canSee(Pose3d objectPose, Pose3d cameraPose, double HorizontalFOV, double VerticalFOV) { + Pose3d adjPose = objectPose.relativeTo(cameraPose); + double horizontalAngle = Math.atan2(adjPose.getY(), adjPose.getX()); + double verticalAngle = Math.asin(adjPose.getZ()/adjPose.getTranslation().getNorm()); + Translation3d adjPose2 = cameraPose.relativeTo(objectPose).getTranslation(); + double distanceToCamera = adjPose2.getNorm(); + double cosIncidence = adjPose2.getX() / distanceToCamera; + if (cosIncidence<=0) { + return false; + } + return abs(horizontalAngle) < HorizontalFOV/2 && abs(verticalAngle) < VerticalFOV/2;// TODO: Maybe change Later By Implementing Math Calculations (Linear Algebra) + } +}