diff --git a/.gitignore b/.gitignore index 5528d4f..7b5d42c 100755 --- a/.gitignore +++ b/.gitignore @@ -176,3 +176,5 @@ logs/ # Folder that has CTRE Phoenix Sim device config storage ctre_sim/ + +kls_database.db diff --git a/src/main/java/frc/robot/Robot.kt b/src/main/java/frc/robot/Robot.kt index e5016d9..e485916 100755 --- a/src/main/java/frc/robot/Robot.kt +++ b/src/main/java/frc/robot/Robot.kt @@ -16,6 +16,8 @@ class Robot : TimedRobot() { private var m_robotContainer: RobotContainer? = null + + init { // if (isReal()) { PowerDistribution(1, PowerDistribution.ModuleType.kRev) // Enables power distribution logging @@ -48,6 +50,7 @@ class Robot : TimedRobot() { override fun robotPeriodic() { CommandScheduler.getInstance().run() + m_robotContainer!!.updatePoseEstimation() } override fun disabledInit() {} @@ -87,4 +90,10 @@ class Robot : TimedRobot() { override fun testExit() {} override fun simulationPeriodic() {} + + companion object { + fun sim(): Boolean { + return isSimulation() + } + } } diff --git a/src/main/java/frc/robot/RobotContainer.kt b/src/main/java/frc/robot/RobotContainer.kt index 3cd5f0d..37733fb 100755 --- a/src/main/java/frc/robot/RobotContainer.kt +++ b/src/main/java/frc/robot/RobotContainer.kt @@ -17,13 +17,12 @@ import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.button.CommandXboxController import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine -import frc.robot.commands.GoToNoteCommand import frc.robot.commands.PutArmDownCommand import frc.robot.generated.TunerConstants import frc.robot.generated.TunerConstants.createDrivetrain import frc.robot.subsystems.ArmSubsystem import frc.robot.subsystems.drive.CommandSwerveDrivetrain -import frc.robot.subsystems.vision.VisionSubsystem +import frc.robot.subsystems.vision.BetterVisionSubsystem class RobotContainer { private val MaxSpeed = @@ -39,9 +38,8 @@ class RobotContainer { val drivetrain: CommandSwerveDrivetrain = createDrivetrain() val armSubsystem: ArmSubsystem = ArmSubsystem() - val visionSubsystem: VisionSubsystem = VisionSubsystem.instance + val visionSubsystem: BetterVisionSubsystem = BetterVisionSubsystem() - val goToNoteCommand = GoToNoteCommand(drivetrain, visionSubsystem) val putArmDownCommand = PutArmDownCommand(armSubsystem) /* Setting up bindings for necessary control of the swerve drive platform */ @@ -115,7 +113,6 @@ class RobotContainer { // reset the field-centric heading on left bumper press joystick.leftBumper().onTrue(drivetrain.runOnce { drivetrain::resetPose }) - joystick.rightBumper().whileTrue(goToNoteCommand.andThen(putArmDownCommand)) drivetrain.registerTelemetry { state: SwerveDriveState? -> if (state != null) { @@ -127,4 +124,26 @@ class RobotContainer { val autonomousCommand: Command get() = /* First put the drivetrain into auto run mode, then run the auto */ autoChooser.selected + + + fun updatePoseEstimation() { + var visionEst = visionSubsystem.leftEstimatedGlobalPose + visionEst.ifPresent { + est -> + run { + var leftEstStdDevs = visionSubsystem.leftEstimationStdDevs + + drivetrain.addVisionMeasurement(est.estimatedPose.toPose2d(), est.timestampSeconds, leftEstStdDevs) + } + } + visionEst = visionSubsystem.rightEstimatedGlobalPose + visionEst.ifPresent { + est -> + run { + var rightEstStdDevs = visionSubsystem.rightEstimationStdDevs + + drivetrain.addVisionMeasurement(est.estimatedPose.toPose2d(), est.timestampSeconds, rightEstStdDevs) + } + } + } } diff --git a/src/main/java/frc/robot/commands/GoToNoteCommand.kt b/src/main/java/frc/robot/commands/GoToNoteCommand.kt deleted file mode 100644 index 5162007..0000000 --- a/src/main/java/frc/robot/commands/GoToNoteCommand.kt +++ /dev/null @@ -1,64 +0,0 @@ -package frc.robot.commands - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType -import com.ctre.phoenix6.swerve.SwerveRequest -import edu.wpi.first.math.controller.PIDController -import edu.wpi.first.units.Units -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard -import edu.wpi.first.wpilibj2.command.Command -import frc.robot.generated.TunerConstants -import frc.robot.subsystems.drive.CommandSwerveDrivetrain -import frc.robot.subsystems.vision.VisionSubsystem -import kotlin.math.PI - -class GoToNoteCommand( - private val commandSwerveDrivetrain: CommandSwerveDrivetrain, - private val visionSubsystem: VisionSubsystem, -) : Command() { - - var drive: SwerveRequest.RobotCentric = - SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.Velocity) - .withDeadband(0.0) // Deadband is handled on input - .withRotationalDeadband(0.0) - - val anglePid = PIDController(0.03, 0.0, 0.001) - val drivePid = PIDController(0.03, 0.0, 0.001) - - init { - // each subsystem used by the command must be passed into the addRequirements() method - addRequirements(commandSwerveDrivetrain, visionSubsystem) - SmartDashboard.putData("Angle PID", anglePid) - SmartDashboard.putData("Drive PID", drivePid) - } - - override fun initialize() { - } - - override fun execute() { - try { - commandSwerveDrivetrain.setControl( - drive.withVelocityX( - -drivePid.calculate( - visionSubsystem.getNoteCamPitch() * TunerConstants.kSpeedAt12VoltsMps.`in`( - Units.MetersPerSecond, - ), - ), - ) - .withVelocityY(0.0).withRotationalDeadband(0.1).withDeadband(1.0) - .withRotationalRate(anglePid.calculate(visionSubsystem.getNoteCamYaw()) * 2 * PI), - ) - } finally {} - } - - override fun isFinished(): Boolean { - // TODO: Make this return true when this Command no longer needs to run execute() - try { - return visionSubsystem.isCloseEnoughToDone() - } finally { - return false - } - } - - override fun end(interrupted: Boolean) { - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.kt b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.kt index 45f3970..e26df92 100755 --- a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.kt +++ b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.kt @@ -20,7 +20,6 @@ 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.kinematics.ChassisSpeeds -import edu.wpi.first.math.kinematics.SwerveDriveOdometry import edu.wpi.first.math.numbers.N1 import edu.wpi.first.math.numbers.N3 import edu.wpi.first.units.Units @@ -29,13 +28,15 @@ import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.DriverStation.Alliance import edu.wpi.first.wpilibj.Notifier import edu.wpi.first.wpilibj.RobotController +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard +import edu.wpi.first.wpilibj.smartdashboard.Field2d +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Subsystem import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism import frc.robot.util.RectanglePoseArea -import frc.robot.subsystems.vision.VisionSubsystem import java.util.function.DoubleSupplier import java.util.function.Supplier import kotlin.math.PI @@ -248,8 +249,19 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem { return m_sysIdRoutineToApply.dynamic(direction) } +// var rightConfidence = SmartDashboard.getEntry("Right Confidence") +// +// var leftConfidence = SmartDashboard.getEntry("Left Confidence") +// +// var rightField = Field2d() +// var leftField = Field2d() init { OdometryThread().start() // shrug + SmartDashboard.putNumber("Left Confidence", 0.0) + SmartDashboard.putNumber("Right Confidence", 0.0) + +// Shuffleboard.getTab("stuff").add("left field", {leftField}) +// Shuffleboard.getTab("stuff").add("right field", {rightField}) } val field: RectanglePoseArea = RectanglePoseArea(Translation2d(0.0, 0.0), Translation2d(16.54, 8.02)) @@ -271,27 +283,47 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem { } } - var left = VisionSubsystem.instance.updateLeft() - var right = VisionSubsystem.instance.updateRight() - if (left !== null) { - var dist = VisionSubsystem.instance.getLeftResult()?.bestTarget?.bestCameraToTarget?.translation?.getDistance(Translation3d()) - var confidence = 1 - ((dist!! - 1) / 6) // 1 - ((dist - min) / range) - if(field.isPoseWithinArea(left.estimatedPose.toPose2d())) { - addVisionMeasurement(left.estimatedPose.toPose2d(), left.timestampSeconds, VecBuilder.fill(confidence, confidence, 0.01)) // [x, y, theta] - } - } - if (right !== null) { - var dist = VisionSubsystem.instance.getRightResult()?.bestTarget?.bestCameraToTarget?.translation?.getDistance(Translation3d()) - var confidence = 1 - ((dist!! - 1) / 6) // 1 - ((dist - min) / range) - if(field.isPoseWithinArea(right.estimatedPose.toPose2d())) { - addVisionMeasurement(right.estimatedPose.toPose2d(), right.timestampSeconds, VecBuilder.fill(confidence, confidence, 0.01)) // [x, y, theta] - } - } +// if (VisionSubsystem.instance.getLeftResult().hasTargets()) { +// var left = VisionSubsystem.instance.updateLeft() +// var dist = VisionSubsystem.instance.getLeftResult().bestTarget.bestCameraToTarget.translation.getDistance(Translation3d()) +// +// var confidence = 1 - ((dist!! - 1) / 6) // 1 - ((dist - min) / range) +// leftConfidence.setNumber(confidence) +// if (left != null) { +// if(field.isPoseWithinArea(left.estimatedPose.toPose2d())) { +// // addVisionMeasurement(left.estimatedPose.toPose2d(), left.timestampSeconds, VecBuilder.fill(confidence, confidence, 0.01)) // [x, y, theta] +// addVisionMeasurement(left.estimatedPose.toPose2d(), left.timestampSeconds) +// leftField.robotPose = left.estimatedPose.toPose2d() +// println("left: " + left.estimatedPose.toPose2d()) +// +// +// } +// } +// } +// if (VisionSubsystem.instance.getRightResult().hasTargets()) { +// var right = VisionSubsystem.instance.updateRight() +// var dist = VisionSubsystem.instance.getRightResult()?.bestTarget?.bestCameraToTarget?.translation?.getDistance(Translation3d()) +// var confidence = 1 - ((dist!! - 1) / 6) // 1 - ((dist - min) / range) +// rightConfidence.setNumber(confidence) +// if (right != null) { +// if(field.isPoseWithinArea(right.estimatedPose.toPose2d())) { +// //addVisionMeasurement(right.estimatedPose.toPose2d(), right.timestampSeconds, VecBuilder.fill(confidence, confidence, 0.01)) // [x, y, theta] +// addVisionMeasurement(right.estimatedPose.toPose2d(), right.timestampSeconds) +// +// rightField.robotPose = right.estimatedPose.toPose2d() +// println("right: " + right.estimatedPose.toPose2d()) +// } +// } +// } } +// fun addVisionMeasurement(pose: Pose2d, timestamp: Double, stdDevs: Unit) { +// addVisionMeasurement(pose, timestamp) +// } + private fun startSimThread() { m_lastSimTime = Utils.getCurrentTimeSeconds() diff --git a/src/main/java/frc/robot/subsystems/vision/BetterVisionSubsystem.kt b/src/main/java/frc/robot/subsystems/vision/BetterVisionSubsystem.kt new file mode 100644 index 0000000..3090aa0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/BetterVisionSubsystem.kt @@ -0,0 +1,312 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ +package frc.robot.subsystems.vision + +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.* +import edu.wpi.first.math.numbers.N1 +import edu.wpi.first.math.numbers.N3 +import edu.wpi.first.units.Units +import edu.wpi.first.wpilibj.smartdashboard.Field2d +import edu.wpi.first.wpilibj2.command.SubsystemBase +import frc.robot.Robot +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.PhotonTrackedTarget +import java.util.* + +class BetterVisionSubsystem: SubsystemBase() { + private val leftCamera = PhotonCamera("Left Camera") + private val rightCamera = PhotonCamera("Right Camera") + + val kSingleTagStdDevs: Matrix = VecBuilder.fill(4.0, 4.0, 8.0) + val kMultiTagStdDevs: Matrix = VecBuilder.fill(0.5, 0.5, 1.0) + + private val botToLeftCamera = Transform3d( + Translation3d(-2.25, 11.75, 7.5), + Rotation3d(Units.Degree.of(10.0).`in`(Units.Radian), 0.0, 0.0), + ) + private val botToRightCamera = Transform3d( + Translation3d(2.25, -11.75, 7.5), + Rotation3d(Units.Degree.of(-10.0).`in`(Units.Radian), 0.0, 0.0), + ) + private val leftPhotonEstimator = + PhotonPoseEstimator(AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo), PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, botToLeftCamera) + private val rightPhotonEstimator = + PhotonPoseEstimator(AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo), PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, botToLeftCamera) + + /** + * Returns the latest standard deviations of the estimated pose from [ ][.getEstimatedGlobalPose], for use with [ ]. This should + * only be used when there are targets visible. + */ + var leftEstimationStdDevs: Matrix? = null + private set + /** + * Returns the latest standard deviations of the estimated pose from [ ][.getEstimatedGlobalPose], for use with [ ]. This should + * only be used when there are targets visible. + */ + var rightEstimationStdDevs: Matrix? = null + private set + + // Simulation + private var leftCameraSim: PhotonCameraSim? = null + private var rightCameraSim: PhotonCameraSim? = null + private var leftVisionSim: VisionSystemSim? = null + private var rightVisionSim: VisionSystemSim? = null + + init { + leftPhotonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY) + rightPhotonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY) + + // ----- Simulation + if (Robot.sim()) { + // Create the vision system simulation which handles cameras and targets on the field. + leftVisionSim = VisionSystemSim("main") + rightVisionSim = VisionSystemSim("main") + // Add all the AprilTags inside the tag layout as visible targets to this simulated field. + leftVisionSim!!.addAprilTags(AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo)) + rightVisionSim!!.addAprilTags(AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo)) + // Create simulated camera properties. These can be set to mimic your actual camera. + val cameraProp = SimCameraProperties() + cameraProp.setCalibration(960, 720, Rotation2d.fromDegrees(90.0)) + cameraProp.setCalibError(0.35, 0.10) + cameraProp.fps = 15.0 + cameraProp.avgLatencyMs = 50.0 + cameraProp.latencyStdDevMs = 15.0 + // Create a PhotonCameraSim which will update the linked PhotonCamera's values with visible + // targets. + leftCameraSim = PhotonCameraSim(leftCamera, cameraProp) + rightCameraSim = PhotonCameraSim(rightCamera, cameraProp) + // Add the simulated camera to view the targets on this simulated field. + leftVisionSim!!.addCamera(leftCameraSim, botToLeftCamera) + rightVisionSim!!.addCamera(rightCameraSim, botToRightCamera) + + leftCameraSim!!.enableDrawWireframe(true) + rightCameraSim!!.enableDrawWireframe(true) + } + } + + val leftEstimatedGlobalPose: Optional + /** + * 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 + * [getEstimationStdDevs] + * + * @return An [EstimatedRobotPose] with an estimated pose, estimate timestamp, and targets + * used for estimation. + */ + get() { + var visionEst = Optional.empty() + for (change in leftCamera.getAllUnreadResults()) { + visionEst = leftPhotonEstimator.update(change) + updateLeftEstimationStdDevs(visionEst, change.getTargets()) + + if (Robot.sim()) { + visionEst.ifPresentOrElse( + { est: EstimatedRobotPose -> + leftSimDebugField!! + .getObject("VisionEstimation").pose = est.estimatedPose.toPose2d() + }, + { + leftSimDebugField!!.getObject("VisionEstimation").setPoses() + }) + } + } + return visionEst + } + val rightEstimatedGlobalPose: Optional + /** + * 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 + * [getEstimationStdDevs] + * + * @return An [EstimatedRobotPose] with an estimated pose, estimate timestamp, and targets + * used for estimation. + */ + get() { + var visionEst = Optional.empty() + for (change in rightCamera.getAllUnreadResults()) { + visionEst = rightPhotonEstimator.update(change) + updateLeftEstimationStdDevs(visionEst, change.getTargets()) + + if (Robot.sim()) { + visionEst.ifPresentOrElse( + { est: EstimatedRobotPose -> + rightSimDebugField!! + .getObject("VisionEstimation").pose = est.estimatedPose.toPose2d() + }, + { + rightSimDebugField!!.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 fun updateLeftEstimationStdDevs( + estimatedPose: Optional, targets: List, + ) { + if (estimatedPose.isEmpty) { + // No pose input. Default to single-tag std devs + leftEstimationStdDevs = kSingleTagStdDevs + } else { + // Pose present. Start running Heuristic + var estStdDevs = kSingleTagStdDevs + var numTags = 0 + var avgDist = 0.0 + + // Precalculation - see how many tags we found, and calculate an average-distance metric + for (tgt in targets) { + val tagPose = leftPhotonEstimator.fieldTags.getTagPose(tgt.fiducialId) + if (tagPose.isEmpty) continue + numTags++ + avgDist += + tagPose + .get() + .toPose2d() + .translation + .getDistance(estimatedPose.get().estimatedPose.toPose2d().translation) + } + + if (numTags == 0) { + // No tags visible. Default to single-tag std devs + leftEstimationStdDevs = kSingleTagStdDevs + } else { + // One or more tags visible, run the full heuristic. + avgDist /= numTags.toDouble() + // Decrease std devs if multiple targets are visible + if (numTags > 1) estStdDevs = kMultiTagStdDevs + // Increase std devs based on (average) distance + estStdDevs = if (numTags == 1 && avgDist > 4) VecBuilder.fill( + Double.MAX_VALUE, + Double.MAX_VALUE, + Double.MAX_VALUE + ) + else estStdDevs.times(1 + (avgDist * avgDist / 30)) + leftEstimationStdDevs = estStdDevs + } + } + } + + /** + * 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 fun updateRightEstimationStdDevs( + estimatedPose: Optional, targets: List, + ) { + if (estimatedPose.isEmpty) { + // No pose input. Default to single-tag std devs + rightEstimationStdDevs = kSingleTagStdDevs + } else { + // Pose present. Start running Heuristic + var estStdDevs = kSingleTagStdDevs + var numTags = 0 + var avgDist = 0.0 + + // Precalculation - see how many tags we found, and calculate an average-distance metric + for (tgt in targets) { + val tagPose = rightPhotonEstimator.fieldTags.getTagPose(tgt.fiducialId) + if (tagPose.isEmpty) continue + numTags++ + avgDist += + tagPose + .get() + .toPose2d() + .translation + .getDistance(estimatedPose.get().estimatedPose.toPose2d().translation) + } + + if (numTags == 0) { + // No tags visible. Default to single-tag std devs + rightEstimationStdDevs = kSingleTagStdDevs + } else { + // One or more tags visible, run the full heuristic. + avgDist /= numTags.toDouble() + // Decrease std devs if multiple targets are visible + if (numTags > 1) estStdDevs = kMultiTagStdDevs + // Increase std devs based on (average) distance + estStdDevs = if (numTags == 1 && avgDist > 4) VecBuilder.fill( + Double.MAX_VALUE, + Double.MAX_VALUE, + Double.MAX_VALUE + ) + else estStdDevs.times(1 + (avgDist * avgDist / 30)) + rightEstimationStdDevs = estStdDevs + } + } + } + + // ----- Simulation + fun simulationPeriodic(robotSimPose: Pose2d?) { + leftVisionSim!!.update(robotSimPose) + rightVisionSim!!.update(robotSimPose) + } + + /** Reset pose history of the robot in the vision system simulation. */ + fun resetSimPose(pose: Pose2d?) { + if (Robot.sim()) leftVisionSim!!.resetRobotPose(pose) + } + + val leftSimDebugField: Field2d? + /** A Field2d for visualizing our robot and objects on the field. */ + get() { + if (!Robot.sim()) return null + return leftVisionSim!!.debugField + } + + val rightSimDebugField: Field2d? + /** A Field2d for visualizing our robot and objects on the field. */ + get() { + if (!Robot.sim()) return null + return rightVisionSim!!.debugField + } + + + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.kt b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.kt deleted file mode 100755 index 4782dbf..0000000 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.kt +++ /dev/null @@ -1,144 +0,0 @@ -package frc.robot.subsystems.vision - -import edu.wpi.first.apriltag.AprilTagFieldLayout -import edu.wpi.first.apriltag.AprilTagFields -import edu.wpi.first.math.geometry.* -import edu.wpi.first.units.Units -import edu.wpi.first.wpilibj.DriverStation.Alliance -import edu.wpi.first.wpilibj.RobotBase -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard -import edu.wpi.first.wpilibj2.command.SubsystemBase -import frc.robot.util.RectanglePoseArea -import org.photonvision.EstimatedRobotPose -import org.photonvision.PhotonCamera -import org.photonvision.PhotonPoseEstimator -import org.photonvision.targeting.PhotonPipelineResult -import org.photonvision.targeting.PhotonTrackedTarget -import java.util.* -import kotlin.jvm.optionals.getOrNull -import kotlin.math.abs - -object VisionSubsystem : SubsystemBase() { - var alliance: Alliance? = null - private val leftCam = PhotonCamera("Left Camera") - private val rightCam = PhotonCamera("Right Camera") - private val noteCam = PhotonCamera("ShitCam") - private val botToLeftCamera = Transform3d( - Translation3d(-2.25, 11.75, 7.5), - Rotation3d(Units.Degree.of(10.0).`in`(Units.Radian), 0.0, 0.0), - ) // TODO get this value - private val botToRightCamera = Transform3d( - Translation3d(2.25, -11.75, 7.5), - Rotation3d(Units.Degree.of(-10.0).`in`(Units.Radian), 0.0, 0.0), - ) // TODO get this value - private var enable = false - private var trust = false - private var fieldError = 0 - private var distanceError = 0 - private var botpose: Pose2d? = null - private var aprilTagFieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2024Crescendo) - private var rightPhotonPoseEstimator = PhotonPoseEstimator(aprilTagFieldLayout, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, rightCam, botToRightCamera) - - private var leftPhotonPoseEstimator = PhotonPoseEstimator(aprilTagFieldLayout, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, leftCam, botToLeftCamera) - private val isSim = RobotBase.isSimulation() - - /** Creates a new Limelight. */ - init { - rightPhotonPoseEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY) - leftPhotonPoseEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY) - - SmartDashboard.putNumber("Field Error", fieldError.toDouble()) - SmartDashboard.putNumber("Limelight Error", distanceError.toDouble()) - } - - /** - * Calculates standard deviations for the right camera based on the number of targets detected. Heuristic. - */ - private fun updateLeftEstimationStdDevs( - visionEst: Optional, - targets: List, - ) { - } - - // singleton class - val instance: VisionSubsystem = VisionSubsystem - - fun useLimelight(enable: Boolean) { - this.enable = enable - } - - fun trustLL(trust: Boolean) { - this.trust = trust - } - - override fun periodic() { - } - - fun getNoteCamYaw(): Double { - try { - if (!isSim) { - return if (noteCam.getLatestResult().getBestTarget() != null) { - noteCam.getLatestResult() - .getBestTarget().yaw - } else { - 0.0 - } - } else { - return 0.0 - } - } finally { - } - } - - fun getNoteCamPitch(): Double { - try { - if (!isSim) { - return if (noteCam.getLatestResult().getBestTarget() != null) { - noteCam.getLatestResult() - .getBestTarget().pitch - } else { - 0.0 - } - } else { - return 0.0 - } - } finally { - } - } - - fun isTarget(): Boolean { - try { - return noteCam.latestResult.hasTargets() - } finally { - false - } - } - - fun isCloseEnoughToDone(): Boolean { - try { - return abs(noteCam.latestResult.bestTarget.yaw) < 5 && abs(noteCam.latestResult.bestTarget.pitch) < 5 - } finally { - false - } - } - - private val field = RectanglePoseArea(Translation2d(0.0, 0.0), Translation2d(16.54, 8.02)) - - @JvmStatic - fun updateLeft(): EstimatedRobotPose? { - return VisionSubsystem.instance.leftPhotonPoseEstimator.update().getOrNull() - } - - @JvmStatic - fun updateRight(): EstimatedRobotPose? { - return VisionSubsystem.instance.rightPhotonPoseEstimator.update().getOrNull() - } - - fun getLeftResult(): PhotonPipelineResult? { - return leftCam.latestResult - } - - fun getRightResult(): PhotonPipelineResult? { - return rightCam.latestResult - } -} diff --git a/vendordeps/PathplannerLib-beta.json b/vendordeps/PathplannerLib-beta.json index 8a53e68..4083333 100644 --- a/vendordeps/PathplannerLib-beta.json +++ b/vendordeps/PathplannerLib-beta.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib-beta.json", "name": "PathplannerLib", - "version": "2025.0.0-beta-3.1", + "version": "2025.0.0-beta-4", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.0.0-beta-3.1" + "version": "2025.0.0-beta-4" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.0.0-beta-3.1", + "version": "2025.0.0-beta-4", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/photonlib-json-1.0.json b/vendordeps/photonlib-json-1.0.json index 089d4ac..f25d32b 100644 --- a/vendordeps/photonlib-json-1.0.json +++ b/vendordeps/photonlib-json-1.0.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2024.3.1", + "version": "dev-v2025.0.0-alpha-0-24-gbdb2949b", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2025", "mavenUrls": [ @@ -9,12 +9,52 @@ "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": [], + "jniDependencies": [ + { + "groupId": "edu.wpi.first.wpilibc", + "artifactId": "wpilibc-cpp", + "version": "2025.1.1-beta-1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "dev-v2025.0.0-alpha-0-24-gbdb2949b", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-jni", + "version": "dev-v2025.0.0-alpha-0-24-gbdb2949b", + "skipInvalidPlatforms": true, + "isJar": true, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], "cppDependencies": [ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2024.3.1", + "version": "dev-v2025.0.0-alpha-0-24-gbdb2949b", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -29,7 +69,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2024.3.1", + "version": "dev-v2025.0.0-alpha-0-24-gbdb2949b", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -46,12 +86,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2024.3.1" + "version": "dev-v2025.0.0-alpha-0-24-gbdb2949b" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2024.3.1" + "version": "dev-v2025.0.0-alpha-0-24-gbdb2949b" } ] -} +} \ No newline at end of file