diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/AbsolutePose.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/AbsolutePose.kt index 25c320d..a63594c 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/AbsolutePose.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/AbsolutePose.kt @@ -8,10 +8,7 @@ import com.frcteam3636.frc2025.utils.QuestNav import edu.wpi.first.math.Matrix import edu.wpi.first.math.VecBuilder import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator -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.Transform2d +import edu.wpi.first.math.geometry.* import edu.wpi.first.math.numbers.N1 import edu.wpi.first.math.numbers.N3 import edu.wpi.first.units.Units.* @@ -24,6 +21,10 @@ import edu.wpi.first.wpilibj.Alert.AlertType import edu.wpi.first.wpilibj.Timer import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs +import org.photonvision.PhotonCamera +import org.photonvision.PhotonPoseEstimator +import org.photonvision.simulation.PhotonCameraSim +import org.photonvision.simulation.SimCameraProperties import org.team9432.annotation.Logged import java.nio.ByteBuffer @@ -38,16 +39,20 @@ class AbsolutePoseProviderInputs : LoggableInputs { */ var connected = false + var observedTags: IntArray = intArrayOf() + override fun toLog(table: LogTable) { if (measurement != null) { table.put("Measurement", measurement) } table.put("Connected", connected) + table.put("ObservedTags", observedTags) } override fun fromLog(table: LogTable) { measurement = table.get("Measurement", measurement)[0] connected = table.get("Connected", connected) + observedTags = table.get("ObservedTags", observedTags) } } @@ -161,6 +166,44 @@ class LimelightPoseProvider( } } +@Suppress("unused") +class CameraSimPoseProvider(name: String, val chassisToCamera: Transform3d) : AbsolutePoseProvider { + private val camera = PhotonCamera(name) + private val simProperties = SimCameraProperties().apply { + setCalibration(1280, 800, Rotation2d(LIMELIGHT_FOV)) + fps = 20.0 + avgLatencyMs = 51.0 + latencyStdDevMs = 5.0 + } + val sim = PhotonCameraSim(camera, simProperties) + + private val estimator = + PhotonPoseEstimator( + APRIL_TAGS, + PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + chassisToCamera + ) + + override fun updateInputs(inputs: AbsolutePoseProviderInputs) { + inputs.connected = true + inputs.measurement = null + val unreadResults = camera.allUnreadResults + val latestResult = unreadResults.lastOrNull() + if (latestResult != null) { + estimator.update(latestResult).ifPresent { + inputs.measurement = AbsolutePoseMeasurement( + it.estimatedPose.toPose2d(), + Seconds.of(it.timestampSeconds), + VecBuilder.fill(0.7, 0.7, 9999999.0) + ) + } + inputs.observedTags = latestResult.targets.map { + it.fiducialId + }.toIntArray() + } + } +} + @Logged open class QuestNavInputs { /** @@ -197,32 +240,6 @@ class QuestNavLocalizer( } } -//@Suppress("unused") -//class PhotonVisionPoseIOReal(name: String, chassisToCamera: Transform3d) : AbsolutePoseIO { -// private val camera = PhotonCamera(name).apply { driverMode = false } -// private val estimator = -// PhotonPoseEstimator( -// APRIL_TAG_FIELD_LAYOUT, -// PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, -// camera, -// chassisToCamera -// ) -// -// override fun updateInputs(inputs: AbsolutePoseIO.Inputs) { -// inputs.measurement = null -// estimator.update().ifPresent { -// inputs.measurement = AbsolutePoseMeasurement( -// it.estimatedPose, -// it.timestampSeconds, -// APRIL_TAG_STD_DEV(it.estimatedPose.translation.norm, it.targetsUsed.size) -// ) -// } -// } -// -// override val cameraConnected -// get() = camera.isConnected -//} - data class AbsolutePoseMeasurement( val pose: Pose2d, val timestamp: Time, @@ -272,7 +289,6 @@ class AbsolutePoseMeasurementStruct : Struct { } } -//internal val APRIL_TAG_FIELD_LAYOUT = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile) //internal const val APRIL_TAG_AMBIGUITY_FILTER = 0.3 //internal val APRIL_TAG_STD_DEV = { distance: Double, count: Int -> @@ -283,3 +299,5 @@ class AbsolutePoseMeasurementStruct : Struct { // translationalStdDev, translationalStdDev, rotationalStdDev // ) //} + +val LIMELIGHT_FOV = Degrees.of(75.76079874010732) diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Drivetrain.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Drivetrain.kt index f20608c..0027ea4 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Drivetrain.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/Drivetrain.kt @@ -24,10 +24,7 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController import com.pathplanner.lib.pathfinding.Pathfinding import edu.wpi.first.math.VecBuilder import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator -import edu.wpi.first.math.geometry.Pose2d -import edu.wpi.first.math.geometry.Rotation2d -import edu.wpi.first.math.geometry.Transform2d -import edu.wpi.first.math.geometry.Translation2d +import edu.wpi.first.math.geometry.* import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.math.kinematics.SwerveDriveKinematics import edu.wpi.first.math.kinematics.SwerveModuleState @@ -62,12 +59,18 @@ object Drivetrain : Subsystem, Sendable { private val questNavInputs = LoggedQuestNavInputs() private var questNavCalibrated = false - private val absolutePoseIOs = mapOf( - "Limelight" to LimelightPoseProvider( - "limelight", - algorithm = LimelightAlgorithm.MegaTag2 { inputs } - ), - ).mapValues { Pair(it.value, AbsolutePoseProviderInputs()) } + private val absolutePoseIOs = when (Robot.model) { + Robot.Model.SIMULATION -> mapOf( + "Limelight" to CameraSimPoseProvider("limelight", Transform3d()), + ) + + else -> mapOf( + "Limelight" to LimelightPoseProvider( + "limelight", + algorithm = LimelightAlgorithm.MegaTag2 { inputs }, + ), + ) + }.mapValues { Pair(it.value, AbsolutePoseProviderInputs()) } /** Helper for converting a desired drivetrain velocity into the speeds and angles for each swerve module */ private val kinematics = @@ -113,7 +116,7 @@ object Drivetrain : Subsystem, Sendable { this::measuredChassisSpeeds, this::desiredChassisSpeeds::set, PPHolonomicDriveController( - when(Robot.model) { + when (Robot.model) { Robot.Model.SIMULATION -> DRIVING_PID_GAINS_TALON Robot.Model.COMPETITION -> DRIVING_PID_GAINS_TALON Robot.Model.PROTOTYPE -> DRIVING_PID_GAINS_NEO @@ -126,10 +129,10 @@ object Drivetrain : Subsystem, Sendable { this ) - if (io is DrivetrainIOSim){ + if (io is DrivetrainIOSim) { poseEstimator.resetPose(io.swerveDriveSimulation.simulatedDriveTrainPose) + io.registerPoseProviders(absolutePoseIOs.values.map { it.first }) } - } override fun periodic() { @@ -168,6 +171,14 @@ object Drivetrain : Subsystem, Sendable { Logger.recordOutput("Drivetrain/Chassis Speeds", measuredChassisSpeeds) Logger.recordOutput("Drivetrain/Localizer", localizer.name) questNavInactiveAlert.set(localizer != Localizer.QuestNav) + + Logger.recordOutput( + "Drivetrain/TagPoses", *APRIL_TAGS.tags + .filter { tag -> + absolutePoseIOs.values.any { it.second.observedTags.contains(tag.ID) } + } + .map { it.pose } + .toTypedArray()) } /** The desired speeds and angles of the swerve modules. */ @@ -351,25 +362,25 @@ object Drivetrain : Subsystem, Sendable { val MODULE_POSITIONS = PerCorner( frontLeft = - Pose2d( - Translation2d(WHEEL_BASE, TRACK_WIDTH) / 2.0, - Rotation2d.fromDegrees(0.0) - ), + Pose2d( + Translation2d(WHEEL_BASE, TRACK_WIDTH) / 2.0, + Rotation2d.fromDegrees(0.0) + ), frontRight = - Pose2d( - Translation2d(WHEEL_BASE, -TRACK_WIDTH) / 2.0, - Rotation2d.fromDegrees(270.0) - ), + Pose2d( + Translation2d(WHEEL_BASE, -TRACK_WIDTH) / 2.0, + Rotation2d.fromDegrees(270.0) + ), backLeft = - Pose2d( - Translation2d(-WHEEL_BASE, TRACK_WIDTH) / 2.0, - Rotation2d.fromDegrees(90.0) - ), + Pose2d( + Translation2d(-WHEEL_BASE, TRACK_WIDTH) / 2.0, + Rotation2d.fromDegrees(90.0) + ), backRight = - Pose2d( - Translation2d(-WHEEL_BASE, -TRACK_WIDTH) / 2.0, - Rotation2d.fromDegrees(180.0) - ), + Pose2d( + Translation2d(-WHEEL_BASE, -TRACK_WIDTH) / 2.0, + Rotation2d.fromDegrees(180.0) + ), ) // Chassis Control @@ -378,7 +389,7 @@ object Drivetrain : Subsystem, Sendable { val ROTATION_PID_GAINS = PIDGains(3.0, 0.0, 0.4) -// // Pathing + // // Pathing // val DEFAULT_PATHING_CONSTRAINTS = // PathConstraints(FREE_SPEED.baseUnitMagnitude(), 3.879, ROTATION_SPEED.baseUnitMagnitude(), 24.961) // FIXME: Update for 2025 @@ -414,14 +425,13 @@ object Drivetrain : Subsystem, Sendable { }.toTypedArray() ) - val PP_ROBOT_CONFIG = when(Robot.model) { + val PP_ROBOT_CONFIG = when (Robot.model) { Robot.Model.SIMULATION -> PP_ROBOT_CONFIG_COMP Robot.Model.COMPETITION -> PP_ROBOT_CONFIG_COMP Robot.Model.PROTOTYPE -> PP_ROBOT_CONFIG_PROTOTYPE } - // CAN IDs val KRAKEN_MODULE_CAN_IDS = PerCorner( @@ -450,25 +460,25 @@ object Drivetrain : Subsystem, Sendable { internal val MODULE_CAN_IDS_PRACTICE = PerCorner( frontLeft = - Pair( - REVMotorControllerId.FrontLeftDrivingMotor, - REVMotorControllerId.FrontLeftTurningMotor - ), + Pair( + REVMotorControllerId.FrontLeftDrivingMotor, + REVMotorControllerId.FrontLeftTurningMotor + ), frontRight = - Pair( - REVMotorControllerId.FrontRightDrivingMotor, - REVMotorControllerId.FrontRightTurningMotor - ), + Pair( + REVMotorControllerId.FrontRightDrivingMotor, + REVMotorControllerId.FrontRightTurningMotor + ), backLeft = Pair( REVMotorControllerId.BackLeftDrivingMotor, REVMotorControllerId.BackLeftTurningMotor ), backRight = - Pair( - REVMotorControllerId.BackRightDrivingMotor, - REVMotorControllerId.BackRightTurningMotor - ), + Pair( + REVMotorControllerId.BackRightDrivingMotor, + REVMotorControllerId.BackRightTurningMotor + ), ) /** A position with the modules radiating outwards from the center of the robot, preventing movement. */ diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/DrivetrainIO.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/DrivetrainIO.kt index a06a480..5693893 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/DrivetrainIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/drivetrain/DrivetrainIO.kt @@ -11,6 +11,8 @@ import com.frcteam3636.frc2025.subsystems.drivetrain.Drivetrain.Constants.WHEEL_ import com.frcteam3636.frc2025.utils.math.TAU import com.frcteam3636.frc2025.utils.swerve.PerCorner import com.studica.frc.AHRS +import edu.wpi.first.apriltag.AprilTagFieldLayout +import edu.wpi.first.apriltag.AprilTagFields import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.kinematics.SwerveModulePosition @@ -24,6 +26,7 @@ import org.ironmaple.simulation.drivesims.SwerveDriveSimulation import org.ironmaple.simulation.drivesims.configs.DriveTrainSimulationConfig import org.ironmaple.simulation.drivesims.configs.SwerveModuleSimulationConfig import org.littletonrobotics.junction.Logger +import org.photonvision.simulation.VisionSystemSim import org.team9432.annotation.Logged @Logged @@ -138,6 +141,10 @@ class DrivetrainIOSim : DrivetrainIO() { Pose2d(3.0, 3.0, Rotation2d()) ) + val vision = VisionSystemSim("main").apply { + addAprilTags(APRIL_TAGS) + } + override val modules = PerCorner.generate { SimSwerveModule(swerveDriveSimulation.modules[it.ordinal]) } override val gyro = GyroMapleSim(swerveDriveSimulation.gyroSimulation) @@ -147,8 +154,21 @@ class DrivetrainIOSim : DrivetrainIO() { override fun updateInputs(inputs: DrivetrainInputs) { super.updateInputs(inputs) + vision.update(swerveDriveSimulation.simulatedDriveTrainPose) Logger.recordOutput("FieldSimulation/RobotPosition", swerveDriveSimulation.simulatedDriveTrainPose) } + fun registerPoseProviders(providers: Iterable) { + for (provider in providers) { + if (provider is CameraSimPoseProvider) { + vision.addCamera(provider.sim, provider.chassisToCamera) + } + } + } + // Register the drivetrain simulation to the default simulation world } + +val APRIL_TAGS = AprilTagFieldLayout.loadFromResource( + AprilTagFields.k2025Reefscape.m_resourceFile +)!! diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..1163429 --- /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" + } + ] +}